def _set_action(self, action):
assert action.shape == (4,)
action = action.copy() # ensure that we don't change the action outside of this scope
pos_ctrl, gripper_ctrl = action[:3], action[3]
pos_ctrl *= 0.05 # limit maximum change in position
rot_ctrl = [0., 1., 1., 0.] # fixed rotation of the end effector, expressed as a quaternion {Vertical}
#rot_ctrl = [1., 0., 1., 0.] # fixed rotation of the end effector, expressed as a quaternion {Horizontal}
gripper_ctrl = np.array([gripper_ctrl, gripper_ctrl])
assert gripper_ctrl.shape == (2,)
# Apply action to simulation
# Determine the closest cloth node to the gripper
closest, dist_closest = self.find_closest_indice(self.grip_pos)
# Only allow gripping if in proximity
if dist_closest<=0.001:
utils.grasp(self.sim, gripper_ctrl, closest)
if self.block_gripper:
gripper_ctrl = np.zeros_like(gripper_ctrl)
action = np.concatenate([pos_ctrl, rot_ctrl, gripper_ctrl])
utils.ctrl_set_action(self.sim, action)
utils.mocap_set_action(self.sim, action)
def _reset_sim(self):
self.sim.set_state(self.initial_state)
self.sim.forward()
# Randomize start position of object.
if self.has_object:
object_xpos = self.initial_gripper_xpos[:2]
while np.linalg.norm(object_xpos - self.initial_gripper_xpos[:2]) < 0.1:
object_xpos = self.initial_gripper_xpos[:2] + self.np_random.uniform(-self.obj_range, self.obj_range, size=2)
object_qpos = self.sim.data.get_joint_qpos('object0:joint')
assert object_qpos.shape == (7,)
object_qpos[:2] = object_xpos
self.sim.data.set_joint_qpos('object0:joint', object_qpos)
if self.has_cloth:
if self.behavior=="diagonally":
joint_vertice = 'CB'+str(self.cloth_length-1)+'_'+str(self.cloth_length-1)
elif self.behavior=="sideways":
joint_vertice = 'CB0'+'_'+str(self.cloth_length-1)
new_position = self.sim.data.get_body_xpos(joint_vertice)
# Make the joint to be the first point
randomness = self.np_random.uniform(-self.randomize_cloth, self.randomize_cloth, size=2)
new_position[0] = new_position[0] + randomness[0]
new_position[1] = new_position[1] + randomness[1]
new_position = np.append(new_position, [1, 0, 0, 0])
gripper_ctrl = np.array([0.0, 0.0])
utils.grasp(self.sim, gripper_ctrl, 'CB0_0')
self.sim.data.set_joint_qpos('cloth', new_position)
# if self.visual_randomize:
# for name in self.sim.model.geom_names:
# self.modder.whiten_materials()
# self.modder.set_checker(name, (255, 0, 0), (0, 0, 0))
# self.modder.rand_all(name)
# self.modder.set_checker('skin', (255, 0, 0), (0, 0, 0))
# self.modder.rand_all('skin')
self.sim.forward()
return True
