Пример #1
0
        def process_touch_pose(ud):
            ######################################################################### 
            # tranformation logic for manipulation
            # put touch pose in torso frame
            frame_B_touch = util.pose_msg_to_mat(ud.touch_click_pose)
            torso_B_frame = self.get_transform("torso_lift_link", 
                                               ud.touch_click_pose.header.frame_id)
            if torso_B_frame is None:
                return 'tf_failure'
            torso_B_touch_bad = torso_B_frame * frame_B_touch

            # rotate pixel23d the right way
            t_pos, t_quat = util.pose_mat_to_pq(torso_B_touch_bad)
            # rotate so x axis pointing out
            quat_flip_rot = tf_trans.quaternion_from_euler(0.0, np.pi/2.0, 0.0)
            quat_flipped = tf_trans.quaternion_multiply(t_quat, quat_flip_rot)
            # rotate around x axis so the y axis is flat
            mat_flipped = tf_trans.quaternion_matrix(quat_flipped)
            rot_angle = np.arctan(-mat_flipped[2,1] / mat_flipped[2,2])
            quat_ortho_rot = tf_trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0)
            quat_flipped_ortho = tf_trans.quaternion_multiply(quat_flipped, quat_ortho_rot)

            torso_B_touch = util.pose_pq_to_mat(t_pos, quat_flipped_ortho)

            # offset the touch location by the approach tranform
            appr_B_tool = self.get_transform(self.tool_approach_frame, self.tool_frame) 
            if appr_B_tool is None:
                return 'tf_failure'
            torso_B_touch_appr = torso_B_touch * appr_B_tool

            # project the approach back into the wrist
            appr_B_wrist = self.get_transform(self.tool_frame, 
                                              self.arm + "_wrist_roll_link") 
            if appr_B_wrist is None:
                return 'tf_failure'

            torso_B_wrist = torso_B_touch_appr * appr_B_wrist
            ######################################################################### 
            # create PoseStamped
            appr_wrist_ps = util.pose_mat_to_stamped_msg('torso_lift_link',
                                                         torso_B_wrist)
            appr_tool_ps = util.pose_mat_to_stamped_msg('torso_lift_link', 
                                                        torso_B_touch_appr)
            touch_tool_ps = util.pose_mat_to_stamped_msg('torso_lift_link', 
                                                         torso_B_touch)

            # visualization
            self.wrist_pub.publish(appr_wrist_ps)
            self.appr_pub.publish(appr_tool_ps)
            self.touch_pub.publish(touch_tool_ps)

            # set return values
            ud.appr_wrist_mat = torso_B_wrist
            ud.appr_tool_ps = appr_tool_ps
            ud.touch_tool_ps = touch_tool_ps

            
            return 'succeeded'
Пример #2
0
        def make_approach_pose(ud):
            frame_B_head = util.pose_msg_to_mat(ud.head_click_pose)
            base_B_frame = self.get_transform("base_link", ud.head_click_pose.header.frame_id)
            if base_B_frame is None:
                return 'tf_failure'
            base_B_head = base_B_frame * frame_B_head
            norm_z = np.array([base_B_head[0,2], base_B_head[1,2], 0])
            norm_z /= np.linalg.norm(norm_z)
            base_B_head[:3,:3] = np.mat([[-norm_z[0], norm_z[1], 0],
                                          [-norm_z[1],-norm_z[0], 0],
                                          [         0,         0, 1]])
            # offset in the x-direction by the given parameter
            offset_B_base = self.get_transform(self.base_offset_frame, "base_link")
            if offset_B_base is None:
                return 'tf_failure'
            nav_pose = base_B_head * offset_B_base  
            print nav_pose
            nav_pose[:4,3] = nav_pose * np.mat([[-self.nav_approach_dist, 0, 0, 1]]).T
            print nav_pose
            now = rospy.Time.now()
            nav_pose_ps = util.pose_mat_to_stamped_msg('base_link', nav_pose, now) 
            self.tf_listener.waitForTransform("/map", "base_link",
                                              now, timeout=rospy.Duration(20)) 
            nav_pose_map_ps = self.tf_listener.transformPose("/map", nav_pose_ps)

            #self.nav_pub.publish(util.pose_mat_to_stamped_msg('base_link', base_B_head, rospy.Time.now()))
            print base_B_head, base_B_head.T * base_B_head
            self.nav_pub.publish(nav_pose_map_ps)
            ud.nav_pose_ps = nav_pose_map_ps
            return 'succeeded'
