diff --git a/gym_xarm/tasks/base.py b/gym_xarm/tasks/base.py index e20f0c1..44550da 100644 --- 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 index c56b5ff..25be2ae 100644 --- 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)