Forward Kinematics

In this tutorial, I will overview how to perform forward kinematics using a RobotSet.

Example code can be found here:

extern crate optima;

use nalgebra::DVector;
use optima::robot_set_modules::robot_set::RobotSet;
use optima::utils::utils_robot::robot_module_utils::RobotNames;
use optima::utils::utils_se3::optima_se3_pose::OptimaSE3PoseType;

fn main() {
    // Create a base configuration ur5.
    let robot_set = RobotSet::new_from_robot_names(vec![RobotNames::new_base("ur5")]);

    // Spawn joint state with zeros for all degrees of freedom.
    let joint_state = robot_set.spawn_robot_set_joint_state(DVector::from_vec(vec![0.0; 6])).expect("error");

    // Compute forward kinematics.  The second argument in `compute_fk` is the SE(3) representation that should
    // be used to compute FK.  All options are `ImplicitDualQuaternion`, `EulerAnglesAndTranslation`,
    // `RotationMatrixAndTranslation`, `UnitQuaternionAndTranslation`, and `HomogeneousMatrix`.
    // The result is an `RobotSetFKResult`.
    let fk_res = robot_set.robot_set_kinematics_module().compute_fk(&joint_state, &OptimaSE3PoseType::ImplicitDualQuaternion).expect("error");

    // Print summary of the fk result.
    fk_res.print_summary();

    println!("////////////////////////////////////////////////////////////////////////////////////");

    // Load the test configuration created in the previous tutorial.
    // Make sure that you ran example 3_robot_set_configurations prior to starting this tutorial.
    let robot_set = RobotSet::new_from_set_name("test_configuration");

    // Spawn joint state with zeros for all degrees of freedom.
    let joint_state = robot_set.spawn_robot_set_joint_state(DVector::from_vec(vec![0.0; 16])).expect("error");

    // Compute forward kinematics.  The second argument in `compute_fk` is the SE(3) representation that should
    // be used to compute FK.  All options are `ImplicitDualQuaternion`, `EulerAnglesAndTranslation`,
    // `RotationMatrixAndTranslation`, `UnitQuaternionAndTranslation`, and `HomogeneousMatrix`.
    // The result is an `RobotSetFKResult`.
    let fk_res = robot_set.robot_set_kinematics_module().compute_fk(&joint_state, &OptimaSE3PoseType::HomogeneousMatrix).expect("error");

    // Print summary of the fk result.
    fk_res.print_summary();

    println!("////////////////////////////////////////////////////////////////////////////////////");

    // Access the pose of the just computed fk result.
    // The arguments in `get_pose_from_idxs` here are `robot_idx_in_set` (in this case,
    // the sawyer robot, the "second" robot in the set) and `link_idx_in_robot` (in this case,
    // link 18 "right hand" on the sawyer robot).
    let pose = fk_res.get_pose_from_idxs(1, 18);
    println!("{:?}", pose);
}

Explanations are written in this code line by line.

Note that any link that is removed from the model (i.e., by setting a dead-end link in the configuration) will return a pose of None in the result. This can be seen in the result below on the sawyer robot link 2 (pedestal) as we removed that link in this test configuration.

The code for this tutorial can be found in the file optima_toolbox/optima/examples/5_forward_kinematics.rs and can be run from the optima_toolbox/optima directory using the following command:

cargo run --example 5_forward_kinematics

The output of this example should be the following:

Robot 0 ---> 
Link 0 base_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.0, 0.0]], is_identity: true, rot_is_identity: true, translation_is_zeros: true }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
Link 1 shoulder_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.0, 0.089159]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.089159]]
Link 2 upper_arm_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.13585, 0.089159]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.13585, 0.089159]]
Link 3 forearm_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.016149999999999998, 0.514159]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.016149999999999998, 0.514159]]
Link 4 wrist_1_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.016149999999999998, 0.906409]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.016149999999999998, 0.906409]]
Link 5 wrist_2_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.10915, 0.906409]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.10915, 0.906409]]
Link 6 wrist_3_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.10915, 1.001059]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.10915, 1.001059]]
Link 7 ee_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.7071067805519557, 0.7071067818211392], translation: [[0.0, 0.19145, 1.001059]], is_identity: false, rot_is_identity: false, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 1.5707963250000003]]
   > Pose Translation: [[0.0, 0.19145, 1.001059]]
