fix bug with ee pose controller for panda

mani_skill2/agents/controllers/pd_ee_pose.py

@@ -90,14 +90,14 @@ def reset(self):
     def compute_ik(self, target_pose: Pose, action: Array, max_iterations=100):
         # Assume the target pose is defined in the base frame
         if physx.is_gpu_enabled():
-            jacobian = self.pk_chain.jacobian(self.articulation.get_qpos())
+            jacobian = self.pk_chain.jacobian(self.qpos)
             # NOTE (stao): a bit of a hacky way to check if we want to do IK on position or pose here
             if action.shape[1] == 3:
                 jacobian = jacobian[:, 0:3]
 
             # NOTE (stao): this method of IK is from https://mathweb.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf by Samuel R. Buss
             delta_joint_pos = torch.linalg.pinv(jacobian) @ action.unsqueeze(-1)
-            return self.articulation.get_qpos() + delta_joint_pos.squeeze(-1)
+            return self.qpos + delta_joint_pos.squeeze(-1)
 
         else:
             result, success, error = self.pmodel.compute_inverse_kinematics(

manualtest/visual_all_envs_cpu.py

@@ -7,16 +7,16 @@
 
 if __name__ == "__main__":
     # , "StackCube-v0", "LiftCube-v0"
-    num_envs = 1
+    num_envs = 2
     for env_id in ["PushObject-v0"]:  # , "StackCube-v0", "LiftCube-v0"]:
         env = gym.make(
             env_id,
             num_envs=num_envs,
             enable_shadow=True,
-            robot_uid="fetch",
+            robot_uid="panda",
             reward_mode="normalized_dense",
             render_mode="rgb_array",
-            control_mode="pd_joint_delta_pos",
+            control_mode="pd_ee_delta_pos",
             sim_freq=500,
             control_freq=100,
         )

setup.py

@@ -34,6 +34,7 @@
         "opencv-python",
         "imageio",
         "imageio[ffmpeg]",
+        "pytorch_kinematics",
     ],
     # Glob patterns do not automatically match dotfiles
     package_data={