Python pose_converter.PoseConv类代码示例

 def cart_error(self, ep_actual, ep_desired):
     pos_act, rot_act = PoseConv.to_pos_rot(ep_actual)
     pos_des, rot_des = PoseConv.to_pos_rot(ep_desired)
     err = np.mat(np.zeros((6, 1)))
     err[:3,0] = pos_act - pos_des
     err[3:6,0] = np.mat(trans.euler_from_matrix(rot_des.T * rot_act)).T
     return err

    def _normal_to_ellipse_oblate(self, lat, lon, height):
        pos = self.ellipsoidal_to_cart(lat, lon, height)
        df_du = self.partial_height(-lat, lon, height)
        nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
        j = np.sqrt(1./(1.+ny*ny/(nz*nz)))
        k = -ny*j/nz
        norm_rot = np.mat([[-nx,  ny*k - nz*j,  0],      
                           [-ny,  -nx*k,        j],      
                           [-nz,  nx*j,         k]])
        _, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
        rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2])
        #print norm_rot
        quat_ortho_rot = trans.quaternion_from_euler(rot_angle, 0.0, 0.0)
        norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
        quat_ortho_rot2 = trans.quaternion_from_euler(0.0, np.pi/2, 0.0)
        norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_ortho_rot2)
        if lon >= np.pi:
            quat_flip = trans.quaternion_from_euler(0.0, 0.0, np.pi)
            norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_flip)

        pose = PoseConv.to_pos_quat(pos, norm_quat_ortho)
        #print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
        #       (lat, lon, height) +
        #       str(PoseConv.to_homo_mat(pose)))
        return pose

 def robot_ellipsoidal_pose(self, lat, lon, height, orient_quat, kinect_frame_mat=None):
     if kinect_frame_mat is None:
         kinect_frame_mat = self.get_ell_frame()
     pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
     quat_rotated = trans.quaternion_multiply(quat, orient_quat)
     ell_pose_mat = PoseConv.to_homo_mat(pos, quat_rotated)
     return PoseConv.to_pos_rot(kinect_frame_mat * ell_pose_mat)

    def retreat_move(self, height, velocity, forced=False):
        if forced:
            self.is_forced_retreat = True
        cart_pos, cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
        ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
        #print "Retreat EP:", ell_pos
        latitude = ell_pos[0]
        if self.trim_retreat:
            latitude = min(latitude, TRIM_RETREAT_LATITUDE)
        #rospy.loginfo("[face_adls_manager] Retreating from current location.")

        retreat_pos = [latitude, ell_pos[1], height]
        retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos)
        retreat_quat = [0,0,0,1]
        if forced:
            cart_path = self.ellipsoid.ellipsoidal_to_pose((retreat_pos, retreat_quat))
            cart_msg = [PoseConv.to_pose_msg(cart_path)]
        else:
            ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos,
                                                              ell_quat,
                                                              retreat_pos,
                                                              retreat_quat,
                                                              velocity=0.01,
                                                              min_jerk=True)
            cart_path = [self.ellipsoid.ellipsoidal_to_pose(ell_pose) for ell_pose in ell_path]
            cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path]
            
        head = Header()
        head.frame_id = '/ellipse_frame'
        head.stamp = rospy.Time.now()
        pose_array = PoseArray(head, cart_msg)
        self.pose_traj_pub.publish(pose_array)

        self.is_forced_retreat = False

    def _normal_to_ellipse_prolate(self, lat, lon, height):
        pos = self.ellipsoidal_to_cart(lat, lon, height)
        df_du = self.partial_height(lat, lon, height)
        nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
        j = np.sqrt(1./(1.+ny*ny/(nz*nz)))
        k = -ny*j/nz
        norm_rot = np.mat([[-nx,  ny*k - nz*j,  0],      
                           [-ny,  -nx*k,        j],      
                           [-nz,  nx*j,         k]])
        _, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
        rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2])
        #print norm_rot
        quat_ortho_rot = trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0)
        norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
        norm_rot_ortho = np.mat(trans.quaternion_matrix(norm_quat_ortho)[:3,:3])
        if norm_rot_ortho[2, 2] > 0:
            flip_axis_ang = 0
        else:
            flip_axis_ang = np.pi
        quat_flip = trans.quaternion_from_euler(flip_axis_ang, 0.0, 0.0)
        norm_quat_ortho_flipped = trans.quaternion_multiply(norm_quat_ortho, quat_flip)

        pose = PoseConv.to_pos_quat(pos, norm_quat_ortho_flipped)
        #print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
        #       (lat, lon, height) +
        #       str(PoseConv.to_homo_mat(pose)))
        return pose

 def load_params(self, params):
     kinect_B_head = PoseConv.to_homo_mat(params.e_frame)
     base_B_kinect = self.kin_head.forward(base_link="base_link",
                                           end_link=self.kinect_frame)
     base_B_head = base_B_kinect * kinect_B_head
     self.head_center = PoseConv.to_pose_stamped_msg("/base_link",base_B_head)
     self.ell_space = EllipsoidSpace()
     self.ell_space.load_ell_params(params.E, params.is_oblate, params.height)
     rospy.loginfo("Loaded ellispoidal parameters.")

 def create_tool_arrow():
     arrows = MarkerArray()
     color = ColorRGBA(0., 0., 1., 1.)
     for i, param in enumerate(params):
         ell_pos, ell_rot = params[param]
         _, ell_rot_mat = PoseConv.to_pos_rot([0]*3, ell_rot)
         cart_pose = PoseConv.to_homo_mat(ell_space.ellipsoidal_to_pose(*ell_pos))
         cart_pose[:3,:3] = cart_pose[:3,:3] * ell_rot_mat
         arrow = create_arrow_marker(cart_pose, i, color)
         arrow.header.stamp = rospy.Time.now()
         arrows.markers.append(arrow)
     return arrows

    def inverse(self, pose, q_guess=None, min_joints=None, max_joints=None):
        pos, rot = PoseConv.to_pos_rot(pose)
        pos_kdl = kdl.Vector(pos[0,0], pos[1,0], pos[2,0])
        rot_kdl = kdl.Rotation(rot[0,0], rot[0,1], rot[0,2],
                               rot[1,0], rot[1,1], rot[1,2],
                               rot[2,0], rot[2,1], rot[2,2])
        frame_kdl = kdl.Frame(rot_kdl, pos_kdl)
        if min_joints is None:
            min_joints = self.joint_safety_lower
        if max_joints is None:
            max_joints = self.joint_safety_upper
        mins_kdl = joint_list_to_kdl(min_joints)
        maxs_kdl = joint_list_to_kdl(max_joints)
        ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(self.chain, mins_kdl, maxs_kdl, 
                                              self._fk_kdl, self._ik_v_kdl)

        if q_guess == None:
            # use the midpoint of the joint limits as the guess
            lower_lim = np.where(np.isfinite(min_joints), min_joints, 0.)
            upper_lim = np.where(np.isfinite(max_joints), max_joints, 0.)
            q_guess = (lower_lim + upper_lim) / 2.0
            q_guess = np.where(np.isnan(q_guess), [0.]*len(q_guess), q_guess)

        q_kdl = kdl.JntArray(self.num_joints)
        q_guess_kdl = joint_list_to_kdl(q_guess)
        if ik_p_kdl.CartToJnt(q_guess_kdl, frame_kdl, q_kdl) >= 0:
            return np.array(joint_kdl_to_list(q_kdl))
        else:
            return None

