Robot Configurations

In this tutorial, we will cover how to create a new robot configuration, how to use this robot configuration module to instantiate a robot, and how to save this configuration for later use.

A RobotConfigurationModule affords extra specificity and functionality over a robot model. For example, a robot configuration can include a mobile base, a base offset, removed links, as well as fixed joint values that will not be considered degrees of freedom. Let's examine some code that creates, modifies, saves, and loads a robot configuration:

extern crate optima;

use optima::robot_modules::robot::Robot;
use optima::robot_modules::robot_configuration_module::{ContiguousChainMobilityMode, RobotConfigurationModule};
use optima::utils::utils_robot::robot_module_utils::RobotNames;

fn main() {
    // Initialize a new robot configuration for the ur5 robot.
    let mut robot_configuration = RobotConfigurationModule::new_from_names(RobotNames::new_base("ur5")).expect("error");

    // Sets the 0 joint idx (should_pan_joint, in the case of the ur5 robot) with joint_sub_idx 0 (the
    // default sub-idx for joints with only 1 degree of freedom) to a fixed value of 1.2.  From here on
    // out, this joint will no longer be considered a degree of freedom and, instead,
    // will remain locked at 1.2.
    robot_configuration.set_fixed_joint(0, 0, 1.2);

    // Sets link with idx 5 (wrist_2_link, in the case of the ur5 robot) as a "dead end link".  A
    // dead-end link means that this link, as well as all predecessor links in the kinematic chain,
    // will be removed from the model.  Note that this will also remove all joints that connect
    // these removed links, thus possibly reducing the number of degrees of freedom of the model.
    // Links and joints that are removed in this way are said to be not "present" in the model.
    robot_configuration.set_dead_end_link(5);

    // Sets the base of the robot to be mobile.  In this case, the base is floating (not
    // realistic for the ur5 robot, but is very useful, for example, for the hips
    // of humanoid robots).  This base will automatically add 6 degrees of freedom
    // to the model, 3 for translation (x, y, z) and 3 for rotation (rx, ry, rz).
    // All lower and upper bounds are set for each of these degrees of freedom.
    // All mobility modes are PlanarTranslation (x and y translation), PlanarRotation (rz rotation),
    // PlanarTranslationAndRotation (x and y translation + z rotation),
    // Static (i.e., 0 DOFs), and Floating (x, y, z translation + rx, ry, rz rotation).
    robot_configuration.set_mobile_base(ContiguousChainMobilityMode::Floating {
        x_bounds: (-2.0, 2.0),
        y_bounds: (-2.0, 2.0),
        z_bounds: (-2.0, 2.0),
        xr_bounds: (-3.14, 3.14),
        yr_bounds: (-3.14, 3.14),
        zr_bounds: (-3.14, 3.14)
    });

    // Saves the robot configuration for later use.  This will add a file to
    // `optima_toolbox/optima_assets/optima_robots/ur5/configurations/test_configuration.JSON`.
    // NOTE: if you run this example multiple times and the test_configuration file is already
    // present, a prompt will show up in the console asking if you would like to save over
    // the already saved file.
    robot_configuration.save("test_configuration");

    // For illustrative purposes, let's now load in our robot configuration from the file that
    // was just saved.  The second parameter in the `RobotNames` struct is now an option of
    // `Some` with the name of our recently saved configuration.
    let loaded_robot_configuration = RobotConfigurationModule::new_from_names(RobotNames::new("ur5", Some("test_configuration"))).expect("error");

    // This configuration can now be used to instantiate a `Robot`.
    let robot = Robot::new_from_robot_configuration_module(loaded_robot_configuration);

    // when we print information about the robot, we see that the information that we
    // inputted above is reflected in the printed output.
    robot.robot_configuration_module().print_summary();
    robot.robot_joint_state_module().print_dof_summary();
}

Explanations for the code above are written as comments line by line.

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

cargo run --example 1_robot_configurations

Note that, if this example is run, a file called test_configuration.JSON will be saved in optima_toolbox/optima_assets/optima_robots/ur5/configurations. Because this configuration is only for educational purposes and is mostly nonsensical, I recommend deleting this file before moving on.

The output of this example should be the following:

  Link index:  0   Link name:  base_link   Present:  true 
  Link index:  1   Link name:  shoulder_link   Present:  true 
  Link index:  2   Link name:  upper_arm_link   Present:  true 
  Link index:  3   Link name:  forearm_link   Present:  true 
  Link index:  4   Link name:  wrist_1_link   Present:  true 
  Link index:  5   Link name:  wrist_2_link   Present:  false 
  Link index:  6   Link name:  wrist_3_link   Present:  false 
  Link index:  7   Link name:  ee_link   Present:  false 
  Link index:  8   Link name:  base   Present:  true 
  Link index:  9   Link name:  tool0   Present:  false 
  Link index:  10   Link name:  world   Present:  true 
  Link index:  11   Link name:  base_of_chain_link_with_child_link_10   Present:  true 

