Python tf.transformations.quaternion_matrix() Examples

The following are 7 code examples of tf.transformations.quaternion_matrix(). You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example. You may also want to check out all available functions/classes of the module tf.transformations , or try the search function .
Example #1
Source File: simtrack.py    From prpy with BSD 3-Clause "New" or "Revised" License 6 votes vote down vote up 
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
Example #2
Source File: simtrack.py    From prpy with BSD 3-Clause "New" or "Revised" License 6 votes vote down vote up 
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
Example #3
Source File: vncc.py    From prpy with BSD 3-Clause "New" or "Revised" License 6 votes vote down vote up 
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
Example #4
Source File: detect_aruco.py    From flock with BSD 3-Clause "New" or "Revised" License 5 votes vote down vote up 
def tf_to_matrix(ros_transform):
    """ROS transform to 4x4 matrix"""
    t, q = ros_transform
    t_matrix = tft.translation_matrix(t)
    r_matrix = tft.quaternion_matrix(q)
    return np.dot(t_matrix, r_matrix)
Example #5
Source File: detect_aruco.py    From flock with BSD 3-Clause "New" or "Revised" License 5 votes vote down vote up 
def pose_to_matrix(p):
    """geometry_msgs.msg.Pose to 4x4 matrix"""
    t_matrix = tft.translation_matrix([p.position.x, p.position.y, p.position.z])
    r_matrix = tft.quaternion_matrix([p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w])
    return np.dot(t_matrix, r_matrix)
Example #6
Source File: geometry.py    From ros_numpy with MIT License 5 votes vote down vote up 
def transform_to_numpy(msg):
	from tf import transformations

	return np.dot(
        transformations.translation_matrix(numpify(msg.translation)),
        transformations.quaternion_matrix(numpify(msg.rotation))
    )
Example #7
Source File: geometry.py    From ros_numpy with MIT License 5 votes vote down vote up 
def pose_to_numpy(msg):
	from tf import transformations

	return np.dot(
        transformations.translation_matrix(numpify(msg.position)),
        transformations.quaternion_matrix(numpify(msg.orientation))
    )