Python PoseConv.to_pos_quat方法代码示例

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_quat [as 别名]
    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

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_quat [as 别名]
    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

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_quat [as 别名]
 def add_marker(self, pose, name, color=[1,0,1], type=Marker.SPHERE, scale = [.02,.02,.02], points=None):
     vis_pub = rospy.Publisher(name, Marker, queue_size=10)
     marker = Marker()
     marker.header.frame_id = "world"
     marker.header.stamp = rospy.Time() 
     marker.ns = "my_namespace"
     marker.id = 0
     marker.type = type
     marker.action = Marker.ADD
     
     if type == Marker.LINE_LIST:
         for point in points:
             p = Point()
             p.x = point[0]
             p.y = point[1]
             p.z = point[2]
             marker.points.append(p)
     else:
         r = self.find_rotation_matrix_between_two_vectors([1,0,0], [0,0,1])
         rot = pose[0:3,0:3] * r
         pose2 = numpy.matrix(numpy.identity(4))
         pose2[0:3,0:3] = rot
         pose2[0:3,3] = pose[0:3,3]
         quat_pose = PoseConv.to_pos_quat(pose2)
         
         marker.pose.position.x = quat_pose[0][0]
         marker.pose.position.y = quat_pose[0][1]
         marker.pose.position.z = quat_pose[0][2]
         marker.pose.orientation.x = quat_pose[1][0]
         marker.pose.orientation.y = quat_pose[1][1] 
         marker.pose.orientation.z = quat_pose[1][2]
         marker.pose.orientation.w = quat_pose[1][3] 
         
     marker.scale.x = scale[0]
     marker.scale.y = scale[1]
     marker.scale.z = scale[2]
     marker.color.a = .5
     marker.color.r = color[0]
     marker.color.g = color[1]
     marker.color.b = color[2]
     
     
     
     vis_pub.publish(marker)

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_quat [as 别名]
    def global_input_cb(self, msg):
        self.force_monitor.update_activity()
        if self.is_forced_retreat:
            return
        rospy.loginfo("[face_adls_manager] Performing global move.")
        if type(msg) == String:
            self.publish_feedback(Messages.GLOBAL_START %msg.data)
            goal_ell_pose = self.global_poses[msg.data]
        elif type(msg) == PoseStamped:
            try:
                self.tfl.waitForTransform(msg.header.frame_id, '/ellipse_frame', msg.header.stamp, rospy.Duration(8.0))
                goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
                goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(goal_cart_pose)
                flip_quat = trans.quaternion_from_euler(-np.pi/2, np.pi, 0)
                goal_cart_quat_flipped = trans.quaternion_multiply(goal_cart_quat, flip_quat)
                goal_cart_pose = PoseConv.to_pose_stamped_msg('/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
                self.publish_feedback('Moving around ellipse to clicked position).')
                goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(goal_cart_pose)
            except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
                rospy.logwarn("[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s" %e)
                return

        curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
        curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
        retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
        retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot, [1.,0.,0.,0.])
        approach_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT]
        approach_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
        final_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST]
        final_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
        
        cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose((retreat_ell_pos, retreat_ell_quat))
        cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_ret_pose)

        cart_app_pose = self.ellipsoid.ellipsoidal_to_pose((approach_ell_pos, approach_ell_quat))
        cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_app_pose)

        cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose((final_ell_pos, final_ell_quat))
        cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_goal_pose)

        retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
                                                                  retreat_ell_pos, retreat_ell_quat,
                                                                  velocity=0.01, min_jerk=True)
        transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(retreat_ell_pos, retreat_ell_quat,
                                                                     approach_ell_pos, approach_ell_quat,
                                                                     velocity=0.01, min_jerk=True)
        approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(approach_ell_pos, approach_ell_quat,
                                                                   final_ell_pos, final_ell_quat,
                                                                   velocity=0.01, min_jerk=True)
        
        full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
        full_cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj]
        cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
        head = Header()
        head.frame_id = '/ellipse_frame'
        head.stamp = rospy.Time.now()
        pose_array = PoseArray(head, cart_traj_msg)
        self.pose_traj_pub.publish(pose_array)

        ps = PoseStamped()
        ps.header = head
        ps.pose = cart_traj_msg[0]
        
        self.force_monitor.update_activity()

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_quat [as 别名]
 def pose_to_ellipsoidal(self, pose):
     pose_pos, pose_rot = PoseConv.to_pos_rot(pose)
     lat, lon, height = self.pos_to_ellipsoidal(pose_pos[0,0], pose_pos[1,0], pose_pos[2,0])
     _, ell_rot = PoseConv.to_pos_rot(self.normal_to_ellipse(lat, lon, height))
     _, quat_rot = PoseConv.to_pos_quat(np.mat([0]*3).T, ell_rot.T * pose_rot)
     return [lat, lon, height], quat_rot

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_quat [as 别名]
 def ellipsoidal_to_pose(self, pose):
     ell_pos, ell_quat = PoseConv.to_pos_quat(pose)
     if not self.is_oblate:
         return self._ellipsoidal_to_pose_prolate(ell_pos, ell_quat)
     else:
         return self._ellipsoidal_to_pose_oblate(ell_pos, ell_quat)

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_quat [as 别名]
#!/usr/bin/python
import numpy as np
import roslib
roslib.load_manifest("hrl_geom")
import rospy
from hrl_geom.pose_converter import PoseConv

if __name__ == "__main__":
    rospy.init_node("test_poseconv")
    homo_mat = PoseConv.to_homo_mat([0., 1., 2.], [0., 0., np.pi/2])
    pose_msg = PoseConv.to_pose_msg(homo_mat)
    pos, quat = PoseConv.to_pos_quat(pose_msg)
    pose_stamped_msg = PoseConv.to_pose_stamped_msg("/base_link", pos, quat)
    pose_stamped_msg2 = PoseConv.to_pose_stamped_msg("/base_link", [pos, quat])
    tf_stamped = PoseConv.to_tf_stamped_msg("/new_link", pose_stamped_msg)
    p, R = PoseConv.to_pos_rot("/new_link", tf_stamped)
    for name in ["homo_mat", "pose_msg", "pos", "quat", "pose_stamped_msg", 
                 "pose_stamped_msg2", "tf_stamped", "p", "R"]:
        print "%s: \n" % name, locals()[name]