Unstable Postion Control

[ztr583]

System:

• OS version: [e.g. Ubuntu 16.04]
• Python version (if applicable): [e.g. Python 3.6]
• SAPIEN version (pip freeze | grep sapien):
• Environment: [Desktop / Server with xvfb / Docker with xvfb]

Describe the bug
I use the compute_passive_force to compensate the gravity. However, at some poses, the compensation doesn't work at all.

To Reproduce

qpos_final = np.array([-1.5,3.6,-9.0735190e-02,-1.4535658e+00,4.5006253e-02,-9.7704554e-01,0])
self.robot.set_qpos(qpos_final)
for iteration in range(100):
      current_qpos = self.robot.get_qpos()
      error = qpos_final - current_qpos
      qf = self.robot.compute_passive_force(gravity=True, coriolis_and_centrifugal=True, external=False)
      qpos_prev = current_qpos
      self.robot.set_qf(qf)
      self.scene.step()
      print(error)

Expected behavior
error should be quite close to 0.

Screenshots
[ 1.5717626e-02 -4.0000510e-01 4.8072156e-02 3.9999151e-01
2.9237978e-02 1.2618303e-04 -2.1611266e-02]
[ 6.5611601e-03 -3.3859110e-01 -1.0541581e-02 8.0195165e-01
3.0018456e-02 -6.6697598e-05 -2.1611614e-02]
[ 0.0642339 -0.28389168 0.07027106 1.1989497 0.03019235 0.00569385
-0.02161189]
[ 0.11840463 -0.20899272 -0.00787933 1.2637141 0.0303156 0.00574058
-0.02161172]
[ 0.45436633 -0.434896 0.90632606 1.2606282 0.2650725 0.1318996
-0.02161168]
[ 0.23079848 -0.36291957 0.41938102 1.2781768 0.26507077 0.13193989
-0.0216118 ]
[-0.0491662 -0.28087568 -0.21084815 1.2955148 0.26499566 0.1319328
-0.02161207]
[ 0.02522922 -0.41915536 0.42234164 1.2859576 0.29337725 0.13181877
-0.02161241]
[-0.12638128 -0.31081128 -0.01928277 1.3101594 0.29370868 0.13184726
-0.02161222]
[-0.12177956 -0.27417612 -0.10974083 1.3131788 0.29239386 0.13179934
-0.02161233]
[ 0.0792166 -0.47034168 0.92001987 1.2529626 0.6342988 0.24240327
-0.02161237]
[-0.20187783 -0.3997593 0.41706908 1.2842815 0.6345366 0.24240726
-0.02161239]
[-0.538795 -0.317806 -0.2697631 1.3513267 0.63051397 0.24243301
-0.02161255]
[-0.48532462 -0.285563 -0.25090557 1.3790693 0.6238842 0.24243367
-0.02161304]
[-0.42955995 -0.49895 0.5637382 1.3678916 0.9190787 0.37033516
-0.02161317]
[-0.6082611 -0.44917202 0.24701884 1.3702464 0.91983795 0.37033343
-0.02161318]
[-0.9062896 -0.38236547 -0.32403335 1.4443424 0.914556 0.3702755
-0.02161223]
[-0.7844453 -0.37645268 -0.12359608 1.5379603 0.90779305 0.370292
-0.0216124 ]
[-0.7343435 -0.35540366 -0.06224252 1.657558 0.9031383 0.37028593
-0.02161267]
[-0.7178154 -0.33306146 -0.03211952 1.788943 0.90029675 0.37028307
-0.02161252]

Additional context
Add any other context about the problem here.