Link 8 base ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 1.0, -1.7948964430104646e-9], translation: [[0.0, 0.0, 0.0]], is_identity: false, rot_is_identity: false, translation_is_zeros: true }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, -3.14159265]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
Link 9 tool0 ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [-0.7071067805519557, 0.0, 0.0, 0.7071067818211392], translation: [[0.0, 0.19145, 1.001059]], is_identity: false, rot_is_identity: false, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[-1.5707963250000003, 0.0, 0.0]]
   > Pose Translation: [[0.0, 0.19145, 1.001059]]
Link 10 world ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.0, 0.0]], is_identity: true, rot_is_identity: true, translation_is_zeros: true }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
////////////////////////////////////////////////////////////////////////////////////
Robot 0 ---> 
Link 0 base_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
Link 1 shoulder_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.089159, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.089159]]
Link 2 upper_arm_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.13585, 0.089159, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.13585, 0.089159]]
Link 3 forearm_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.016149999999999998, 0.514159, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.016149999999999998, 0.514159]]
Link 4 wrist_1_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.016149999999999998, 0.906409, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.016149999999999998, 0.906409]]
Link 5 wrist_2_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.10915, 0.906409, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.10915, 0.906409]]
Link 6 wrist_3_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.10915, 1.001059, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.10915, 1.001059]]
Link 7 ee_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.7948965149208059e-9, 1.0, 0.0, 0.0], [-1.0, 1.7948965149208059e-9, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.19145, 1.001059, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 1.5707963250000003]]
   > Pose Translation: [[0.0, 0.19145, 1.001059]]
Link 8 base ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[-1.0, -3.5897930298416118e-9, 0.0, 0.0], [3.5897930298416118e-9, -1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, -3.14159265]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
Link 9 tool0 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.7948965149208059e-9, -1.0, 0.0], [0.0, 1.0, 1.7948965149208059e-9, 0.0], [0.0, 0.19145, 1.001059, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-1.5707963250000003, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.19145, 1.001059]]
Link 10 world ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
Link 11 base_of_chain_link_with_child_link_10 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
Robot 1 ---> 
Link 0 base ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.0]]
Link 1 torso ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.0]]
Link 2 pedestal ---> 
   > Pose: None
Link 3 controller_box ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.0]]
Link 4 pedestal_feet ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.0]]
Link 5 right_arm_base_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.0]]
Link 6 right_l0 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.08, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.08]]
Link 7 head ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.3765, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.3765]]
Link 8 right_torso_itb ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [-1.0, 0.0, 4.8965888601467475e-12, 0.0], [-0.055, 1.0, 0.3, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -1.5707963118937354, 0.0]]
   > Pose Translation: [[-0.055, 1.0, 0.3]]
Link 9 right_l1 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [-1.0, 4.8965888601467475e-12, -4.8965888601467475e-12, 0.0], [4.8965888601467475e-12, 1.0, 2.3976582465313225e-23, 0.0], [0.081, 1.05, 0.317, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-1.570789747253445, 1.5707963057214724, -4.509437977892592e-5]]
   > Pose Translation: [[0.081, 1.05, 0.317]]
Link 10 right_l2 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 4.8965888601467475e-12, 0.0], [0.2210000000006978, 1.1924999999993144, 0.3170000000006855, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, 1.5707963118937354, 0.0]]
   > Pose Translation: [[0.2210000000006978, 1.1924999999993144, 0.3170000000006855]]
Link 11 right_l3 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [-1.0, 4.8965888601467475e-12, -4.8965888601467475e-12, 0.0], [4.8965888601467475e-12, 1.0, 2.3976582465313225e-23, 0.0], [0.4810000000006978, 1.1504999999993144, 0.3170000000019586, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-1.570789747253445, 1.5707963057214724, -4.509437977892592e-5]]
   > Pose Translation: [[0.4810000000006978, 1.1504999999993144, 0.3170000000019586]]
Link 12 right_l4 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 4.8965888601467475e-12, 0.0], [0.6060000000000784, 1.0239999999987024, 0.31700000000257067, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, 1.5707963118937354, 0.0]]
   > Pose Translation: [[0.6060000000000784, 1.0239999999987024, 0.31700000000257067]]
Link 13 right_arm_itb ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.6809999999998091, 1.0239999999987024, 0.3720000000029379, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.6809999999998091, 1.0239999999987024, 0.3720000000029379]]
Link 14 right_l5 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [-1.0, 4.8965888601467475e-12, -4.8965888601467475e-12, 0.0], [4.8965888601467475e-12, 1.0, 2.3976582465313225e-23, 0.0], [0.8810000000000784, 1.0549999999987023, 0.31700000000391726, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-1.570789747253445, 1.5707963057214724, -4.509437977892592e-5]]
   > Pose Translation: [[0.8810000000000784, 1.0549999999987023, 0.31700000000391726]]
