From 38e8958ebda4bd518ba371f4f05c1befb1552033 Mon Sep 17 00:00:00 2001 From: Hongyi Zhou Date: Tue, 11 Oct 2022 18:16:25 +0200 Subject: [PATCH] box pushing working with mujoco binding --- fancy_gym/envs/mujoco/box_pushing/box_pushing_env.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/fancy_gym/envs/mujoco/box_pushing/box_pushing_env.py b/fancy_gym/envs/mujoco/box_pushing/box_pushing_env.py index 73a2412..2ca6b01 100644 --- a/fancy_gym/envs/mujoco/box_pushing/box_pushing_env.py +++ b/fancy_gym/envs/mujoco/box_pushing/box_pushing_env.py @@ -79,8 +79,7 @@ class BoxPushingEnv(MujocoEnv, utils.EzPickle): # rest box to initial position self.set_state(self.init_qpos_box_pushing, self.init_qvel_box_pushing) box_init_pos = np.array([0.4, 0.3, -0.01, 0.0, 0.0, 0.0, 1.0]) - self.data.body("box_0").xpos = box_init_pos[:3] - self.data.body("box_0").xquat = box_init_pos[3:] + self.data.joint("box_joint").qpos = box_init_pos # set target position box_target_pos = self.sample_context() @@ -90,7 +89,7 @@ class BoxPushingEnv(MujocoEnv, utils.EzPickle): self.model.body_quat[3] = box_target_pos[-4:] # set the robot to the right configuration (rod tip in the box) - desired_tcp_pos = box_target_pos[:3] + np.array([0.0, 0.0, 0.15]) + desired_tcp_pos = box_init_pos[:3] + np.array([0.0, 0.0, 0.15]) desired_tcp_quat = np.array([0, 1, 0, 0]) desired_joint_pos = self.calculateOfflineIK(desired_tcp_pos, desired_tcp_quat) self.data.qpos[:7] = desired_joint_pos