Exemple #1
0
    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), ]
Exemple #2
0
    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), ]
Exemple #4
0
    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