>> Joint index:  0   Joint name:  shoulder_pan_joint   Num dofs:  0   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 0.0, 1.0]], Fixed at value 1.2
>> Joint index:  1   Joint name:  shoulder_lift_joint   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 1.0, 0.0]], Not fixed.
>> Joint index:  2   Joint name:  elbow_joint   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 1.0, 0.0]], Not fixed.
>> Joint index:  3   Joint name:  wrist_1_joint   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 1.0, 0.0]], Not fixed.
>> Joint index:  4   Joint name:  wrist_2_joint   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index:  5   Joint name:  wrist_3_joint   Num dofs:  1   Present:  false 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 1.0, 0.0]], Not fixed.
>> Joint index:  6   Joint name:  ee_fixed_joint   Num dofs:  0   Present:  false 
>> Joint index:  7   Joint name:  base_link-base_fixed_joint   Num dofs:  0   Present:  true 
>> Joint index:  8   Joint name:  wrist_3_link-tool0_fixed_joint   Num dofs:  0   Present:  false 
>> Joint index:  9   Joint name:  world_joint   Num dofs:  0   Present:  true 
>> Joint index:  10   Joint name:  base_of_chain_connector_joint_with_child_link_11   Num dofs:  6   Present:  true 
      -- Joint sub idx 0:  Translation about axis [[1.0, 0.0, 0.0]], Not fixed.
      -- Joint sub idx 1:  Translation about axis [[0.0, 1.0, 0.0]], Not fixed.
      -- Joint sub idx 2:  Translation about axis [[0.0, 0.0, 1.0]], Not fixed.
      -- Joint sub idx 3:  Rotation about axis [[1.0, 0.0, 0.0]], Not fixed.
      -- Joint sub idx 4:  Rotation about axis [[0.0, 1.0, 0.0]], Not fixed.
      -- Joint sub idx 5:  Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.

DOF Joint Axis 0 ---> JointAxis { joint_idx: 1, joint_sub_dof_idx: 0, fixed_value: None, axis_as_unit: [[0.0, 1.0, 0.0]], axis: [[0.0, 1.0, 0.0]], axis_primitive_type: Rotation, bounds: (-6.2831853, 6.2831853) }
DOF Joint Axis 1 ---> JointAxis { joint_idx: 2, joint_sub_dof_idx: 0, fixed_value: None, axis_as_unit: [[0.0, 1.0, 0.0]], axis: [[0.0, 1.0, 0.0]], axis_primitive_type: Rotation, bounds: (-6.2831853, 6.2831853) }
DOF Joint Axis 2 ---> JointAxis { joint_idx: 3, joint_sub_dof_idx: 0, fixed_value: None, axis_as_unit: [[0.0, 1.0, 0.0]], axis: [[0.0, 1.0, 0.0]], axis_primitive_type: Rotation, bounds: (-6.2831853, 6.2831853) }
DOF Joint Axis 3 ---> JointAxis { joint_idx: 4, joint_sub_dof_idx: 0, fixed_value: None, axis_as_unit: [[0.0, 0.0, 1.0]], axis: [[0.0, 0.0, 1.0]], axis_primitive_type: Rotation, bounds: (-6.2831853, 6.2831853) }
DOF Joint Axis 4 ---> JointAxis { joint_idx: 10, joint_sub_dof_idx: 0, fixed_value: None, axis_as_unit: [[1.0, 0.0, 0.0]], axis: [[1.0, 0.0, 0.0]], axis_primitive_type: Translation, bounds: (-2.0, 2.0) }
DOF Joint Axis 5 ---> JointAxis { joint_idx: 10, joint_sub_dof_idx: 1, fixed_value: None, axis_as_unit: [[0.0, 1.0, 0.0]], axis: [[0.0, 1.0, 0.0]], axis_primitive_type: Translation, bounds: (-2.0, 2.0) }
DOF Joint Axis 6 ---> JointAxis { joint_idx: 10, joint_sub_dof_idx: 2, fixed_value: None, axis_as_unit: [[0.0, 0.0, 1.0]], axis: [[0.0, 0.0, 1.0]], axis_primitive_type: Translation, bounds: (-2.0, 2.0) }
DOF Joint Axis 7 ---> JointAxis { joint_idx: 10, joint_sub_dof_idx: 3, fixed_value: None, axis_as_unit: [[1.0, 0.0, 0.0]], axis: [[1.0, 0.0, 0.0]], axis_primitive_type: Rotation, bounds: (-3.14, 3.14) }
DOF Joint Axis 8 ---> JointAxis { joint_idx: 10, joint_sub_dof_idx: 4, fixed_value: None, axis_as_unit: [[0.0, 1.0, 0.0]], axis: [[0.0, 1.0, 0.0]], axis_primitive_type: Rotation, bounds: (-3.14, 3.14) }
DOF Joint Axis 9 ---> JointAxis { joint_idx: 10, joint_sub_dof_idx: 5, fixed_value: None, axis_as_unit: [[0.0, 0.0, 1.0]], axis: [[0.0, 0.0, 1.0]], axis_primitive_type: Rotation, bounds: (-3.14, 3.14) }