Joint States

In this tutorial, we will cover how to spawn and interpret robot joint states.

"Robot states" are vectors that contain scalar joint values for each joint axis in the robot model. These objects are sometimes referred to as robot configurations or robot poses in the robotics literature, but in this library, we will stick to the convention of referring to them as robot states.

A RobotSetJointState object contains the vector of joint angles in the field joint_state, as well as a state type (either DOF or Full). A DOF joint state only contains values for joint values that are free (not fixed), while the Full joint state includes joint values for all present joint axes (even if they are fixed). A dof joint state is important for operations such as optimization where only the free values are decision variables, while a full joint state is important for operations such as forward kinematics where all present joint axes need to somehow contribute to the model. The library will ensure that mathematical operations (additions, scalar multiplication, etc) can only occur over robot states of the same type. Conversions between DOF and Full states can be done via the RobotSetJointStateModule.

The code for this example is as follows:

extern crate optima;

use nalgebra::DVector;
use optima::robot_modules::robot_configuration_module::RobotConfigurationModule;
use optima::robot_set_modules::robot_set::RobotSet;
use optima::utils::utils_robot::robot_module_utils::RobotNames;

fn main() {
    // Let's start this demonstration by initializing a base configuration of the ur5 robot
    // and fixing its 0th joint (should_pan_joint, in the case of the ur5 robot) to -1.0.
    let mut robot_configuration = RobotConfigurationModule::new_from_names(RobotNames::new_base("ur5")).expect("error");
    robot_configuration.set_fixed_joint(0, 0, -1.0).expect("error");
    let robot_set = RobotSet::new_from_robot_configuration_modules(vec![robot_configuration]);

    // Use the robot_set_joint_state_module to spawn a `RobotSetJointState`.  In this case, the
    // resulting `RobotSetJointState` will be of type DOF because the model only has five
    // degrees of freedom (one is fixed), and five values are provided.
    let dof_joint_state = robot_set.robot_set_joint_state_module().spawn_robot_set_joint_state_try_auto_type(DVector::from_vec(vec![0., 1., 2., 3., 4.])).expect("error");
    println!("DOF joint state 1: {:?}", dof_joint_state);

    // This converts the DOF `RobotSetJointState` to a Full `RobotSetJointState`, i.e., it places the
    // fixed value of -1.0 in the right place within the full state.
    let full_joint_state = robot_set.robot_set_joint_state_module().convert_state_to_full_state(&dof_joint_state).expect("error");
    println!("Full joint state 1: {:?}", full_joint_state);

    // We can use the robot_set_joint_state_module to print a summary of the joint state.
    robot_set.robot_set_joint_state_module().print_robot_joint_state_summary(&full_joint_state);

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

    // Let's now try a different configuration, where we set a dead end link at link index 4.
    // This will remove four links (wrist_2_link, wrist_3_link, ee_link, and tool0) from the model,
    // and will also remove connecting joints between these links as well.
    let mut robot_configuration = RobotConfigurationModule::new_from_names(RobotNames::new_base("ur5")).expect("error");
    robot_configuration.set_dead_end_link(5).expect("error");
    let robot_set = RobotSet::new_from_robot_configuration_modules(vec![robot_configuration]);

    // Use the robot_set_joint_state_module to spawn a `RobotSetJointState`.  In this case, the
    // resulting `RobotSetJointState` will automatically be of type Full because the model only has five
    // degrees of freedom, and five values are provided.
    let full_joint_state = robot_set.robot_set_joint_state_module().spawn_robot_set_joint_state_try_auto_type(DVector::from_vec(vec![0., 1., 2., 3., 4.])).expect("error");
    println!("Full joint state 2: {:?}", full_joint_state);

    let dof_joint_state = robot_set.robot_set_joint_state_module().convert_state_to_dof_state(&dof_joint_state).expect("error");
    println!("DOF joint state 2: {:?}", dof_joint_state);

    // Again, we can use the robot_set_joint_state_module to print a summary of the joint state.
    robot_set.robot_set_joint_state_module().print_robot_joint_state_summary(&dof_joint_state);

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

    // Lastly, let's show how joint states work for robot sets with multiple robots.  For simplicity,
    // we will use a ur5 base model (6DOF) and a sawyer base model (8DOF, including the screen).
    let robot_set = RobotSet::new_from_robot_names(vec![RobotNames::new_base("ur5"), RobotNames::new_base("sawyer")]);

    // The joint state here will have 14 degrees of freedom (6 from ur5 and 8 from sawyer) instantiated
    // using a concatenated vector of 14 values.
    // Because no joints are fixed or removed, the DOF and Full joint state types here will be
    // identical (by default, the underlying type will be Full in these situations).
    let joint_state = robot_set.robot_set_joint_state_module().spawn_robot_set_joint_state_try_auto_type(DVector::from_vec(vec![0.0; 14])).expect("error");
    println!("joint state 3: {:?}", joint_state);

    // For a last time, we can use the robot_set_joint_state_module to print a summary of the joint state.
    robot_set.robot_set_joint_state_module().print_robot_joint_state_summary(&joint_state);
}

Explanations for this code are in the comments line by line.

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

cargo run --example 4_joint_states

The output of this example should be the following:

