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

    Args:
        ros_pose: A pose in ROS Pose format (type: Pose)

    Returns:
        An HTM (type: np.array).
    """

    # orientation
    np_pose = tf.quaternion_matrix([ros_pose.orientation.x, ros_pose.orientation.y, \
                                    ros_pose.orientation.z, ros_pose.orientation.w])

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

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

    Args:
        ros_pose: A pose in ROS Pose format (type: Pose)

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

    Args:
        ros_pose: A pose in ROS Pose format (type: Pose)

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