Robot Test
We will now further verify that the Optima Toolbox is properly setup with the Rust toolchain.
In the optima_toolbox/optima directory, run the following command:
cargo run --bin print_robot_info ur5
If all is working, the console should contain configuration and diagnostics information for the given robot model (ur5 in this case):
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
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) }
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) }
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) }
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) }
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) }
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) }
Feel free to run the command above with different robot models as an argument. Any robot in the optima_toolbox/optima_assets/optima_robots directory will work. For instance, the following command:
cargo run --bin print_robot_info yumi
will result in the following output:
Link index: 0 Link name: yumi_base_link Present: true
Link index: 1 Link name: yumi_body Present: true
Link index: 2 Link name: yumi_link_1_r Present: true
Link index: 3 Link name: yumi_link_2_r Present: true
Link index: 4 Link name: yumi_link_3_r Present: true
Link index: 5 Link name: yumi_link_4_r Present: true
Link index: 6 Link name: yumi_link_5_r Present: true
Link index: 7 Link name: yumi_link_6_r Present: true
Link index: 8 Link name: yumi_link_7_r Present: true
Link index: 9 Link name: yumi_link_1_l Present: true
Link index: 10 Link name: yumi_link_2_l Present: true
Link index: 11 Link name: yumi_link_3_l Present: true
Link index: 12 Link name: yumi_link_4_l Present: true
Link index: 13 Link name: yumi_link_5_l Present: true
Link index: 14 Link name: yumi_link_6_l Present: true
Link index: 15 Link name: yumi_link_7_l Present: true
Link index: 16 Link name: gripper_r_base Present: true
Link index: 17 Link name: gripper_r_finger_r Present: true
Link index: 18 Link name: gripper_r_finger_l Present: true
Link index: 19 Link name: gripper_l_base Present: true
Link index: 20 Link name: gripper_l_finger_r Present: true
Link index: 21 Link name: gripper_l_finger_l Present: true
Link index: 22 Link name: world Present: true
>> Joint index: 0 Joint name: yumi_base_link_to_body Num dofs: 0 Present: true
>> Joint index: 1 Joint name: yumi_joint_1_r Num dofs: 1 Present: true
-- Joint sub idx 0: Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index: 2 Joint name: yumi_joint_2_r Num dofs: 1 Present: true
-- Joint sub idx 0: Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index: 3 Joint name: yumi_joint_7_r Num dofs: 1 Present: true
-- Joint sub idx 0: Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index: 4 Joint name: yumi_joint_3_r 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: yumi_joint_4_r 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: yumi_joint_5_r 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: yumi_joint_6_r Num dofs: 1 Present: true
-- Joint sub idx 0: Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index: 8 Joint name: yumi_joint_1_l 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: yumi_joint_2_l 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: yumi_joint_7_l 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: yumi_joint_3_l 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: yumi_joint_4_l Num dofs: 1 Present: true
-- Joint sub idx 0: Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index: 13 Joint name: yumi_joint_5_l 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: yumi_joint_6_l Num dofs: 1 Present: true
-- Joint sub idx 0: Rotation about axis [[0.0, 0.0, 1.0]], Not fixed.
>> Joint index: 15 Joint name: yumi_link_7_r_joint Num dofs: 0 Present: true
>> Joint index: 16 Joint name: gripper_r_joint Num dofs: 1 Present: true
-- Joint sub idx 0: Translation about axis [[-1.0, 0.0, 0.0]], Not fixed.
>> Joint index: 17 Joint name: gripper_r_joint_m Num dofs: 1 Present: true
-- Joint sub idx 0: Translation about axis [[-1.0, 0.0, 0.0]], Not fixed.
>> Joint index: 18 Joint name: yumi_link_7_l_joint Num dofs: 0 Present: true
>> Joint index: 19 Joint name: gripper_l_joint Num dofs: 1 Present: true
-- Joint sub idx 0: Translation about axis [[-1.0, 0.0, 0.0]], Not fixed.
>> Joint index: 20 Joint name: gripper_l_joint_m Num dofs: 1 Present: true
-- Joint sub idx 0: Translation about axis [[-1.0, 0.0, 0.0]], Not fixed.
>> Joint index: 21 Joint name: world_joint Num dofs: 0 Present: true
DOF Joint Axis 0 ---> JointAxis { joint_idx: 1, 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.94087978961, 2.94087978961) }
DOF Joint Axis 1 ---> JointAxis { joint_idx: 2, 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.50454747661, 0.759218224618) }
DOF Joint Axis 2 ---> JointAxis { joint_idx: 3, 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.94087978961, 2.94087978961) }
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: (-2.15548162621, 1.3962634016) }
DOF Joint Axis 4 ---> 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: (-5.06145483078, 5.06145483078) }
DOF Joint Axis 5 ---> 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: (-1.53588974176, 2.40855436775) }
DOF Joint Axis 6 ---> JointAxis { joint_idx: 7, 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.99680398707, 3.99680398707) }
DOF Joint Axis 7 ---> 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: (-2.94087978961, 2.94087978961) }
DOF Joint Axis 8 ---> 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: (-2.50454747661, 0.759218224618) }
DOF Joint Axis 9 ---> 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: (-2.94087978961, 2.94087978961) }
DOF Joint Axis 10 ---> 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.15548162621, 1.3962634016) }
DOF Joint Axis 11 ---> JointAxis { joint_idx: 12, 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.06145483078, 5.06145483078) }
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: (-1.53588974176, 2.40855436775) }
DOF Joint Axis 13 ---> JointAxis { joint_idx: 14, 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.99680398707, 3.99680398707) }
DOF Joint Axis 14 ---> JointAxis { joint_idx: 16, 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: (0.0, 0.025) }
DOF Joint Axis 15 ---> JointAxis { joint_idx: 17, 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: (0.0, 0.025) }
DOF Joint Axis 16 ---> JointAxis { joint_idx: 19, 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: (0.0, 0.025) }
DOF Joint Axis 17 ---> JointAxis { joint_idx: 20, 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: (0.0, 0.025) }
The outputs of these commands will make more sense as you move through the tutorials ahead, but for now, as long as the outputs in your console appear as they do here, the toolbox is correctly set up on your system!