forward_kinematics.py
def np2ros(np_pose):
    """Transform pose from np.array format to ROS Pose format.

    Args:
        np_pose: A pose in np.array format (type: np.array)

    Returns:
        An HTM (type: Pose).
    """

    # ROS pose
    ros_pose = Pose()

    # ROS position
    ros_pose.position.x = np_pose[0, 3]
    ros_pose.position.y = np_pose[1, 3]
    ros_pose.position.z = np_pose[2, 3]

    # ROS orientation
    np_q = tf.quaternion_from_matrix(np_pose)
    ros_pose.orientation.x = np_q[0]
    ros_pose.orientation.y = np_q[1]
    ros_pose.orientation.z = np_q[2]
    ros_pose.orientation.w = np_q[3]

    return ros_pose
Transform pose from np.array format to ROS Pose format.

    Args:
        np_pose: A pose in np.array format (type: np.array)

    Returns:
        An HTM (type: Pose).
    
Transform pose from np.array format to ROS Pose format.
Transform pose from np . array format to ROS Pose format .
def np2ros(np_pose):
    ros_pose = Pose()
    ros_pose.position.x = np_pose[0, 3]
    ros_pose.position.y = np_pose[1, 3]
    ros_pose.position.z = np_pose[2, 3]
    np_q = tf.quaternion_from_matrix(np_pose)
    ros_pose.orientation.x = np_q[0]
    ros_pose.orientation.y = np_q[1]
    ros_pose.orientation.z = np_q[2]
    ros_pose.orientation.w = np_q[3]
    return ros_pose
def np2ros ( np_pose ) : ros_pose = Pose ( ) ros_pose . position . x = np_pose [ 0 , 3 ] ros_pose . position . y = np_pose [ 1 , 3 ] ros_pose . position . z = np_pose [ 2 , 3 ] np_q = tf . quaternion_from_matrix ( np_pose ) ros_pose . orientation . x = np_q [ 0 ] ros_pose . orientation . y = np_q [ 1 ] ros_pose . orientation . z = np_q [ 2 ] ros_pose . orientation . w = np_q [ 3 ] return ros_pose
Transform pose from np.array format to ROS Pose format.
Transform pose from np . array format to ROS Pose format .
"""Transform pose from np.array format to ROS Pose format.

    Args:
        np_pose: A pose in np.array format (type: np.array)

    Returns:
        An HTM (type: Pose).
    """ # ROS pose # ROS position # ROS orientation
An HTM (type: Pose).
An HTM ( type : Pose ) .
np_pose
A pose in np.array format (type: np.array)
A pose in np . array format ( type : np . array )