DOF joint state 1: RobotSetJointState { robot_set_joint_state_type: DOF, concatenated_state: VecStorage { data: [0.0, 1.0, 2.0, 3.0, 4.0], nrows: Dynamic { value: 5 }, ncols: Const } }
Full joint state 1: RobotSetJointState { robot_set_joint_state_type: Full, concatenated_state: VecStorage { data: [-1.0, 0.0, 1.0, 2.0, 3.0, 4.0], nrows: Dynamic { value: 6 }, ncols: Const } }
Joint state index 0 ---> 
   > Robot Index in Robot Set: 0
   > joint name: shoulder_pan_joint
   > joint index: 0
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: -1
Joint state index 1 ---> 
   > Robot Index in Robot Set: 0
   > joint name: shoulder_lift_joint
   > joint index: 1
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 1.0, 0.0]]
   > joint value: 0
Joint state index 2 ---> 
   > Robot Index in Robot Set: 0
   > joint name: elbow_joint
   > joint index: 2
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 1.0, 0.0]]
   > joint value: 1
Joint state index 3 ---> 
   > Robot Index in Robot Set: 0
   > joint name: wrist_1_joint
   > joint index: 3
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 1.0, 0.0]]
   > joint value: 2
Joint state index 4 ---> 
   > Robot Index in Robot Set: 0
   > joint name: wrist_2_joint
   > joint index: 4
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 3
Joint state index 5 ---> 
   > Robot Index in Robot Set: 0
   > joint name: wrist_3_joint
   > joint index: 5
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 1.0, 0.0]]
   > joint value: 4

Full joint state 2: RobotSetJointState { robot_set_joint_state_type: Full, concatenated_state: VecStorage { data: [0.0, 1.0, 2.0, 3.0, 4.0], nrows: Dynamic { value: 5 }, ncols: Const } }
DOF joint state 2: RobotSetJointState { robot_set_joint_state_type: DOF, concatenated_state: VecStorage { data: [0.0, 1.0, 2.0, 3.0, 4.0], nrows: Dynamic { value: 5 }, ncols: Const } }
Joint state index 0 ---> 
   > Robot Index in Robot Set: 0
   > joint name: shoulder_pan_joint
   > joint index: 0
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 0
Joint state index 1 ---> 
   > Robot Index in Robot Set: 0
   > joint name: shoulder_lift_joint
   > joint index: 1
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 1.0, 0.0]]
   > joint value: 1
Joint state index 2 ---> 
   > Robot Index in Robot Set: 0
   > joint name: elbow_joint
   > joint index: 2
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 1.0, 0.0]]
   > joint value: 2
Joint state index 3 ---> 
   > Robot Index in Robot Set: 0
   > joint name: wrist_1_joint
   > joint index: 3
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 1.0, 0.0]]
   > joint value: 3
Joint state index 4 ---> 
   > Robot Index in Robot Set: 0
   > joint name: wrist_2_joint
   > joint index: 4
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 4

joint state 3: RobotSetJointState { robot_set_joint_state_type: Full, concatenated_state: VecStorage { data: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], nrows: Dynamic { value: 14 }, ncols: Const } }
Joint state index 0 ---> 
   > Robot Index in Robot Set: 0
   > joint name: shoulder_pan_joint
   > joint index: 0
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 0
Joint state index 1 ---> 
   > Robot Index in Robot Set: 0
   > joint name: shoulder_lift_joint
   > joint index: 1
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 1.0, 0.0]]
   > joint value: 0
Joint state index 2 ---> 
   > Robot Index in Robot Set: 0
   > joint name: elbow_joint
   > joint index: 2
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 1.0, 0.0]]
   > joint value: 0
Joint state index 3 ---> 
   > Robot Index in Robot Set: 0
   > joint name: wrist_1_joint
   > joint index: 3
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 1.0, 0.0]]
   > joint value: 0
Joint state index 4 ---> 
   > Robot Index in Robot Set: 0
   > joint name: wrist_2_joint
   > joint index: 4
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 0
Joint state index 5 ---> 
   > Robot Index in Robot Set: 0
   > joint name: wrist_3_joint
   > joint index: 5
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 1.0, 0.0]]
   > joint value: 0
Joint state index 6 ---> 
   > Robot Index in Robot Set: 1
   > joint name: right_j0
   > joint index: 5
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 0
Joint state index 7 ---> 
   > Robot Index in Robot Set: 1
   > joint name: head_pan
   > joint index: 6
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 0
Joint state index 8 ---> 
   > Robot Index in Robot Set: 1
   > joint name: right_j1
   > joint index: 8
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 0
Joint state index 9 ---> 
   > Robot Index in Robot Set: 1
   > joint name: right_j2
   > joint index: 9
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 0
Joint state index 10 ---> 
   > Robot Index in Robot Set: 1
   > joint name: right_j3
   > joint index: 10
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 0
Joint state index 11 ---> 
   > Robot Index in Robot Set: 1
   > joint name: right_j4
   > joint index: 11
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 0
Joint state index 12 ---> 
   > Robot Index in Robot Set: 1
   > joint name: right_j5
   > joint index: 13
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 0
Joint state index 13 ---> 
   > Robot Index in Robot Set: 1
   > joint name: right_j6
   > joint index: 16
   > joint sub dof index: 0
   > joint sub dof axis type: Rotation
   > axis: [[0.0, 0.0, 1.0]]
   > joint value: 0