forward_kinematics.py
def ur2ros(ur_pose):
    """Transform pose from UR format to ROS Pose format.

    Args:
        ur_pose: A pose in UR format [px, py, pz, rx, ry, rz]
        (type: list)

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

    # ROS pose
    ros_pose = Pose()

    # ROS position
    ros_pose.position.x = ur_pose[0]
    ros_pose.position.y = ur_pose[1]
    ros_pose.position.z = ur_pose[2]

    # Ros orientation
    angle = sqrt(ur_pose[3] ** 2 + ur_pose[4] ** 2 + ur_pose[5] ** 2)
    direction = [i / angle for i in ur_pose[3:6]]
    np_T = tf.rotation_matrix(angle, direction)
    np_q = tf.quaternion_from_matrix(np_T)
    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 UR format to ROS Pose format.

    Args:
        ur_pose: A pose in UR format [px, py, pz, rx, ry, rz]
        (type: list)

    Returns:
        An HTM (type: Pose).
    
Transform pose from UR format to ROS Pose format.
Transform pose from UR format to ROS Pose format .
def ur2ros(ur_pose):
    ros_pose = Pose()
    ros_pose.position.x = ur_pose[0]
    ros_pose.position.y = ur_pose[1]
    ros_pose.position.z = ur_pose[2]
    angle = sqrt(ur_pose[3] ** 2 + ur_pose[4] ** 2 + ur_pose[5] ** 2)
    direction = [i / angle for i in ur_pose[3:6]]
    np_T = tf.rotation_matrix(angle, direction)
    np_q = tf.quaternion_from_matrix(np_T)
    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 ur2ros ( ur_pose ) : ros_pose = Pose ( ) ros_pose . position . x = ur_pose [ 0 ] ros_pose . position . y = ur_pose [ 1 ] ros_pose . position . z = ur_pose [ 2 ] angle = sqrt ( ur_pose [ 3 ] ** 2 + ur_pose [ 4 ] ** 2 + ur_pose [ 5 ] ** 2 ) direction = [ i / angle for i in ur_pose [ 3 : 6 ] ] np_T = tf . rotation_matrix ( angle , direction ) np_q = tf . quaternion_from_matrix ( np_T ) 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 UR format to ROS Pose format.
Transform pose from UR format to ROS Pose format .
"""Transform pose from UR format to ROS Pose format.

    Args:
        ur_pose: A pose in UR format [px, py, pz, rx, ry, rz]
        (type: list)

    Returns:
        An HTM (type: Pose).
    """ # ROS pose # ROS position # Ros orientation
An HTM (type: Pose).
An HTM ( type : Pose ) .
ur_pose
A pose in UR format [px, py, pz, rx, ry, rz]
A pose in UR format [ px py pz rx ry rz ]
(type
None