def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
# gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip')
# gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('jaco_joint_finger_1')
gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) #+ self.sim.data.get_site_xpos('jaco_joint_finger_1')
gripper_rotation = np.array([1., 0., 1., 0.])
# self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
# self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
# print(self.sim.data.get_body_xpos('jaco_link_finger_1'))
# print(self.sim.data.get_joint_qpos('jaco_joint_finger_1'))
# print(self.sim.data.get_joint_qvel('jaco_joint_finger_1'))
# self.sim.data.set_mocap_pos('jaco_link_finger_1', gripper_target)
# self.sim.data.set_mocap_quat('jaco_link_finger_1', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
# self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy()
# if self.has_object:
# self.height_offset = self.sim.data.get_site_xpos('object0')[2]
self.initial_gripper_xpos = self.sim.data.get_site_xpos('jaco_joint_finger_1').copy()
if self.has_object:
self.height_offset = self.sim.data.get_site_xpos('target')[2]
print('DONE Setting UP\n')
def _env_setup(self, initial_qpos):
# Randomize the starting position
shoulder_pan_val = -0.1 + (random.random()*0.2)
initial_qpos['robot0:shoulder_pan_joint'] = shoulder_pan_val
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position
gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip')
gripper_rotation = np.array([1.,0.,1.,0.])
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()