Robot Set Configurations
In the previous tutorial, we overviewed how to instantiate a robot set with multiple robots. In this case, both robot models were simply their "base" configurations; however, it is possible to build up unique configurations for all robots in the set using what is called a RobotSetConfiguration. In this section, we will show how to achieve this effect.
Example code that instantiates a RobotSet using a RobotSetConfiguration can be seen here:
extern crate optima; use optima::robot_modules::robot_configuration_module::{ContiguousChainMobilityMode, RobotConfigurationModule}; use optima::robot_set_modules::robot_set::RobotSet; use optima::robot_set_modules::robot_set_configuration_module::RobotSetConfigurationModule; use optima::utils::utils_robot::robot_module_utils::RobotNames; use optima::utils::utils_se3::optima_se3_pose::{OptimaSE3Pose, OptimaSE3PoseType}; fn main() { // Initialize `RobotSetConfigurationModule` let mut robot_set_configuration_module = RobotSetConfigurationModule::new_empty(); // Load a base robot configuration module let mut ur5_configuration = RobotConfigurationModule::new_from_names(RobotNames::new_base("ur5")).expect("error"); // Set the ur5 to have a planar translation mobile base. ur5_configuration.set_mobile_base(ContiguousChainMobilityMode::PlanarTranslation { x_bounds: (-2.0, 2.0), y_bounds: (-2.0, 2.0) }).expect("error"); // Add the ur5 configuration to the robot_set_configuration robot_set_configuration_module.add_robot_configuration(ur5_configuration).expect("error"); let mut sawyer_configuration = RobotConfigurationModule::new_from_names(RobotNames::new_base("sawyer")).expect("error"); // Move the sawyer robot over 1 meter on the y axis so it is not overlapping with the ur5 robot. sawyer_configuration.set_base_offset(&OptimaSE3Pose::new_from_euler_angles(0.,0.,0., 0., 1., 0., &OptimaSE3PoseType::ImplicitDualQuaternion)).expect("error"); // Remove the pedestal from the sawyer model. sawyer_configuration.set_dead_end_link(2).expect("error"); // Add the sawyer configuration to the robot_set_configuration robot_set_configuration_module.add_robot_configuration(sawyer_configuration).expect("error"); // Similar to a `RobotConfigurationModule`, a `RobotSetConfigurationModule` can be saved to a file // to allow for easy loading of a model at a later time. This command will save a file // `optima_toolbox/optima_assets/optima_robot_sets/test_configuration.JSON` robot_set_configuration_module.save_robot_set_configuration_module("test_configuration").expect("error"); // We will now load the just saved configuration let loaded_robot_set_configuration_module = RobotSetConfigurationModule::new_from_set_name("test_configuration").expect("error"); // This loaded robot set configuration will now be used to initialize a `RobotSet`. let robot_set = RobotSet::new_from_robot_set_configuration_module(loaded_robot_set_configuration_module); // When we now print information about the robot set, the changes we made to the configurations // are reflected in the combined model. robot_set.print_summary(); robot_set.robot_set_configuration_module().print_summary(); robot_set.robot_set_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/3_robot_set_configurations.rs and can be run from the optima_toolbox/optima directory using the following command:
cargo run --example 3_robot_set_configurations
Note that, if this example is run, a directory called test_configuration will be saved in optima_toolbox/optima_assets/optima_robot_sets. Because this configuration will be used in future examples, please to leave this directory in place while you are going through tutorials. However, long-term, feel free to delete this directory.
The output of this example should be the following:
2 robots.
Robot 0 ---> "ur5"
Base Offset: EulerAnglesAndTranslation { euler_angles: [[0.0, -0.0, 0.0]], translation: [[0.0, 0.0, 0.0]], phantom_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: EulerAnglesAndTranslation }
Robot 1 ---> "sawyer"
Base Offset: EulerAnglesAndTranslation { euler_angles: [[0.0, -0.0, 0.0]], translation: [[0.0, 1.0, 0.0]], phantom_data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 1.0, 0.0]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: EulerAnglesAndTranslation }
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
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: 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
>> Joint index: 10 Joint name: base_of_chain_connector_joint_with_child_link_11 Num dofs: 2 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.
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: false
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 0, DOF Joint Axis 6 ---> 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) }
Robot 0, DOF Joint Axis 7 ---> 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) }
------------------------------------------------------------------------
Robot 1, DOF Joint Axis 8 ---> 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 9 ---> 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 10 ---> 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 11 ---> 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 12 ---> 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 13 ---> 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 14 ---> 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 15 ---> 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) }
----------------------------------------------------------------------