Python tf.transformations方法代碼示例

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def get_homo_matrix_from_pose_msg(pose, tag=""):
    basetrans = tf.transformations.translation_matrix((pose.position.x,
                                                       pose.position.y,
                                                       pose.position.z))

    baserot = tf.transformations.quaternion_matrix((pose.orientation.x,
                                                    pose.orientation.y,
                                                    pose.orientation.z,
                                                    pose.orientation.w))

    # rospy.loginfo(tag + " basetrans: " + str(basetrans))
    # rospy.loginfo(tag +" baserot: " + str(baserot))

    combined = numpy.matmul(basetrans, baserot)

    # rospy.loginfo(tag + " combined: " + str(combined))
    trans = tf.transformations.translation_from_matrix(combined)
    quat = tf.transformations.quaternion_from_matrix(combined)

    # rospy.loginfo(tag + " back basetrans: " + str(trans))
    # rospy.loginfo(tag +" back baserot: " + str(quat))

    return combined 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def multiply_transform_old(t1, t2):
    """
    Combines two transformations together
    The order is translation first, rotation then
    :param t1: [[x, y, z], [x, y, z, w]] or matrix 4x4
    :param t2: [[x, y, z], [x, y, z, w]] or matrix 4x4
    :return: The combination t1-t2 in the form [[x, y, z], [x, y, z, w]] or matrix 4x4
    """
    if _is_indexable(t1) and len(t1) == 2:
        t1m = tf.transformations.translation_matrix(t1[0])
        r1m = tf.transformations.quaternion_matrix(t1[1])
        m1m = dot(t1m, r1m)
        t2m = tf.transformations.translation_matrix(t2[0])
        r2m = tf.transformations.quaternion_matrix(t2[1])
        m2m = dot(t2m, r2m)
        rm = dot(m1m, m2m)
        rt = tf.transformations.translation_from_matrix(rm)
        rr = tf.transformations.quaternion_from_matrix(rm)
        return [list(rt), list(rr)]
    else:
        return dot(t1, t2) 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def compute_grasp_pose_offset(self, pose):
        """
        :param pose:
        :return:
        """

        yrot = tf.transformations.quaternion_from_euler(0, math.pi, 0)

        cubeorientation = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
        # oorient = [overhead_orientation.x,overhead_orientation.y,overhead_orientation.z,overhead_orientation.w]

        # resultingorient = tf.transformations.quaternion_multiply(cubeorientation, tf.transformations.quaternion_conjugate(oorient))
        resultingorient = tf.transformations.quaternion_multiply(cubeorientation, yrot)

        # resultingorient = cubeorientation


        pose.orientation = Quaternion(x=resultingorient[0], y=resultingorient[1], z=resultingorient[2],
                                      w=resultingorient[3])

        pose.position.x += 0
        pose.position.y += 0
        pose.position.z = demo_constants.TABLE_HEIGHT + demo_constants.CUBE_EDGE_LENGTH
        return pose 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def _MsgToPose(msg):
        """
        Parse the ROS message to a 4x4 pose format
        @param msg The ros message containing a pose
        @return A 4x4 transformation matrix containing the pose
        as read from the message
        """
        import tf.transformations as transformations
        #Get translation and rotation (from Euler angles)
        pose = transformations.quaternion_matrix(numpy.array([msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w]))
    
        pose[0,3] = msg.pose.position.x
        pose[1,3] = msg.pose.position.y
        pose[2,3] = msg.pose.position.z
        
	return pose 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def _LocalToWorld(self,pose):
        """
        Transform a pose from local frame to world frame
        @param pose The 4x4 transformation matrix containing the pose to transform
        @return The 4x4 transformation matrix describing the pose in world frame
        """
        import rospy
        #Get pose w.r.t world frame
        self.listener.waitForTransform(self.world_frame,self.detection_frame,
                                       rospy.Time(),rospy.Duration(10))
        t, r = self.listener.lookupTransform(self.world_frame,self.detection_frame,
                                             rospy.Time(0))
        
        #Get relative transform between frames
        offset_to_world = numpy.matrix(transformations.quaternion_matrix(r))
        offset_to_world[0,3] = t[0]
        offset_to_world[1,3] = t[1]
        offset_to_world[2,3] = t[2]
        
        #Compose with pose to get pose in world frame
        result = numpy.array(numpy.dot(offset_to_world, pose))
        
        return result 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def _LocalToWorld(self,pose):
        """
        Transform a pose from local frame to world frame
        @param pose The 4x4 transformation matrix containing the pose to transform
        @return The 4x4 transformation matrix describing the pose in world frame
        """
        #Get pose w.r.t world frame
        self.listener.waitForTransform(self.world_frame,self.detection_frame,
                                       rospy.Time(),rospy.Duration(10))
        t, r = self.listener.lookupTransform(self.world_frame,self.detection_frame,
                                             rospy.Time(0))
        
        #Get relative transform between frames
        offset_to_world = numpy.matrix(transformations.quaternion_matrix(r))
        offset_to_world[0,3] = t[0]
        offset_to_world[1,3] = t[1]
        offset_to_world[2,3] = t[2]
        
        #Compose with pose to get pose in world frame
        result = numpy.array(numpy.dot(offset_to_world, pose))
        
        return result 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def base_pose_xyyaw(self):
        """
        Returns the (x, y, yaw) of the base. Note that here we assume
         that the robot moves on a flat floor

        :returns: (x, y, yaw)
        :rtype: (float, float, float)
        """
        trans, rot, T = self.base_pose
        if T is None:
            return None, None, None
        angle = np.arctan2(rot[1, 0], rot[0, 0])
        # angle, axis, _ = tf.transformations.rotation_from_matrix(T)
        x = trans[0]
        y = trans[1]
        yaw = angle
        return x, y, yaw 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def quat_rotate(rotation, vector):
    """
    Rotate a vector according to a quaternion. Equivalent to the C++ method tf::quatRotate
    :param rotation: the rotation
    :param vector: the vector to rotate
    :return: the rotated vector
    """

    def quat_mult_point(q, w):
        return (q[3] * w[0] + q[1] * w[2] - q[2] * w[1],
                q[3] * w[1] + q[2] * w[0] - q[0] * w[2],
                q[3] * w[2] + q[0] * w[1] - q[1] * w[0],
                -q[0] * w[0] - q[1] * w[1] - q[2] * w[2])

    q = quat_mult_point(rotation, vector)
    q = tf.transformations.quaternion_multiply(
        q, tf.transformations.quaternion_inverse(rotation))
    return [q[0], q[1], q[2]] 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def publish_waypoints(self):
        """
        Publish the ROS message containing the waypoints
        """
        msg = Path()
        msg.header.frame_id = "map"
        msg.header.stamp = rospy.Time.now()
        if self.current_route is not None:
            for wp in self.current_route:
                pose = PoseStamped()
                pose.pose.position.x = wp[0].transform.location.x
                pose.pose.position.y = -wp[0].transform.location.y
                pose.pose.position.z = wp[0].transform.location.z

                quaternion = tf.transformations.quaternion_from_euler(
                    0, 0, -math.radians(wp[0].transform.rotation.yaw))
                pose.pose.orientation.x = quaternion[0]
                pose.pose.orientation.y = quaternion[1]
                pose.pose.orientation.z = quaternion[2]
                pose.pose.orientation.w = quaternion[3]
                msg.poses.append(pose)

        self.waypoint_publisher.publish(msg)
        rospy.loginfo("Published {} waypoints.".format(len(msg.poses))) 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def get_tray_place_block_pose(self):
        #return self.gazebo_pose
        copygazebopose = copy.deepcopy(self.gazebo_pose)

        yoffset = -0.08 + 0.075 * len(self.blocks)
        copygazebopose.position.y -= yoffset
        copygazebopose.position.z += self.TRAY_SURFACE_THICKNESS

        zrot = tf.transformations.quaternion_from_euler(0, 0, -math.pi/2.0)

        trayorientatin = [copygazebopose.orientation.x, copygazebopose.orientation.y, copygazebopose.orientation.z, copygazebopose.orientation.w]
        # oorient = [overhead_orientation.x,overhead_orientation.y,overhead_orientation.z,overhead_orientation.w]

        # resultingorient = tf.transformations.quaternion_multiply(cubeorientation, tf.transformations.quaternion_conjugate(oorient))
        resultingorient = tf.transformations.quaternion_multiply(trayorientatin, zrot)

        # resultingorient = cubeorientation


        copygazebopose.orientation = Quaternion(x=resultingorient[0], y=resultingorient[1], z=resultingorient[2],
                                      w=resultingorient[3])


        rospy.logwarn("Tray Place location (objects %d, %lf): " % (len(self.blocks), yoffset) + str(copygazebopose))

        return copygazebopose 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def homotransform_to_pose_msg(homotransform):
    trans = tf.transformations.translation_from_matrix(homotransform)
    quat = tf.transformations.quaternion_from_matrix(homotransform)
    return Pose(
        position=Point(x=trans[0], y=trans[1], z=trans[2]),
        orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def __init__(self, kinbody_path, detection_frame, world_frame,
                 service_namespace=None):
        """
        This initializes a simtrack detector.
        
        @param kinbody_path The path to the folder where kinbodies are stored
        @param detection_frame The TF frame of the camera
        @param world_frame The desired world TF frame
        @param service_namespace The namespace for the simtrack service (default: /simtrack)
        """
        import rospy
        import tf
        import tf.transformations as transformations
        # Initialize a new ros node if one has not already been created
        try:
            rospy.init_node('simtrack_detector', anonymous=True)
        except rospy.exceptions.ROSException:
            pass
            
        #For getting transforms in world frame
        if detection_frame is not None and world_frame is not None:
            self.listener = tf.TransformListener()
        else:
            self.listener = None
                
        if service_namespace is None:
            service_namespace='/simtrack'

        self.detection_frame = detection_frame
        self.world_frame = world_frame
        self.service_namespace = service_namespace
            
        self.kinbody_path = kinbody_path

        # A map of known objects that can be queries
        self.kinbody_to_query_map = {'fuze_bottle': 'fuze_bottle_visual',
                                     'pop_tarts': 'pop_tarts_visual'}
        self.query_to_kinbody_map = {'fuze_bottle_visual': 'fuze_bottle',
                                     'pop_tarts_visual': 'pop_tarts'} 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def _MsgToPose(msg):
        """
        Parse the ROS message to a 4x4 pose format
        @param msg The ros message containing a pose
        @return A 4x4 transformation matrix containing the pose
        as read from the message
        """
        #Get translation and rotation (from Euler angles)
        pose = transformations.euler_matrix(msg.roll*0.0,msg.pitch*0.0,msg.yaw*0.0) 
    
        pose[0,3] = msg.pt.x
        pose[1,3] = msg.pt.y
        pose[2,3] = msg.pt.z
        
	return pose 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def pose_to_tf(msg):
    q = np.array([msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w])
    T = tf.transformations.quaternion_matrix(q)
    T[0:3,3] = np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
    return (T) 

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import transformations [as 別名]
def _parse_pose_msg(self, pose):
        """
        Convert the ros topic pose (geometry_msgs/Pose)
        into translational vector
        and the rotational vector

        :param pose: pose
        :type pose: geometry_msgs.Pose

        :return: (pos, ori)
                  pos: translational vector (shape: :math:`[3, ]`)
                  ori: rotational vector (shape: :math:`[3, 3]`)
        :rtype: (numpy.ndarray, numpy.ndarray)
        """
        pos = np.array(
            [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z]
        )
        quat = np.array(
            [
                pose.pose.orientation.x,
                pose.pose.orientation.y,
                pose.pose.orientation.z,
                pose.pose.orientation.w,
            ]
        )
        ori = tf.transformations.quaternion_matrix(quat)[:3, :3]
        return pos, ori