def main():
    rospy.init_node("visualize_poses")
    pose_file = file(sys.argv[1], 'r')
    params = yaml.load(pose_file)
    ell_reg_bag = rosbag.Bag(sys.argv[2], 'r')
    for topic, ell_reg, ts in ell_reg_bag.read_messages():
        pass
    ell_space = EllipsoidSpace(E=ell_reg.E)

    pub_head_pose = rospy.Publisher("/head_center_test", PoseStamped)
    pub_arrows = rospy.Publisher("visualization_markers_array", MarkerArray)
    def create_tool_arrow():
        arrows = MarkerArray()
        color = ColorRGBA(0., 0., 1., 1.)
        for i, param in enumerate(params):
            ell_pos, ell_rot = params[param]
            _, ell_rot_mat = PoseConv.to_pos_rot([0]*3, ell_rot)
            cart_pose = PoseConv.to_homo_mat(ell_space.ellipsoidal_to_pose(*ell_pos))
            cart_pose[:3,:3] = cart_pose[:3,:3] * ell_rot_mat
            arrow = create_arrow_marker(cart_pose, i, color)
            arrow.header.stamp = rospy.Time.now()
            arrows.markers.append(arrow)
        return arrows
    r = rospy.Rate(10)
    while not rospy.is_shutdown():
        pub_head_pose.publish(PoseConv.to_pose_stamped_msg("/base_link", [0]*3, [0]*3))
        arrows = create_tool_arrow()
        pub_arrows.publish(arrows)
        r.sleep()

    def init_reg_cb(self, req):
        # TODO REMOVE THIS FACE SIDE MESS
        self.publish_feedback("Performing Head Registration. Please Wait.")
        self.face_side = rospy.get_param("~face_side1", 'r')
        bag_str = self.reg_dir + '/' + '_'.join([self.subject, self.face_side, "head_transform"]) + ".bag"
        rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
        try:
            bag = rosbag.Bag(bag_str, 'r')
            for topic, msg, ts in bag.read_messages():
                head_tf = msg
            assert (head_tf is not None), "Error reading head transform bagfile"
            bag.close()
        except Exception as e:
            self.publish_feedback("Registration failed: Error loading saved registration.")
            rospy.logerr("[%s] Cannot load registration parameters from %s:\r\n%s" %
                         (rospy.get_name(), bag_str, e))
            return (False, PoseStamped())

        if self.face_side == 'r':
            head_registration = self.head_registration_r
        else:
            head_registration = self.head_registration_l
        try:
            rospy.loginfo("[%s] Requesting head registration for %s at pixel (%d, %d)." %(rospy.get_name(),
                                                                                          self.subject,
                                                                                          req.u, req.v))
            self.head_pc_reg = head_registration(req.u, req.v).reg_pose
            if ((self.head_pc_reg.pose.position == Point(0.0, 0.0, 0.0)) and
                (self.head_pc_reg.pose.orientation == Quaternion(0.0, 0.0, 0.0, 1.0))):
               raise rospy.ServiceException("Unable to find a good match.")
               self.head_pc_reg = None
        except rospy.ServiceException as se:
            self.publish_feedback("Registration failed: %s" %se)
            return (False, PoseStamped())

        pc_reg_mat = PoseConv.to_homo_mat(self.head_pc_reg)
        head_tf_mat = PoseConv.to_homo_mat(Transform(head_tf.transform.translation,
                                                     head_tf.transform.rotation))
        self.head_pose = PoseConv.to_pose_stamped_msg(pc_reg_mat**-1 * head_tf_mat)
        self.head_pose.header.frame_id = self.head_pc_reg.header.frame_id
        self.head_pose.header.stamp = self.head_pc_reg.header.stamp

        side = "right" if (self.face_side == 'r') else "left"
        self.publish_feedback("Registered head using %s cheek model, please check and confirm." %side)