Пример #3
0
    def find_controlled_tool_pose(self, target_pose):
        # find current tool location
        t_B_c = self.get_transform("torso_lift_link", self.tool_frame)
        # find error in position
        err_pos = target_pose[:3, 3] - t_B_c[:3, 3]
        # find error in angle
        err_ang = util.quaternion_dist(target_pose, t_B_c)

        # find control values
        u_x = self.x_pid.update_state(err_pos[0, 0])
        u_y = self.y_pid.update_state(err_pos[1, 0])
        u_z = self.z_pid.update_state(err_pos[2, 0]) + self.grav_comp
        u_pos = np.mat([u_x, u_y, u_z]).T
        u_a = self.a_pid.update_state(err_ang)
        #       quat_diff = (np.array(tf_trans.quaternion_from_matrix(target_pose)) -
        #                    np.array(tf_trans.quaternion_from_matrix(t_B_c)))
        #       u_qx = self.qx_pid.update_state(quat_diff[0])
        #       u_qy = self.qy_pid.update_state(quat_diff[1])
        #       u_qz = self.qz_pid.update_state(quat_diff[2])
        #       u_qw = self.qw_pid.update_state(quat_diff[3])
        #       u_quat = np.array([u_qx, u_qy, u_qz, u_qw])
        #       u_quat = u_quat / np.linalg.norm(u_quat)

        # find commanded frame of tool
        # rotation
        u_a = np.clip(u_a, 0, 1)
        ei_q_slerp = tf_trans.quaternion_slerp(
            tf_trans.quaternion_from_matrix(t_B_c),
            tf_trans.quaternion_from_matrix(target_pose), u_a)
        new_quat = ei_q_slerp
        #       new_quat = tf_trans.quaternion_multiply(tf_trans.quaternion_from_matrix(t_B_c),
        #                                               u_quat)
        t_B_new = np.mat(tf_trans.quaternion_matrix(new_quat))

        # position
        u_pos_clipped = np.clip(u_pos, -self.u_pos_max, self.u_pos_max)
        t_B_new[:3, 3] = t_B_c[:3, 3] + u_pos_clipped

        # publish informative topics
        self.goal_pub.publish(
            util.pose_mat_to_stamped_msg("torso_lift_link", target_pose))
        self.cur_pub.publish(
            util.pose_mat_to_stamped_msg("torso_lift_link", t_B_c))
        self.cmd_pub.publish(
            util.pose_mat_to_stamped_msg("torso_lift_link", t_B_new))

        return t_B_new, err_pos, err_ang
Пример #4
0
 def retreat_cb(ud, goal):
     # get the current location of the tool
     start_pose_mat = self.get_transform("torso_lift_link", self.tool_frame)
     start_pose_ps = util.pose_mat_to_stamped_msg("torso_lift_link", start_pose_mat)
     # move from the current location to the approach location
     lm_goal = EPCLinearMoveGoal(start_pose_ps, ud.end_pose, self.tool_frame, 
                                 False, 0.03, 0.1, 1.0, 3)
     return lm_goal
Пример #5
0
    def find_controlled_tool_pose(self, target_pose):
        # find current tool location
        t_B_c = self.get_transform("torso_lift_link", self.tool_frame)
        # find error in position
        err_pos = target_pose[:3,3] - t_B_c[:3,3]
        # find error in angle
        err_ang = util.quaternion_dist(target_pose, t_B_c)

        # find control values
        u_x = self.x_pid.update_state(err_pos[0,0])
        u_y = self.y_pid.update_state(err_pos[1,0])
        u_z = self.z_pid.update_state(err_pos[2,0]) + self.grav_comp
        u_pos = np.mat([u_x, u_y, u_z]).T
        u_a = self.a_pid.update_state(err_ang)
#       quat_diff = (np.array(tf_trans.quaternion_from_matrix(target_pose)) -
#                    np.array(tf_trans.quaternion_from_matrix(t_B_c)))
#       u_qx = self.qx_pid.update_state(quat_diff[0])
#       u_qy = self.qy_pid.update_state(quat_diff[1])
#       u_qz = self.qz_pid.update_state(quat_diff[2])
#       u_qw = self.qw_pid.update_state(quat_diff[3])
#       u_quat = np.array([u_qx, u_qy, u_qz, u_qw])
#       u_quat = u_quat / np.linalg.norm(u_quat)

        # find commanded frame of tool
        # rotation
        u_a = np.clip(u_a, 0, 1)
        ei_q_slerp = tf_trans.quaternion_slerp(tf_trans.quaternion_from_matrix(t_B_c),
                                               tf_trans.quaternion_from_matrix(target_pose),
                                               u_a)
        new_quat = ei_q_slerp
#       new_quat = tf_trans.quaternion_multiply(tf_trans.quaternion_from_matrix(t_B_c),
#                                               u_quat)
        t_B_new = np.mat(tf_trans.quaternion_matrix(new_quat))

        # position
        u_pos_clipped = np.clip(u_pos, -self.u_pos_max, self.u_pos_max)
        t_B_new[:3,3] = t_B_c[:3,3] + u_pos_clipped 

        # publish informative topics
        self.goal_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", target_pose))
        self.cur_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", t_B_c))
        self.cmd_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", t_B_new))
        
        return t_B_new, err_pos, err_ang