本文整理汇总了Python中tf.transformations.quaternion_matrix方法的典型用法代码示例。如果您正苦于以下问题：Python transformations.quaternion_matrix方法的具体用法？Python transformations.quaternion_matrix怎么用？Python transformations.quaternion_matrix使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类tf.transformations的用法示例。
在下文中一共展示了transformations.quaternion_matrix方法的7个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: _MsgToPose
# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import quaternion_matrix [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 示例2: _LocalToWorld
# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import quaternion_matrix [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 示例3: _LocalToWorld
# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import quaternion_matrix [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 示例4: tf_to_matrix
# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import quaternion_matrix [as 别名]
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) 示例5: pose_to_matrix
# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import quaternion_matrix [as 别名]
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) 示例6: transform_to_numpy
# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import quaternion_matrix [as 别名]
def transform_to_numpy(msg):
from tf import transformations
return np.dot(
transformations.translation_matrix(numpify(msg.translation)),
transformations.quaternion_matrix(numpify(msg.rotation))
) 示例7: pose_to_numpy
# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import quaternion_matrix [as 别名]
def pose_to_numpy(msg):
from tf import transformations
return np.dot(
transformations.translation_matrix(numpify(msg.position)),
transformations.quaternion_matrix(numpify(msg.orientation))
)