#        rospy.loginfo('[%s] Head frame registered at:\r\n %s' %(rospy.get_name(), self.head_pose))
        self.test_pose.publish(self.head_pose)
        return (True, self.head_pose)

 def FK(self, q, link_number=None):
     if link_number is not None:
         end_link = self.get_link_names(fixed=False)[link_number]
     else:
         end_link = None
     homo_mat = self.forward(q, end_link)
     pos, rot = PoseConv.to_pos_rot(homo_mat)
     return pos, rot

 def point_towards_midpoint(self, clean_joints, psm1_pos, psm2_pos, key_hole,ecm_pose):
     mid_point = (psm1_pos + psm2_pos)/2
 #     mid_point = ecm_pose[0:3,3] - numpy.array([0,0,.01]).reshape(3,1)
     self.add_marker(PoseConv.to_homo_mat([mid_point, [0,0,0]]), '/marker_subscriber',color=[1,0,0], scale=[0.047/5,0.047/5,0.047/5])
     self.add_marker(PoseConv.to_homo_mat([key_hole,[0,0,0]]), '/keyhole_subscriber',[0,0,1])
     self.add_marker(ecm_pose, '/current_ecm_pose', [1,0,0], Marker.ARROW, scale=[.1,.005,.005])
     temp = clean_joints['ecm'].position
     b,_ = self.ecm_kin.FK([temp[0],temp[1],.14,temp[3]])
     # find the equation of the line that goes through the key_hole and the 
     # mid_point
     ab_vector = (mid_point-key_hole)
     ecm_current_direction = b-key_hole 
     self.add_marker(ecm_pose, '/midpoint_to_keyhole', [0,1,1], type=Marker.LINE_LIST, scale = [0.005, 0.005, 0.005], points=[b, key_hole])
     
     self.add_marker(PoseConv.to_homo_mat([ab_vector,[0,0,0]]), '/ab_vector',[0,1,0], type=Marker.ARROW)
     r = self.find_rotation_matrix_between_two_vectors(ecm_current_direction, ab_vector)
     m = math.sqrt(ab_vector[0]**2 + ab_vector[1]**2 + ab_vector[2]**2) # ab_vector's length
     
     # insertion joint length
     l = math.sqrt( (ecm_pose[0,3]-key_hole[0])**2 + (ecm_pose[1,3]-key_hole[1])**2 + (ecm_pose[2,3]-key_hole[2])**2)
     
     # Equation of the line that passes through the midpoint of the tools and the key hole
     x = lambda t: key_hole[0] + ab_vector[0] * t
     y = lambda t: key_hole[1] + ab_vector[1] * t
     z = lambda t: key_hole[2] + ab_vector[2] * t
     
     t = l/m
     
     new_ecm_position = numpy.array([x(t), y(t), z(t)]).reshape(3,1)
     
     ecm_pose[0:3,0:3] =  r* ecm_pose[0:3,0:3]  
     ecm_pose[0:3,3] = new_ecm_position
     self.add_marker(ecm_pose, '/target_ecm_pose', [0,0,1], Marker.ARROW, scale=[.1,.005,.005])
     output_msg = clean_joints['ecm']
     
     
     try:
         p = self.ecm_kin.inverse(ecm_pose)
     except Exception as e:
         rospy.logerr('error')
     if p != None:  
         p[3] = 0
         output_msg.position = p
     return output_msg

    def register_ellipse(self, mode, side):
        reg_e_params = EllipsoidParams()
        try:
            now = rospy.Time.now()
            self.tfl.waitForTransform("/base_link", "/head_frame", now, rospy.Duration(10.))
            pos, quat = self.tfl.lookupTransform("/head_frame", "/base_frame", now)
            self.head_pose = PoseStamped()
            self.head_pose.header.stamp = now
            self.head_pose.header.frame_id = "/base_link"
            self.head_pose.pose.position = Point(*pos)
            self.head_pose.pose.orientation = Quaternion(*quat)
        except:
            rospy.logwarn("[%s] Head registration not loaded yet." %rospy.get_name())
            return (False, reg_e_params)
        reg_prefix = rospy.get_param("~registration_prefix", "")
        registration_files = rospy.get_param("~registration_files", None)
        if mode not in registration_files:
            rospy.logerr("[%s] Mode not in registration_files parameters" % (rospy.get_name()))
            return (False, reg_e_params)
        try:
            bag_str = reg_prefix + registration_files[mode][side]
            rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
            bag = rosbag.Bag(bag_str, 'r')
            e_params = None
            for topic, msg, ts in bag.read_messages():
                e_params = msg
            assert e_params is not None
            bag.close()
        except:
            rospy.logerr("[%s] Cannot load registration parameters from %s" %(rospy.get_name(), bag_str))
            return (False, reg_e_params)

        head_reg_mat = PoseConv.to_homo_mat(self.head_pose)
        ell_reg = PoseConv.to_homo_mat(Transform(e_params.e_frame.transform.translation,
                                                      e_params.e_frame.transform.rotation))
        reg_e_params.e_frame = PoseConv.to_tf_stamped_msg(head_reg_mat**-1 * ell_reg)
        reg_e_params.e_frame.header.frame_id = self.head_reg_tf.header.frame_id
        reg_e_params.e_frame.child_frame_id = '/ellipse_frame'
        reg_e_params.height = e_params.height
        reg_e_params.E = e_params.E
        self.ell_params_pub.publish(reg_e_params)
        self.ell_frame = reg_e_params.e_frame
        return (True, reg_e_params)

def create_base_marker(pose, id, color):
    marker = Marker()
    marker.header.frame_id = "base_link"
    marker.header.stamp = rospy.Time.now()
    marker.ns = "ar_servo"
    marker.id = id
    marker.pose = PoseConv.to_pose_msg(pose)
    marker.color = ColorRGBA(*(color + (1.0,)))
    marker.scale.x = 0.7; marker.scale.y = 0.7; marker.scale.z = 0.2
    return marker

 def enable_controller_cb(req):
     if req.enable:
         _, frame_rot = PoseConv.to_pos_rot([0]*3, 
                                     [req.frame_rot.x, req.frame_rot.y, req.frame_rot.z])
         if req.velocity == 0:
             req.velocity = 0.03
         success = self.enable_controller(req.end_link, req.ctrl_params, req.ctrl_name,
                                          frame_rot, velocity=req.velocity)
     else:
         success = self.disable_controller()
     return EnableCartControllerResponse(success)