Unstable Postion Control

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.

haosulab / SAPIEN

Unstable Postion Control #163