Merge pull request #4 from huggingface/user/aliberts/2024_04_22_fix_e…

…ef_translation Fix for eef rotation instead of translation
huggingface · Apr 22, 2024 · 27e65c9 · 27e65c9
2 parents 6a88f7d + 9960150
commit 27e65c9
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 20 deletions.
diff --git a/gym_xarm/tasks/base.py b/gym_xarm/tasks/base.py
@@ -306,8 +306,8 @@ def _apply_action(self, action):
         )
 
     def _set_gripper(self, gripper_pos, gripper_rotation):
-        self._utils.set_mocap_pos(self.model, self.data, "robot0:mocap", gripper_pos)
-        self._utils.set_mocap_quat(self.model, self.data, "robot0:mocap", gripper_rotation)
+        self._utils.set_mocap_pos(self.model, self.data, "robot0:mocap2", gripper_pos)
+        self._utils.set_mocap_quat(self.model, self.data, "robot0:mocap2", gripper_rotation)
         self._utils.set_joint_qpos(self.model, self.data, "right_outer_knuckle_joint", 0)
         self.data.qpos[10] = 0.0
         self.data.qpos[12] = 0.0

diff --git a/gym_xarm/tasks/mocap.py b/gym_xarm/tasks/mocap.py
@@ -19,25 +19,8 @@ def apply_action(model, model_names, data, action):
 def reset(model, data):
     if model.nmocap > 0 and model.eq_data is not None:
         for i in range(model.eq_data.shape[0]):
-            # if sim.model.eq_type[i] == mujoco_py.const.EQ_WELD:
             if model.eq_type[i] == mujoco.mjtEq.mjEQ_WELD:
-                # model.eq_data[i, :] = np.array([0., 0., 0., 1., 0., 0., 0.])
-                model.eq_data[i, :] = np.array(
-                    [
-                        0.0,
-                        0.0,
-                        0.0,
-                        0.0,
-                        0.0,
-                        0.0,
-                        1.0,
-                        0.0,
-                        0.0,
-                        0.0,
-                        0.0,
-                    ]
-                )
-    # sim.forward()
+                model.eq_data[i, :8] = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0])
     mujoco.mj_forward(model, data)