Robot Sets

Thus far, the previous tutorials have been building up an understanding of the Robot struct and its underlying modules in Optima. This struct only contains a model and configuration for a single robot (ur5, yumi, sawyer, etc.). However, what if we want to handle cases where our model may contain multiple robots.

Optima handles this case by providing the RobotSet struct. A RobotSet can be thought of as a superset abstraction of the Robot struct. A RobotSet initialized with a single robot is indistiguishable from a Robot; however, it can also incorporate more robots into a single model when needed.

IMPORTANT: Because the RobotSet struct is a superset of Robot, almost all downstream applications and algorithms in Optima will depend on a RobotSet instead of a Robot. Thus, it is highly recommended to always use a RobotSet in code, even when only one robot is present in the model, in order to maximimze compatibility throughout the library.

Code that loads a RobotSet can be seen here:

extern crate optima;

use optima::robot_set_modules::robot_set::RobotSet;
use optima::utils::utils_robot::robot_module_utils::RobotNames;

fn main() {
    // Loads a robot set with two robots (a ur5 and sawyer).
    let robot_set = RobotSet::new_from_robot_names(vec![RobotNames::new("ur5", None), RobotNames::new("sawyer", None)]);

    // prints a summary of the robot set configuration
    robot_set.robot_set_configuration_module().print_summary();

    // prints a summary of the robot set's degrees of freedom.
    robot_set.robot_set_joint_state_module().print_dof_summary();
}

The robot set loaded in line 8 has two robots, a ur5 and a sawyer. Both of these robots are their "base" configurations (i.e., the second parameter in both RobotNames structs are None). However, either of these could have been named configurations previously saved for either robot.

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

cargo run --example 2_robot_sets

The output of this example should be the following:

Robot 0 ---> 
  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:  true 
  Link index:  6   Link name:  wrist_3_link   Present:  true 
  Link index:  7   Link name:  ee_link   Present:  true 
  Link index:  8   Link name:  base   Present:  true 
  Link index:  9   Link name:  tool0   Present:  true 
  Link index:  10   Link name:  world   Present:  true 

>> Joint index:  0   Joint name:  shoulder_pan_joint   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> 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:  true 
      -- 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:  true 
>> 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:  true 
>> Joint index:  9   Joint name:  world_joint   Num dofs:  0   Present:  true 


Robot 1 ---> 
  Link index:  0   Link name:  base   Present:  true 
  Link index:  1   Link name:  torso   Present:  true 
  Link index:  2   Link name:  pedestal   Present:  true 
  Link index:  3   Link name:  controller_box   Present:  true 
  Link index:  4   Link name:  pedestal_feet   Present:  true 
  Link index:  5   Link name:  right_arm_base_link   Present:  true 
  Link index:  6   Link name:  right_l0   Present:  true 
  Link index:  7   Link name:  head   Present:  true 
  Link index:  8   Link name:  right_torso_itb   Present:  true 
  Link index:  9   Link name:  right_l1   Present:  true 
  Link index:  10   Link name:  right_l2   Present:  true 
  Link index:  11   Link name:  right_l3   Present:  true 
  Link index:  12   Link name:  right_l4   Present:  true 
  Link index:  13   Link name:  right_arm_itb   Present:  true 
  Link index:  14   Link name:  right_l5   Present:  true 
  Link index:  15   Link name:  right_hand_camera   Present:  true 
  Link index:  16   Link name:  right_wrist   Present:  true 
  Link index:  17   Link name:  right_l6   Present:  true 
  Link index:  18   Link name:  right_hand   Present:  true 
  Link index:  19   Link name:  right_l1_2   Present:  true 
  Link index:  20   Link name:  right_l2_2   Present:  true 
  Link index:  21   Link name:  right_l4_2   Present:  true 
  Link index:  22   Link name:  screen   Present:  true 
  Link index:  23   Link name:  head_camera   Present:  true 

>> Joint index:  0   Joint name:  controller_box_fixed   Num dofs:  0   Present:  true 
>> Joint index:  1   Joint name:  pedestal_feet_fixed   Num dofs:  0   Present:  true 
>> Joint index:  2   Joint name:  pedestal_fixed   Num dofs:  0   Present:  true 
>> Joint index:  3   Joint name:  right_arm_mount   Num dofs:  0   Present:  true 
>> Joint index:  4   Joint name:  torso_t0   Num dofs:  0   Present:  true 
>> Joint index:  5   Joint name:  right_j0   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index:  6   Joint name:  head_pan   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index:  7   Joint name:  right_torso_itb   Num dofs:  0   Present:  true 
>> Joint index:  8   Joint name:  right_j1   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index:  9   Joint name:  right_j2   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index:  10   Joint name:  right_j3   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index:  11   Joint name:  right_j4   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index:  12   Joint name:  right_arm_itb   Num dofs:  0   Present:  true 
>> Joint index:  13   Joint name:  right_j5   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index:  14   Joint name:  right_hand_camera   Num dofs:  0   Present:  true 
>> Joint index:  15   Joint name:  right_wrist   Num dofs:  0   Present:  true 
>> Joint index:  16   Joint name:  right_j6   Num dofs:  1   Present:  true 
      -- Joint sub idx 0:  Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index:  17   Joint name:  right_hand   Num dofs:  0   Present:  true 
>> Joint index:  18   Joint name:  right_j1_2   Num dofs:  0   Present:  true 
>> Joint index:  19   Joint name:  right_j2_2   Num dofs:  0   Present:  true 
>> Joint index:  20   Joint name:  right_j4_2   Num dofs:  0   Present:  true 
>> Joint index:  21   Joint name:  display_joint   Num dofs:  0   Present:  true 
>> Joint index:  22   Joint name:  head_camera   Num dofs:  0   Present:  true 


Robot 0, DOF Joint Axis 0 ---> JointAxis { joint_idx: 0, 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) }
Robot 0, DOF Joint Axis 1 ---> 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) }
Robot 0, DOF Joint Axis 2 ---> 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) }
Robot 0, DOF Joint Axis 3 ---> 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) }
Robot 0, DOF Joint Axis 4 ---> 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) }
Robot 0, DOF Joint Axis 5 ---> JointAxis { joint_idx: 5, 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) }
------------------------------------------------------------------------
Robot 1, DOF Joint Axis 6 ---> JointAxis { joint_idx: 5, 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: (-3.0503, 3.0503) }
Robot 1, DOF Joint Axis 7 ---> JointAxis { joint_idx: 6, 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: (-5.0952, 0.9064) }
Robot 1, DOF Joint Axis 8 ---> JointAxis { joint_idx: 8, 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: (-3.8095, 2.2736) }
Robot 1, DOF Joint Axis 9 ---> JointAxis { joint_idx: 9, 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: (-3.0426, 3.0426) }
Robot 1, DOF Joint Axis 10 ---> JointAxis { joint_idx: 10, 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: (-3.0439, 3.0439) }
Robot 1, DOF Joint Axis 11 ---> JointAxis { joint_idx: 11, 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: (-2.9761, 2.9761) }
Robot 1, DOF Joint Axis 12 ---> JointAxis { joint_idx: 13, 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: (-2.9761, 2.9761) }
Robot 1, DOF Joint Axis 13 ---> JointAxis { joint_idx: 16, 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: (-4.7124, 4.7124) }
------------------------------------------------------------------------

Optima Toolbox

Robot Sets