def apply_action(self, action):
if not isinstance(action, np.ndarray):
action = np.array(action).flatten()
if action.size != 2:
raise ValueError('Action should be [d_x, d_y].')
action = np.concatenate([action, np.array([0.])])
pos, quat, rot_mat, euler = self.robot.arm.get_ee_pose()
pos += action * self._ee_pos_scale
rot_vec = np.array([0, 0, 1]) * self.gripper_ori
rot_quat = rotvec2quat(rot_vec)
ee_ori = quat_multiply(self.ref_ee_ori, rot_quat)
jnt_pos = self.robot.arm.compute_ik(pos, ori=ee_ori)
for step in range(self._action_repeat):
self.robot.arm.set_jpos(jnt_pos)
self.robot.pb_client.stepSimulation()
def apply_action(self, action):
if not isinstance(action, np.ndarray):
action = np.array(action).flatten()
if action.size != 5:
raise ValueError('Action should be [d_x, d_y, d_z, '
'd_angle, open/close gripper].')
pos, quat, rot_mat, euler = self.robot.arm.get_ee_pose()
pos += action[:3] * self._ee_pos_scale
self.gripper_ori += action[3] * self._ee_ori_scale
self.gripper_ori = ang_in_mpi_ppi(self.gripper_ori)
rot_vec = np.array([0, 0, 1]) * self.gripper_ori
rot_quat = rotvec2quat(rot_vec)
ee_ori = quat_multiply(self.ref_ee_ori, rot_quat)
jnt_pos = self.robot.arm.compute_ik(pos, ori=ee_ori)
gripper_ang = self._scale_gripper_angle(action[4])
for step in range(self._action_repeat):
self.robot.arm.set_jpos(jnt_pos)
self.robot.arm.eetool.set_pos(gripper_ang)
self.robot.pb_client.stepSimulation()
