def take_observation(self): """ Take observation from the environment and return it. TODO: define return type """ # Take an observation # done = False obs_message = self._observation_msg if obs_message is None: # print("last_observations is empty") return None # Collect the end effector points and velocities in # cartesian coordinates for the process_observationsstate. # Collect the present joint angles and velocities from ROS for the state. last_observations = self.process_observations(obs_message, self.environment) # # # Get Jacobians from present joint angles and KDL trees # # # The Jacobians consist of a 6x6 matrix getting its from from # # # (# joint angles) x (len[x, y, z] + len[roll, pitch, yaw]) ee_link_jacobians = self.get_jacobians(last_observations) if self.environment['link_names'][-1] is None: print("End link is empty!!") return None else: # print(self.environment['link_names'][-1]) trans, rot = forward_kinematics( self.scara_chain, self.environment['link_names'], last_observations[:self.scara_chain.getNrOfJoints()], base_link=self.environment['link_names'][0], end_link=self.environment['link_names'][-1]) # # rotation_matrix = np.eye(4) rotation_matrix[:3, :3] = rot rotation_matrix[:3, 3] = trans # angle, dir, _ = rotation_from_matrix(rotation_matrix) # # # current_quaternion = np.array([angle]+dir.tolist())# # # I need this calculations for the new reward function, need to send them back to the run mara or calculate them here # current_quaternion = quaternion_from_matrix(rotation_matrix) # tgt_quartenion = quaternion_from_matrix(self.target_orientation) current_quaternion = quat.from_rotation_matrix(rotation_matrix) # print("current_quaternion: ", current_quaternion) tgt_quartenion = quat.from_rotation_matrix(self.target_orientation) # A = np.vstack([current_quaternion, np.ones(len(current_quaternion))]).T #quat_error = np.linalg.lstsq(A, tgt_quartenion)[0] # this is wrong!!!! Substraction should be replaced by: quat_error = current_quaternion * tgt_quartenion.conjugate() # quat_error = current_quaternion - tgt_quartenion quat_error = current_quaternion * tgt_quartenion.conjugate() # convert quat to np arrays quat_error = quat.as_float_array(quat_error) # RK: revisit this later, we only take one angle difference here! angle_diff = 2 * np.arccos(np.clip(quat_error[..., 0], -1., 1.)) # print("quat error: ", quat_error) # print("quat_error[..., 0]: ", quat_error[..., 0]) # print("quat_error: ",quat_error) # print("angle_diff: ", angle_diff) # print("self.realgoal: ", self.realgoal) # print("curr quat: ", current_quaternion) current_ee_tgt = np.ndarray.flatten( get_ee_points(self.environment['end_effector_points'], trans, rot).T) ee_points = current_ee_tgt - self.realgoal #self.environment['ee_points_tgt'] ee_points_jac_trans, _ = self.get_ee_points_jacobians( ee_link_jacobians, self.environment['end_effector_points'], rot) ee_velocities = self.get_ee_points_velocities( ee_link_jacobians, self.environment['end_effector_points'], rot, last_observations) # Concatenate the information that defines the robot state # vector, typically denoted asrobot_id 'x'. state = np.r_[np.reshape(last_observations, -1), np.reshape(ee_points, -1), np.reshape(angle_diff, -1), np.reshape(ee_velocities, -1), ] # print("quat_error: ", quat_error) # print("ee_points:", ee_points) # print("angle_diff: ", angle_diff) return np.r_[np.reshape(last_observations, -1), np.reshape(ee_points, -1), np.reshape(angle_diff, -1), np.reshape(ee_velocities, -1), ]
def step(self, action): state = [] reward = [] done = False data_arm_state = None while data_arm_state is None: try: data_arm_state = rospy.wait_for_message('/ariac/arm/state', JointTrajectoryControllerState, timeout=5) except: pass data_arm_state2 = JointTrajectoryControllerState() data_arm_state2.joint_names = data_arm_state.joint_names[1:] data_arm_state2.desired.positions = data_arm_state.desired.positions[1:] data_arm_state2.desired.velocities = data_arm_state.desired.velocities[1:] data_arm_state2.desired.accelerations = data_arm_state.desired.accelerations[1:] data_arm_state2.actual.positions = data_arm_state.actual.positions[1:] data_arm_state2.actual.velocities = data_arm_state.actual.velocities[1:] data_arm_state2.actual.accelerations = data_arm_state.actual.accelerations[1:] data_arm_state2.error.positions = data_arm_state.error.positions[1:] data_arm_state2.error.velocities = data_arm_state.error.velocities[1:] data_arm_state2.error.accelerations = data_arm_state.error.accelerations[1:] # Collect the end effector points and velocities in # cartesian coordinates for the process_observationsstate. # Collect the present joint angles and velocities from ROS for the state. last_observations = self.process_observations(data_arm_state2, self.environment) # # # Get Jacobians from present joint angles and KDL trees # # # The Jacobians consist of a 6x6 matrix getting its from from # # # (# joint angles) x (len[x, y, z] + len[roll, pitch, yaw]) ee_link_jacobians = self.get_jacobians(last_observations) if self.environment['link_names'][-1] is None: print("End link is empty!!") return None else: # print(self.environment['link_names'][-1]) trans, rot = forward_kinematics(self.scara_chain, self.environment['link_names'], last_observations[:self.scara_chain.getNrOfJoints()], base_link=self.environment['link_names'][0], end_link=self.environment['link_names'][-1]) # # rotation_matrix = np.eye(4) rotation_matrix[:3, :3] = rot rotation_matrix[:3, 3] = trans # angle, dir, _ = rotation_from_matrix(rotation_matrix) # # # current_quaternion = np.array([angle]+dir.tolist())# # I need this calculations for the new reward function, need to send them back to the run scara or calculate them here current_quaternion = quaternion_from_matrix(rotation_matrix) current_ee_tgt = np.ndarray.flatten(get_ee_points(self.environment['end_effector_points'], trans, rot).T) ee_points = current_ee_tgt - self.realgoal#self.environment['ee_points_tgt'] ee_points_jac_trans, _ = self.get_ee_points_jacobians(ee_link_jacobians, self.environment['end_effector_points'], rot) ee_velocities = self.get_ee_points_velocities(ee_link_jacobians, self.environment['end_effector_points'], rot, last_observations) # Concatenate the information that defines the robot state # vector, typically denoted asrobot_id 'x'. state = np.r_[np.reshape(last_observations, -1), np.reshape(ee_points, -1), np.reshape(ee_velocities, -1),] self._pub.publish(self.get_trajectory_message([0.018231524968342513, 3.2196073949389703 + np.random.rand(1)[0] - .5, -0.6542426695478509 + np.random.rand(1)[0] - .5, 1.7401498071871018 + np.random.rand(1)[0] - .5, 3.7354939354077596 + np.random.rand(1)[0] - .5, -1.510707301072792 + np.random.rand(1)[0] - .5, 0.00014565660628829136 + np.random.rand(1)[0] - .5])) data_vacuum_state = None while data_vacuum_state is None: try: data_vacuum_state = rospy.wait_for_message('/ariac/gripper/state', VacuumGripperState, timeout=5) except: pass data_camera = None while data_camera is None: try: data_camera = rospy.wait_for_message('/ariac/logical_camera_1', LogicalCameraImage, timeout=5) except: pass try: self.vacuum_gripper_control(enable=bool(state[0])) except (rospy.ServiceException) as e: print ("/gazebo/reset_simulation service call failed") state = [data_arm_state, data_camera, data_vacuum_state] return state, reward, done, {}
def take_observation(self): """ Take observation from the environment and return it. TODO: define return type """ # Take an observation # done = False obs_message = self._observation_msg if obs_message is None: # print("last_observations is empty") return None # Collect the end effector points and velocities in # cartesian coordinates for the process_observationsstate. # Collect the present joint angles and velocities from ROS for the state. last_observations = self.process_observations(obs_message, self.environment) # # # Get Jacobians from present joint angles and KDL trees # # # The Jacobians consist of a 6x6 matrix getting its from from # # # (# joint angles) x (len[x, y, z] + len[roll, pitch, yaw]) ee_link_jacobians = self.get_jacobians(last_observations) if self.environment['link_names'][-1] is None: print("End link is empty!!") return None else: # print(self.environment['link_names'][-1]) trans, rot = forward_kinematics( self.scara_chain, self.environment['link_names'], last_observations[:self.scara_chain.getNrOfJoints()], base_link=self.environment['link_names'][0], end_link=self.environment['link_names'][-1]) # # rotation_matrix = np.eye(4) rotation_matrix[:3, :3] = rot rotation_matrix[:3, 3] = trans # angle, dir, _ = rotation_from_matrix(rotation_matrix) # # # current_quaternion = np.array([angle]+dir.tolist())# # I need this calculations for the new reward function, need to send them back to the run scara or calculate them here current_quaternion = quaternion_from_matrix(rotation_matrix) current_ee_tgt = np.ndarray.flatten( get_ee_points(self.environment['end_effector_points'], trans, rot).T) ee_points = current_ee_tgt - self.environment['ee_points_tgt'] ee_points_jac_trans, _ = self.get_ee_points_jacobians( ee_link_jacobians, self.environment['end_effector_points'], rot) ee_velocities = self.get_ee_points_velocities( ee_link_jacobians, self.environment['end_effector_points'], rot, last_observations) # Concatenate the information that defines the robot state # vector, typically denoted asrobot_id 'x'. state = np.r_[np.reshape(last_observations, -1), np.reshape(ee_points, -1), np.reshape(ee_velocities, -1), ] return np.r_[np.reshape(last_observations, -1), np.reshape(ee_points, -1), np.reshape(ee_velocities, -1), ]
def take_observation(self): """ Take observation from the environment and return it. TODO: define return type """ obs_message = self._observation_msg if obs_message is None: return None # Collect the end effector points and velocities in # cartesian coordinates for the process_observationsstate. # Collect the present joint angles and velocities from ROS for the state. last_observations = self.process_observations(obs_message, self.environment) # # # Get Jacobians from present joint angles and KDL trees # # # The Jacobians consist of a 6x6 matrix getting its from from # # # (# joint angles) x (len[x, y, z] + len[roll, pitch, yaw]) ee_link_jacobians = self.get_jacobians(last_observations) if self.environment['link_names'][-1] is None: print("End link is empty!!") return None else: trans, rot = forward_kinematics( self.scara_chain, self.environment['link_names'], last_observations[:self.scara_chain.getNrOfJoints()], base_link=self.environment['link_names'][0], end_link=self.environment['link_names'][-1]) rotation_matrix = np.eye(4) rotation_matrix[:3, :3] = rot rotation_matrix[:3, 3] = trans # angle, dir, _ = rotation_from_matrix(rotation_matrix) # current_quaternion = np.array([angle]+dir.tolist())# # # I need this calculations for the new reward function, need to send them back to the run mara or calculate them here # current_quaternion = quaternion_from_matrix(rotation_matrix) # tgt_quartenion = quaternion_from_matrix(self.target_orientation) current_quaternion = tf.quaternions.mat2quat(rot) #[w, x, y ,z] tgt_quartenion = tf.quaternions.mat2quat(self.target_orientation) # A = np.vstack([current_quaternion, np.ones(len(current_quaternion))]).T #quat_error = np.linalg.lstsq(A, tgt_quartenion)[0] quat_error = current_quaternion * tgt_quartenion.conjugate() vec, angle = tf.quaternions.quat2axangle(quat_error) rot_vec_err = [vec[0], vec[1], vec[2], np.float64(angle)] # convert quat to np arrays # quat_error = np.asarray(quat_error, dtype=np.quaternion).view((np.double, 4)) # RK: revisit this later, we only take one angle difference here! # angle_diff = 2 * np.arccos(np.clip(quat_error[..., 0], -1., 1.)) current_ee_tgt = np.ndarray.flatten( get_ee_points(self.environment['end_effector_points'], trans, rot).T) ee_points = current_ee_tgt - self.realgoal #self.environment['ee_points_tgt'] ee_points_jac_trans, _ = self.get_ee_points_jacobians( ee_link_jacobians, self.environment['end_effector_points'], rot) ee_velocities = self.get_ee_points_velocities( ee_link_jacobians, self.environment['end_effector_points'], rot, last_observations) # Concatenate the information that defines the robot state # vector, typically denoted asrobot_id 'x'. state = np.r_[np.reshape(last_observations, -1), np.reshape(ee_points, -1), np.reshape(rot_vec_err, -1), np.reshape(ee_velocities, -1), ] return state