src/conntact/assembly_tools.py
def matrix_to_pose(input, base_frame):
        """Converts matrix into a pose.
        :param input: (np.Array) 4x4 homogeneous transformation matrix
        :param base_frame: (string) base frame for new pose.
        :return: (geometry_msgs.PoseStamped) Pose based on input.
        """
        output = PoseStamped()
        output.header.stamp = rospy.get_rostime()
        output.header.frame_id = base_frame

        quat = trfm.quaternion_from_matrix(input)
        output.pose.orientation.x = quat[0]
        output.pose.orientation.y = quat[1]
        output.pose.orientation.z = quat[2]
        output.pose.orientation.w = quat[3]
        output.pose.position.x = input[0][3]
        output.pose.position.y = input[1][3]
        output.pose.position.z = input[2][3]
        return output
Converts matrix into a pose.
        :param input: (np.Array) 4x4 homogeneous transformation matrix
        :param base_frame: (string) base frame for new pose.
        :return: (geometry_msgs.PoseStamped) Pose based on input.
        
Converts matrix into a pose.
Converts matrix into a pose .
def matrix_to_pose(input, base_frame):
        output = PoseStamped()
        output.header.stamp = rospy.get_rostime()
        output.header.frame_id = base_frame
        quat = trfm.quaternion_from_matrix(input)
        output.pose.orientation.x = quat[0]
        output.pose.orientation.y = quat[1]
        output.pose.orientation.z = quat[2]
        output.pose.orientation.w = quat[3]
        output.pose.position.x = input[0][3]
        output.pose.position.y = input[1][3]
        output.pose.position.z = input[2][3]
        return output
def matrix_to_pose ( input , base_frame ) : output = PoseStamped ( ) output . header . stamp = rospy . get_rostime ( ) output . header . frame_id = base_frame quat = trfm . quaternion_from_matrix ( input ) output . pose . orientation . x = quat [ 0 ] output . pose . orientation . y = quat [ 1 ] output . pose . orientation . z = quat [ 2 ] output . pose . orientation . w = quat [ 3 ] output . pose . position . x = input [ 0 ] [ 3 ] output . pose . position . y = input [ 1 ] [ 3 ] output . pose . position . z = input [ 2 ] [ 3 ] return output
Converts matrix into a pose.
Converts matrix into a pose .
"""Converts matrix into a pose.
        :param input: (np.Array) 4x4 homogeneous transformation matrix
        :param base_frame: (string) base frame for new pose.
        :return: (geometry_msgs.PoseStamped) Pose based on input.
        """
(geometry_msgs.PoseStamped) Pose based on input.
( geometry_msgs . PoseStamped ) Pose based on input .
input
(np.Array) 4x4 homogeneous transformation matrix
( np . Array ) 4x4 homogeneous transformation matrix
base_frame
(string) base frame for new pose.
( string ) base frame for new pose .