Link 15 right_hand_camera ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[-4.8965888601227706e-12, -1.0, -4.8965888601707244e-12, 0.0], [-1.0, 4.8965888601467475e-12, -4.8965888601467475e-12, 0.0], [4.8965888601707244e-12, 4.8965888601467475e-12, -1.0, 0.0], [0.9140000000006123, 1.1244999999985408, 0.2774480000040789, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-3.1415926535848966, 4.896679426577553e-12, -1.5707963267997933]]
   > Pose Translation: [[0.9140000000006123, 1.1244999999985408, 0.2774480000040789]]
Link 16 right_wrist ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 4.8965888601467475e-12, 0.0], [0.8810000000005945, 1.1604099999987023, 0.31700000000391726, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, 1.5707963118937354, 0.0]]
   > Pose Translation: [[0.8810000000005945, 1.1604099999987023, 0.31700000000391726]]
Link 17 right_l6 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[7.234801487997977e-6, 0.1736452969071067, 0.9848082609369704, 0.0], [1.2756688056520986e-6, -0.9848082609635456, 0.1736452969024209, 0.0], [0.9999999999730153, -7.438770000002659e-14, -7.3464061603243535e-6, 0.0], [0.991000000000594, 1.1602999999981636, 0.3170000000044559, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[1.5708386337636842, -1.3962663266418527, 1.5707546625424669]]
   > Pose Translation: [[0.991000000000594, 1.1602999999981636, 0.3170000000044559]]
Link 18 right_hand ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.2756711699482342e-6, -0.9848082042170961, 0.17364561873272535, 0.0], [-7.234801071115536e-6, -0.1736456187374112, -0.984808204190521, 0.0], [0.9999999999730153, -7.438770000002659e-14, -7.3464061603243535e-6, 0.0], [1.0154999999999328, 1.1602999999981618, 0.31699982001750493, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-1.5708037865277962, -0.17453032679013836, -1.5707950314450365]]
   > Pose Translation: [[1.0154999999999328, 1.1602999999981618, 0.31699982001750493]]
Link 19 right_l1_2 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [-1.0, 4.8965888601467475e-12, -4.8965888601467475e-12, 0.0], [4.8965888601467475e-12, 1.0, 2.3976582465313225e-23, 0.0], [0.081, 1.05, 0.317, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-1.570789747253445, 1.5707963057214724, -4.509437977892592e-5]]
   > Pose Translation: [[0.081, 1.05, 0.317]]
Link 20 right_l2_2 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 4.8965888601467475e-12, 0.0], [0.2210000000006978, 1.1924999999993144, 0.3170000000006855, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, 1.5707963118937354, 0.0]]
   > Pose Translation: [[0.2210000000006978, 1.1924999999993144, 0.3170000000006855]]
Link 21 right_l4_2 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 4.8965888601467475e-12, 0.0], [0.6060000000000784, 1.0239999999987024, 0.31700000000257067, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, 1.5707963118937354, 0.0]]
   > Pose Translation: [[0.6060000000000784, 1.0239999999987024, 0.31700000000257067]]
Link 22 screen ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 1.0, 0.0, 0.0], [-4.8965888601467475e-12, 2.3976582465313225e-23, 1.0, 0.0], [1.0, -4.8965888601467475e-12, 4.8965888601467475e-12, 0.0], [0.03, 1.0, 0.4815, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[1.5707963267899998, -4.387820765954416e-16, 1.5707963267899998]]
   > Pose Translation: [[0.03, 1.0, 0.4815]]
Link 23 head_camera ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, -1.0, 0.0, 0.0], [-0.529917833208038, -2.594789758879581e-12, -0.8480489903585158, 0.0], [0.8480489903585158, 4.152547239048205e-12, -0.529917833208038, 0.0], [0.0228027, 1.0, 0.593072, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-2.1293, 1.4578619949671078e-17, -1.5707963267899998]]
   > Pose Translation: [[0.0228027, 1.0, 0.593072]]
////////////////////////////////////////////////////////////////////////////////////
HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.2756711699482342e-6, -0.9848082042170961, 0.17364561873272535, 0.0], [-7.234801071115536e-6, -0.1736456187374112, -0.984808204190521, 0.0], [0.9999999999730153, -7.438770000002659e-14, -7.3464061603243535e-6, 0.0], [1.0154999999999328, 1.1602999999981618, 0.31699982001750493, 1.0]] }, pose_type: HomogeneousMatrix }