Introduction

Optima is an easy to set up and easy to use robotics toolbox. Its primary use-case is robot motion generation (e.g., trajectory optimization, motion planning, optimization-based inverse kinematics, etc), though its underlying structures are general and can apply to many planning and optimization problem domains. The core library is written in Rust, though high quality support for other targets such as Python and Webassembly are afforded via Rust's flexible compiler.

Optima features an extensive suite of efficient and flexible robotics subroutines, including forward kinematics, jacobian computations, etc. Optima also features implementations of state-of-the-art robotics algorithms related to planning, optimization, and proximity computations, such as RelaxedIK, CollisionIK, and Proxima (with more algorithms on the way). These robotics features are designed to be easy to set up and access. Models of many popular robots such as UR5, Franka Emika Panda, Kinova Gen3, etc., are included out-of-the-box and can be instantiated with a single line of code. If your robot model is not included in our provided assets, a robot can be added to the library in just minutes.

High-level Features

The underlying non-linear optimization engine has several backend options, such as the Open Engine and NLopt. All algorithms support a non-linear objective function, non-linear equality constraints, and non-linear inequality constraints. Most provided algorithms are local solvers, though several global optimization algorithms are provided via NLopt as well. These optimization algorithm options can be swapped in and out by changing a single parameter, making testing and benchmarking over optimization algorithms easy to conduct.
The library presents a powerful Rust trait called OptimaTensorFunction; any function that implements this trait can automatically compute derivatives (up to the fourth derivative) using a derivative composition graph that automatically selects either analytical derivatives, when provided, or finite-difference approximate derivatives, by default, through its traversal. Thus, instead of depending on low-level automatic differentiation libraries that often fail on important subroutines, this approach provides a highly efficient and flexible way to perform derivatives for any implemented function. High order derivatives are even computable on multi-dimensional functions as the library supports tensors of any arbitrary dimension. For instance, a function \( \mathit{f}: \mathbb{R}^{3 \times 5} \rightarrow {R}^{4 \times 4} \) implemented as an OptimaTensorFunction can automatically provide derivatives \( \frac{\partial f}{\partial \mathbf{X}} \in {R}^{4 \times 4 \times 3 \times 5} \), \( \frac{\partial^2 f}{\partial \mathbf{X}^2} \in {R}^{4 \times 4 \times 3 \times 5 \times 3 \times 5} \), etc.
Optima uses flexible transform and rotation implementations that make SE(3) computations a breeze. For instance, any computations over transforms can use homogeneous matrices, implicit dual quaternions, a rotation matrix + vector, unit quaternion + vector, euler angles + vector, etc. No matter what SE(3) representation is chosen, the library will automatically ensure that all conversions and computations are handled correctly under the hood.
In just a few lines of code, individual robot models can be combined to form a "robot set", allowing for easy planning and optimization over several robots all at once. For instance, a UR5 (6DOF) can be easily combined with two Rethink Sawyer (7DOF) robots to form a robot set with 20 total DOF. In addition, any robot in the library (including those in a robot set) can be easily supplemented with a mobile base of numerous types (e.g., floating base, planar base, etc.). The extra degrees of freedom accompanying these mobile base options are automatically added to the robot (or robot set) model.

Sub-repositories

The Optima Toolbox is comprised of four primary sub-repositories:

optima: This repository contains the core Rust code of the Optima Toolbox. This code can be compiled to Rust, Python, and Web-assembly.
optima_assets: This repository contains asset files that are pre-packaged with the toolbox. For instance, these assets include pre-processed robot models (in the optima_robots repository), as well as 3D mesh files that can be used to build up scenes around robot models.
optima_js: This repository will contain utility code when using Optima in Javascript. This folder is an empty place-holder right now; code will be added here over time.
optima_py: This repository will contain utility code when using Optima in Python. This folder is an empty place-holder right now; code will be added here over time.

Getting Started

In this Chapter, I will overview how to set up the Optima Toolbox.

Optima Toolbox Installation

The Optima Toolbox code lives on github. A standard way to install this code is to ensure that git is installed on your system, then clone the code by running the following command in your terminal after navigating to a desired directory:

git clone https://github.com/djrakita/optima_toolbox.git

The Optima Toolbox repository has several sub-repositories. In order to pull all updates from the sub-repositories, navigate into the optima_toolbox directory:

cd optima_toolbox

then run the following command in your terminal:

git submodule update --init --recursive

And that's it! The Optima Toolbox files are now on your computer. In the following section, I will outline how to install the Rust toolchain in order to further set up the Optima Toolbox.

Note that if the optima_bevy feature is on (it is by default), you will also need to setup dependencies for the bevy game engine on your system.

Rust Installation

We will next ensure that the Rust programming language toolchain is installed on your computer. Rust is required in order to set up and compile all of the features offered by Optima, even if you are mostly interested in using Optima in Python, Webassembly, etc.

Fortunately, the Rust toolchain is very easy to install. Simply follow the one-step installation guide here. To verify that Rust has been successfully installed, run the following command:

cargo --version

Also, to verify that the newly installed Rust compiler works with the Optima Toolbox, navigate to the optima sub-directory in the Optima Toolbox (assuming you are already in the optima_toolbox directory.):

cd optima

Then run:

cargo test

Numerous tests should run, all passing with a result of ok.

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!

Adding a Robot

The Optima Toolbox comes pre-packaged with models of several popular robots. These models can be found in the optima_toolbox/optima_assets/optima_robots directory. Each robot directory here (e.g., ur5, sawyer, etc.) contains all necessary information for using their respective robot models throughout the Optima Toolbox.

Guide on adding a robot is coming soon! If you need a new robot added to the Optima Toolbox before this guide is made available, please email me at daniel.rakita@yale.edu.

Python Setup

Coming Soon

WASM Setup

Coming Soon

Citing Optima

If you use any part of the the Optima Toolbox in your research, please cite the software as follows:

 @misc{rakita_2022, url={https://djrakita.github.io/optima_toolbox/}, 
 author={Rakita, Daniel}, 
 title={Optima: An Applied Planning and Optimization Toolbox}
 year={2022}}

Robots and Robot Sets

In this Chapter, I will cover brief tutorial topics on how to load and save robot models in Optima.

Loading a Robot Model

In this tutorial, we will cover how to load a robot model into Optima. The code for this example is as follows:

extern crate optima;

use optima::robot_modules::robot::Robot;
use optima::utils::utils_robot::robot_module_utils::RobotNames;

fn main () {
    // load the given robot
    let robot = Robot::new_from_names(RobotNames::new("ur5", None));

    // prints a summary of the robot configuration
    robot.robot_configuration_module().print_summary();

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

Let's take a look through the code. The important part of this code is line 8 where the ur5 robot model is loaded. The easiest way to load a robot is using the new_from_names function. This function takes a RobotNames parameter, which has two parameters of its own. The first parameter here is the name of the robot ("ur5" in this case). This string literal parameter could be any robot in the directory optima_toolbox/optima_assets/optima_robots. The second parameter is a Rust Option that can, as the name implies, serve as an optional string parameter specifying the name of a robot configuration. Robot configurations will be covered in a following section, so in this case, the configuration is specifies as None, meaning the loaded robot model will simply be its base configuration.

A Robot in Optima is a large struct that aggregates smaller structs that contain specialized code to perform particular tasks, referred to as modules. For instance, a Robot contains a RobotConfigurationModule, a RobotJointStateModule, a RobotKinematicsModule, and so on. For details on each module, refer to the Optima API documentation. Lines 11 and 14 here print a summary of the configuration module and joint state module, respectively, to the console.

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 0_robots

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:  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) }

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) }

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) }
------------------------------------------------------------------------

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) }
----------------------------------------------------------------------

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

Robot Kinematics

In this Chapter, I will cover how to perform standard kinematics subroutines, such as forward kinematics and jacobian computations, in Optima.

Forward Kinematics

In this tutorial, I will overview how to perform forward kinematics using a RobotSet.

Example code can be found here:

extern crate optima;

use nalgebra::DVector;
use optima::robot_set_modules::robot_set::RobotSet;
use optima::utils::utils_robot::robot_module_utils::RobotNames;
use optima::utils::utils_se3::optima_se3_pose::OptimaSE3PoseType;

fn main() {
    // Create a base configuration ur5.
    let robot_set = RobotSet::new_from_robot_names(vec![RobotNames::new_base("ur5")]);

    // Spawn joint state with zeros for all degrees of freedom.
    let joint_state = robot_set.spawn_robot_set_joint_state(DVector::from_vec(vec![0.0; 6])).expect("error");

    // Compute forward kinematics.  The second argument in `compute_fk` is the SE(3) representation that should
    // be used to compute FK.  All options are `ImplicitDualQuaternion`, `EulerAnglesAndTranslation`,
    // `RotationMatrixAndTranslation`, `UnitQuaternionAndTranslation`, and `HomogeneousMatrix`.
    // The result is an `RobotSetFKResult`.
    let fk_res = robot_set.robot_set_kinematics_module().compute_fk(&joint_state, &OptimaSE3PoseType::ImplicitDualQuaternion).expect("error");

    // Print summary of the fk result.
    fk_res.print_summary();

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

    // Load the test configuration created in the previous tutorial.
    // Make sure that you ran example 3_robot_set_configurations prior to starting this tutorial.
    let robot_set = RobotSet::new_from_set_name("test_configuration");

    // Spawn joint state with zeros for all degrees of freedom.
    let joint_state = robot_set.spawn_robot_set_joint_state(DVector::from_vec(vec![0.0; 16])).expect("error");

    // Compute forward kinematics.  The second argument in `compute_fk` is the SE(3) representation that should
    // be used to compute FK.  All options are `ImplicitDualQuaternion`, `EulerAnglesAndTranslation`,
    // `RotationMatrixAndTranslation`, `UnitQuaternionAndTranslation`, and `HomogeneousMatrix`.
    // The result is an `RobotSetFKResult`.
    let fk_res = robot_set.robot_set_kinematics_module().compute_fk(&joint_state, &OptimaSE3PoseType::HomogeneousMatrix).expect("error");

    // Print summary of the fk result.
    fk_res.print_summary();

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

    // Access the pose of the just computed fk result.
    // The arguments in `get_pose_from_idxs` here are `robot_idx_in_set` (in this case,
    // the sawyer robot, the "second" robot in the set) and `link_idx_in_robot` (in this case,
    // link 18 "right hand" on the sawyer robot).
    let pose = fk_res.get_pose_from_idxs(1, 18);
    println!("{:?}", pose);
}

Explanations are written in this code line by line.

Note that any link that is removed from the model (i.e., by setting a dead-end link in the configuration) will return a pose of None in the result. This can be seen in the result below on the sawyer robot link 2 (pedestal) as we removed that link in this test configuration.

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

cargo run --example 5_forward_kinematics

The output of this example should be the following:

Robot 0 ---> 
Link 0 base_link ---> 
   > Pose: Some(ImplicitDualQuaternion { 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: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
Link 1 shoulder_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.0, 0.089159]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.089159]]
Link 2 upper_arm_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.13585, 0.089159]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.13585, 0.089159]]
Link 3 forearm_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.016149999999999998, 0.514159]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.016149999999999998, 0.514159]]
Link 4 wrist_1_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.016149999999999998, 0.906409]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.016149999999999998, 0.906409]]
Link 5 wrist_2_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.10915, 0.906409]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.10915, 0.906409]]
Link 6 wrist_3_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.10915, 1.001059]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.10915, 1.001059]]
Link 7 ee_link ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.7071067805519557, 0.7071067818211392], translation: [[0.0, 0.19145, 1.001059]], is_identity: false, rot_is_identity: false, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 1.5707963250000003]]
   > Pose Translation: [[0.0, 0.19145, 1.001059]]
Link 8 base ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 1.0, -1.7948964430104646e-9], translation: [[0.0, 0.0, 0.0]], is_identity: false, rot_is_identity: false, translation_is_zeros: true }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, -3.14159265]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
Link 9 tool0 ---> 
   > Pose: Some(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [-0.7071067805519557, 0.0, 0.0, 0.7071067818211392], translation: [[0.0, 0.19145, 1.001059]], is_identity: false, rot_is_identity: false, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion })
   > Pose Euler Angles: [[-1.5707963250000003, 0.0, 0.0]]
   > Pose Translation: [[0.0, 0.19145, 1.001059]]
Link 10 world ---> 
   > Pose: Some(ImplicitDualQuaternion { 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: ImplicitDualQuaternion })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
////////////////////////////////////////////////////////////////////////////////////
Robot 0 ---> 
Link 0 base_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
Link 1 shoulder_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.089159, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.089159]]
Link 2 upper_arm_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.13585, 0.089159, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.13585, 0.089159]]
Link 3 forearm_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.016149999999999998, 0.514159, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.016149999999999998, 0.514159]]
Link 4 wrist_1_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.016149999999999998, 0.906409, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.016149999999999998, 0.906409]]
Link 5 wrist_2_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.10915, 0.906409, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.10915, 0.906409]]
Link 6 wrist_3_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.10915, 1.001059, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.10915, 1.001059]]
Link 7 ee_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.7948965149208059e-9, 1.0, 0.0, 0.0], [-1.0, 1.7948965149208059e-9, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.19145, 1.001059, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 1.5707963250000003]]
   > Pose Translation: [[0.0, 0.19145, 1.001059]]
Link 8 base ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[-1.0, -3.5897930298416118e-9, 0.0, 0.0], [3.5897930298416118e-9, -1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, -3.14159265]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
Link 9 tool0 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.7948965149208059e-9, -1.0, 0.0], [0.0, 1.0, 1.7948965149208059e-9, 0.0], [0.0, 0.19145, 1.001059, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-1.5707963250000003, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.19145, 1.001059]]
Link 10 world ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
Link 11 base_of_chain_link_with_child_link_10 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 0.0, 0.0]]
Robot 1 ---> 
Link 0 base ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.0]]
Link 1 torso ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.0]]
Link 2 pedestal ---> 
   > Pose: None
Link 3 controller_box ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.0]]
Link 4 pedestal_feet ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.0]]
Link 5 right_arm_base_link ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.0]]
Link 6 right_l0 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.08, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.08]]
Link 7 head ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.3765, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.0, 1.0, 0.3765]]
Link 8 right_torso_itb ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [-1.0, 0.0, 4.8965888601467475e-12, 0.0], [-0.055, 1.0, 0.3, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -1.5707963118937354, 0.0]]
   > Pose Translation: [[-0.055, 1.0, 0.3]]
Link 9 right_l1 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [-1.0, 4.8965888601467475e-12, -4.8965888601467475e-12, 0.0], [4.8965888601467475e-12, 1.0, 2.3976582465313225e-23, 0.0], [0.081, 1.05, 0.317, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-1.570789747253445, 1.5707963057214724, -4.509437977892592e-5]]
   > Pose Translation: [[0.081, 1.05, 0.317]]
Link 10 right_l2 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 4.8965888601467475e-12, 0.0], [0.2210000000006978, 1.1924999999993144, 0.3170000000006855, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, 1.5707963118937354, 0.0]]
   > Pose Translation: [[0.2210000000006978, 1.1924999999993144, 0.3170000000006855]]
Link 11 right_l3 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [-1.0, 4.8965888601467475e-12, -4.8965888601467475e-12, 0.0], [4.8965888601467475e-12, 1.0, 2.3976582465313225e-23, 0.0], [0.4810000000006978, 1.1504999999993144, 0.3170000000019586, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-1.570789747253445, 1.5707963057214724, -4.509437977892592e-5]]
   > Pose Translation: [[0.4810000000006978, 1.1504999999993144, 0.3170000000019586]]
Link 12 right_l4 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 4.8965888601467475e-12, 0.0], [0.6060000000000784, 1.0239999999987024, 0.31700000000257067, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, 1.5707963118937354, 0.0]]
   > Pose Translation: [[0.6060000000000784, 1.0239999999987024, 0.31700000000257067]]
Link 13 right_arm_itb ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.6809999999998091, 1.0239999999987024, 0.3720000000029379, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, -0.0, 0.0]]
   > Pose Translation: [[0.6809999999998091, 1.0239999999987024, 0.3720000000029379]]
Link 14 right_l5 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [-1.0, 4.8965888601467475e-12, -4.8965888601467475e-12, 0.0], [4.8965888601467475e-12, 1.0, 2.3976582465313225e-23, 0.0], [0.8810000000000784, 1.0549999999987023, 0.31700000000391726, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-1.570789747253445, 1.5707963057214724, -4.509437977892592e-5]]
   > Pose Translation: [[0.8810000000000784, 1.0549999999987023, 0.31700000000391726]]
Link 15 right_hand_camera ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[-4.8965888601227706e-12, -1.0, -4.8965888601707244e-12, 0.0], [-1.0, 4.8965888601467475e-12, -4.8965888601467475e-12, 0.0], [4.8965888601707244e-12, 4.8965888601467475e-12, -1.0, 0.0], [0.9140000000006123, 1.1244999999985408, 0.2774480000040789, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-3.1415926535848966, 4.896679426577553e-12, -1.5707963267997933]]
   > Pose Translation: [[0.9140000000006123, 1.1244999999985408, 0.2774480000040789]]
Link 16 right_wrist ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 4.8965888601467475e-12, 0.0], [0.8810000000005945, 1.1604099999987023, 0.31700000000391726, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, 1.5707963118937354, 0.0]]
   > Pose Translation: [[0.8810000000005945, 1.1604099999987023, 0.31700000000391726]]
Link 17 right_l6 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[7.234801487997977e-6, 0.1736452969071067, 0.9848082609369704, 0.0], [1.2756688056520986e-6, -0.9848082609635456, 0.1736452969024209, 0.0], [0.9999999999730153, -7.438770000002659e-14, -7.3464061603243535e-6, 0.0], [0.991000000000594, 1.1602999999981636, 0.3170000000044559, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[1.5708386337636842, -1.3962663266418527, 1.5707546625424669]]
   > Pose Translation: [[0.991000000000594, 1.1602999999981636, 0.3170000000044559]]
Link 18 right_hand ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.2756711699482342e-6, -0.9848082042170961, 0.17364561873272535, 0.0], [-7.234801071115536e-6, -0.1736456187374112, -0.984808204190521, 0.0], [0.9999999999730153, -7.438770000002659e-14, -7.3464061603243535e-6, 0.0], [1.0154999999999328, 1.1602999999981618, 0.31699982001750493, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-1.5708037865277962, -0.17453032679013836, -1.5707950314450365]]
   > Pose Translation: [[1.0154999999999328, 1.1602999999981618, 0.31699982001750493]]
Link 19 right_l1_2 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [-1.0, 4.8965888601467475e-12, -4.8965888601467475e-12, 0.0], [4.8965888601467475e-12, 1.0, 2.3976582465313225e-23, 0.0], [0.081, 1.05, 0.317, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-1.570789747253445, 1.5707963057214724, -4.509437977892592e-5]]
   > Pose Translation: [[0.081, 1.05, 0.317]]
Link 20 right_l2_2 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 4.8965888601467475e-12, 0.0], [0.2210000000006978, 1.1924999999993144, 0.3170000000006855, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, 1.5707963118937354, 0.0]]
   > Pose Translation: [[0.2210000000006978, 1.1924999999993144, 0.3170000000006855]]
Link 21 right_l4_2 ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 0.0, -1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 4.8965888601467475e-12, 0.0], [0.6060000000000784, 1.0239999999987024, 0.31700000000257067, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[0.0, 1.5707963118937354, 0.0]]
   > Pose Translation: [[0.6060000000000784, 1.0239999999987024, 0.31700000000257067]]
Link 22 screen ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, 1.0, 0.0, 0.0], [-4.8965888601467475e-12, 2.3976582465313225e-23, 1.0, 0.0], [1.0, -4.8965888601467475e-12, 4.8965888601467475e-12, 0.0], [0.03, 1.0, 0.4815, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[1.5707963267899998, -4.387820765954416e-16, 1.5707963267899998]]
   > Pose Translation: [[0.03, 1.0, 0.4815]]
Link 23 head_camera ---> 
   > Pose: Some(HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[4.8965888601467475e-12, -1.0, 0.0, 0.0], [-0.529917833208038, -2.594789758879581e-12, -0.8480489903585158, 0.0], [0.8480489903585158, 4.152547239048205e-12, -0.529917833208038, 0.0], [0.0228027, 1.0, 0.593072, 1.0]] }, pose_type: HomogeneousMatrix })
   > Pose Euler Angles: [[-2.1293, 1.4578619949671078e-17, -1.5707963267899998]]
   > Pose Translation: [[0.0228027, 1.0, 0.593072]]
////////////////////////////////////////////////////////////////////////////////////
HomogeneousMatrix { data: HomogeneousMatrix { matrix: [[1.2756711699482342e-6, -0.9848082042170961, 0.17364561873272535, 0.0], [-7.234801071115536e-6, -0.1736456187374112, -0.984808204190521, 0.0], [0.9999999999730153, -7.438770000002659e-14, -7.3464061603243535e-6, 0.0], [1.0154999999999328, 1.1602999999981618, 0.31699982001750493, 1.0]] }, pose_type: HomogeneousMatrix }

Robot Jacobians

In this tutorial, I will overview how to compute the jacobian matrix of a RobotSet.

Example code can be found here:

extern crate optima;

use nalgebra::DVector;
use optima::robot_modules::robot_kinematics_module::{JacobianEndPoint, JacobianMode};
use optima::robot_set_modules::robot_set::RobotSet;
use optima::utils::utils_robot::robot_module_utils::RobotNames;

fn main() {
    // Create a base configuration ur5.
    let robot_set = RobotSet::new_from_robot_names(vec![RobotNames::new_base("ur5")]);

    // Spawn joint state with zeros for all degrees of freedom.
    let joint_state = robot_set.spawn_robot_set_joint_state(DVector::from_vec(vec![0.0; 6])).expect("error");

    // Computes the full jacobian matrix with end link idx of 9.  The `robot_jacobian_end_point` in
    // this case is simply the link center.
    let full_jacobian_matrix = robot_set.robot_set_kinematics_module().compute_jacobian(&joint_state, 0, None, 9, &JacobianEndPoint::Link, None, JacobianMode::Full).expect("error");

    println!("{}", full_jacobian_matrix);

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

    // Computes the 3 x N (N = 6 for the ur5 robot) translation jacobian matrix with end link idx of 9.
    // The `robot_jacobian_end_point` in this case is simply the link center.
    let translational_jacobian_matrix = robot_set.robot_set_kinematics_module().compute_jacobian(&joint_state, 0, None, 9, &JacobianEndPoint::Link, None, JacobianMode::Translational).expect("error");

    println!("{}", translational_jacobian_matrix);

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

    // Load the test configuration created in the previous tutorial.
    // Make sure that you ran example 3_robot_set_configurations prior to starting this tutorial.
    let robot_set = RobotSet::new_from_set_name("test_configuration");

    // Spawn joint state with zeros for all degrees of freedom.
    let joint_state = robot_set.spawn_robot_set_joint_state(DVector::from_vec(vec![0.0; 16])).expect("error");

    // Computes the 6 x N (N = 16 for this configuration) full jacobian matrix with end link idx of 18 on robot 1
    // (sawyer, in this case of this `RobotSet`).
    // The `robot_jacobian_end_point` in this case is the inertial origin of the link.
    let full_jacobian_matrix = robot_set.robot_set_kinematics_module().compute_jacobian(&joint_state, 1, None, 18, &JacobianEndPoint::InertialOrigin, None, JacobianMode::Full).expect("error");

    println!("{}", full_jacobian_matrix);

    println!("////////////////////////////////////////////////////////////////////////////////////");
}

Jacobian matrices are outputted as nalgebra DMatrix objects. These objects are compatible and can multiply with DVector objects (that are often used throughout the rest of the library).

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

cargo run --example 6_jacobians

The output of this example should be the following:

  ┌                                                                                                                               ┐
  │             -0.19145   0.9118999999999999   0.4868999999999999   0.0946499999999999 -0.08230000000000001                    0 │
  │                    0                    0                    0                    0                    0                    0 │
  │                    0                    0                    0                    0                    0                    0 │
  │                    0                    0                    0                    0                    0                    0 │
  │                    0                    1                    1                    1                    0                    1 │
  │                    1                    0                    0                    0                    1                    0 │
  └                                                                                                                               ┘


////////////////////////////////////////////////////////////////////////////////////

  ┌                                                                                                                               ┐
  │             -0.19145   0.9118999999999999   0.4868999999999999   0.0946499999999999 -0.08230000000000001                    0 │
  │                    0                    0                    0                    0                    0                    0 │
  │                    0                    0                    0                    0                    0                    0 │
  └                                                                                                                               ┘


////////////////////////////////////////////////////////////////////////////////////

  ┌                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 ┐
  │                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              0                           -0.16029998841362336                                              0                     -0.00000018809419351637482                0.00000000000015770399553929073                     -0.00000018809615244339527                -0.0000000000006676236440362641                     -0.00000018809811155149317               -0.00000000000008510472191613595 │
  │                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              0                             1.0155000099998726                                              0              0.0000000000000006234230649834017                       0.0000001880987702344505              0.0000000000000006536766262225693                      0.00000018809877014091413              0.0000000000000002398410089369438                     0.000000008111625825649074 │
  │                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              0                             -0.934500009999332                           -0.03220001158569105                            -0.5345000099991265                            0.13629998841492075                           -0.13450000999927902                    -0.000000011584538162702774 │
  │                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              0                 0.0000000000048965276278067904                             0.9999999999999992                  0.000000000004896361094353097                             0.9999999999999986                  0.000000000004896361094353097                             0.9999999999730148 │
  │                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              0                             0.9999999999999996 -0.0000000000000000000000000003844217798755143                             0.9999999999999989             0.00000000000000007850462293399649                             0.9999999999999984               -0.00000000000007473188734508085 │
  │                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              0                                              1                                              0              0.0000000000000006661338147750939                 0.0000000000048976378508314156              0.0000000000000012212453270876722                  0.000000000004898192962343728              0.0000000000000017763568394002505                       -0.000007346406160069208 │
  └                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 ┘


////////////////////////////////////////////////////////////////////////////////////

Intersection, Contact, and Proximity Checks

In this Chapter, I will cover brief tutorial topics on how to perform collision and proximity checks in Optima.

Robot Geometric Shape Scenes

All tutorials up to this point have involved robot sets in empty environments. However, robots are commonly surrounded by other obstacles. Optima allows for the modeling of these obstacles as static or dynamic geometric shapes in a RobotGeometricShapeScene. This struct is another abstraction layer on top of the RobotSet struct that allows for easy intersection checks, proximity checks, contact checks, ray casting, etc.

Example code can be found here:

extern crate optima;

use nalgebra::Vector3;
use optima::robot_set_modules::robot_set::RobotSet;
use optima::scenes::robot_geometric_shape_scene::{EnvObjInfoBlock, EnvObjPoseConstraint, RobotGeometricShapeScene};
use optima::utils::utils_robot::robot_module_utils::RobotNames;
use optima::utils::utils_se3::optima_se3_pose::{OptimaSE3Pose, OptimaSE3PoseType};

fn main() {
    // Initialize a `RobotSet`.  In this case, it is just a base model ur5 robot.
    let robot_set = RobotSet::new_from_robot_names(vec![RobotNames::new_base("ur5")]);

    // We use the `RobotSet` to initialize a `RobotGeometricShapeScene`.  By default, the
    // environment is empty.
    let mut robot_geometric_shape_scene = RobotGeometricShapeScene::new(robot_set, None).expect("error");

    // This line adds a "sphere" object to the environment, scaled down to a size of 0.1.
    // Note that any mesh directory in `optima_toolbox/optima_assets/optima_scenes/mesh_files`
    // can be loaded in.
    let env_obj_idx = robot_geometric_shape_scene.add_environment_object(EnvObjInfoBlock {
        asset_name: "sphere".to_string(),
        scale: Vector3::new(0.1, 0.1, 0.1),
        ..Default::default()
    }, false).expect("error");

    // Adding the object to the scene above returns the index of the added environment object.
    // Indices are assigned in order, so the first returned index will be 0.
    assert_eq!(env_obj_idx, 0);

    // This line can update the rotation and translation of the given environment object in the scene.
    // In this case, we are moving the sphere such that its position will be at (0.5,0.5,0).
    // Note that we can also set the transform of an object such that it is relative to another
    // shape in the environment.
    robot_geometric_shape_scene.set_curr_single_env_obj_pose_constraint(env_obj_idx, EnvObjPoseConstraint::Absolute(OptimaSE3Pose::new_from_euler_angles(0., 0., 0., 0.5, 0.5, 0., &OptimaSE3PoseType::ImplicitDualQuaternion))).expect("error");

    // This prints a summary of the whole scene.
    robot_geometric_shape_scene.print_summary();
}

Explanations are written in this code line by line. I will show how to use this robot geometric shape scene for various queries in the following tutorials, we are just focused on initializing the scene here.

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

cargo run --example 7_robot_geometric_shape_scene

The output of this example should be the following:

1 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 }

1 objects.
 Object 0 ---> 
    Object Info: EnvObjInfoBlock { asset_name: "sphere", scale: [[0.1, 0.1, 0.1]], shape_representation: BestFitConvexShape, decomposition_resolution: Medium, pose_constraint: Absolute(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.5, 0.5, 0.0]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion }) }

Adding New Environment Mesh

Coming Soon

Intersection Checking

Coming Soon

Contact Checking

Coming Soon

Bounding Volume Hierarchies

Coming Soon

Standard Proximity Checking

Coming Soon

Standard Proximity Checking

Coming Soon

Inverse Kinematics

In this Chapter, I will cover brief tutorial topics on how to perform inverse kinematics in Optima.

The default IK solver in Optima is an nonlinear optimization-based solver called OptimaIK. It is built from the ground up in Rust is feature rich. The folowing sections will overview some options and features.

Static IK

The OptimaIK solver can be used to solve for "static" IK solutions. By static here, I mean that the user is looking to compute a single, indpenedent robot pose that matches some objectives (e.g., have the robot's end-effector exhibit X position and Y orientation). The static solver is focused more on the exactness of a single solution rather than the smoothness and continutity of a sequence of solutions. This static solver can optionally take as input an initial guess and, in this case, the output solution may be somewhat close to the initial guess if such a valid solution exists; however, it is more focused on precision and accuracy of result rather than keeping the solution close to the intial condition. Thus, we recommend this version of the solver be used only if indepenent, single solutions are desired; if you would like a high quality sequence of solutions from OptimaIK (e.g., to create a motion trajectory), please refer to the following "motion" IK section.

Note that the static IK version of OptimaIK is not optimized for speed or efficiency! While it may be fast on some occasions (e.g., if allowable error bounds are large, if the initial condition is close to a sufficient solution, etc.), its main focus is on accuracy and precision of the result. In fact, under the hood, the solver may be trying hundreds of individual optimizations from random initial conditions in order to find a sufficient solution. Thus, echoing the same message mentioned above: only use this static IK option if you are focused on getting independent, high quality solutions from the IK solver and do not use it for getting a sequence of fast solutions.

Example code can be found here:

extern crate optima;

use optima::inverse_kinematics::OptimaIK;
use optima::optima_tensor_function::robotics_functions::RobotCollisionProximityGenericParams;
use optima::optimization::{NonlinearOptimizerType, OptimizerParameters};
use optima::robot_modules::robot_geometric_shape_module::RobotLinkShapeRepresentation;
use optima::robot_set_modules::robot_set::RobotSet;
use optima::scenes::robot_geometric_shape_scene::RobotGeometricShapeScene;
use optima::utils::utils_console::OptimaDebug;
use optima::utils::utils_robot::robot_module_utils::RobotNames;
use optima::utils::utils_robot::robot_set_link_specification::{RobotLinkTFAllowableError, RobotLinkTFGoal, RobotLinkTFSpec, RobotLinkTFSpecAndAllowableError, RobotLinkTFSpecAndAllowableErrorCollection};
use optima::utils::utils_se3::optima_se3_pose::{OptimaSE3Pose, OptimaSE3PoseType};

fn main() {
    // Initialize `RobotSet` with base ur5 configuration.
    let robot_set = RobotSet::new_from_robot_names(vec![RobotNames::new_base("ur5")]);

    // Initialize a `RobotGeometricShapeScene`.
    let robot_geometric_shape_scene = RobotGeometricShapeScene::new(robot_set, RobotLinkShapeRepresentation::ConvexShapes, vec![]).expect("error");

    // Initialize "static" IK solver.
    // The `NonlinearOptimizerType` enum many options.  Feel free to check them all out; here we
    // will just use the OpEn (Optimization Engine) option.
    // This solver can optionally exhibit a collision avoidance objective.  This is controlled
    // by the `RobotCollisionProximityGenericParams` enum.  Here, it is None (i.e., no collision
    // avoidance objective will be present).
    let mut optima_ik = OptimaIK::new_static_ik(robot_geometric_shape_scene, NonlinearOptimizerType::OpEn, RobotCollisionProximityGenericParams::None);

    // Initialize a `RobotLinkTFSpecAndAllowableErrorCollection`.  This will be used to specify
    // which links should be at what location or orientation, as well as what errors will be
    // tolerable, in our final solution.
    let mut robot_link_tf_spec_and_allowable_error_collection = RobotLinkTFSpecAndAllowableErrorCollection::new_empty();

    // Creates a `RobotLinkTFSpecAndAllowableError` object.  This specifies that the ur5's link 9
    // (its end effector link) should exhibit an identity orientation (euler angles 0,0,0) and
    // a location of x: 0.5, y: 0.1, z: 0.4.  also, the translation errors along all axes whould
    // be within 1 millimenter, while rotation errors along all axes should be within 0.01 radians.
    let robot_link_tf_spec_and_allowable_error = RobotLinkTFSpecAndAllowableError::new(RobotLinkTFSpec::Absolute {
        goal: RobotLinkTFGoal::LinkSE3PoseGoal {
            robot_idx_in_set: 0,
            link_idx_in_robot: 9,
            goal: OptimaSE3Pose::new_from_euler_angles(0., 0., 0., 0.5, 0.1, 0.4, &OptimaSE3PoseType::ImplicitDualQuaternion),
            weight: None
        }
    }, Some(RobotLinkTFAllowableError {
        rx: 0.01,
        ry: 0.01,
        rz: 0.01,
        x: 0.001,
        y: 0.001,
        z: 0.001
    }));

    // This adds the robot_link_tf_spec_and_allowable_error to the collection.  Note that the collection
    // can accept numerous entries and the optimization will try its best to match all of them,
    // if possible.
    robot_link_tf_spec_and_allowable_error_collection.add(robot_link_tf_spec_and_allowable_error);

    // This calls the static ik solver with the `RobotLinkTFSpecAndAllowableErrorCollection` as
    // first input.  This function can optionally take an initial condition (in this case it is `None`).
    // Also, I set a maximum number of tries of 100 here; if an acceptable solution within the error
    // bounds cannot be found in this number of tries, the solver will return None.
    // However, if a valid solution is found, it will be returned as Some(RobotSetJointState).
    // We also specified here that the solver should reject any solution that is in a collision;
    // If a solution is found in a collision, it will throw away the result and try again (up to the
    // maximum number of tries).
    let res = optima_ik.solve_static_ik(robot_link_tf_spec_and_allowable_error_collection, &OptimizerParameters::default(), None, Some(100), true, OptimaDebug::False);

    // This prints the result.  Because, by default, the static ik solver in `OptimaIK` uses
    // random initial conditions, the result will be different each time this script is
    // run.
    println!("{:?}", res);
}

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

cargo run --example _static_ik

The output will be different each time the script is run as the static ik solver uses random initial conditions by default.

Motion IK

In contrast to the "static" IK solver explained in the previous section, the "motion" IK version of OptimaIK is intended to compute a sequence of smooth, temporally coherent motions that each try to achieve some objective as best as possible.

The motion IK version of OptimaIK is heavily influenced by our previously proposed solvers RelaxedIK and CollisionIK. If you use motion IK in your research, please cite the following papers:

@inproceedings{rakita2018relaxedik,
  title={RelaxedIK: Real-time Synthesis of Accurate and Feasible Robot Arm Motion.},
  author={Rakita, Daniel and Mutlu, Bilge and Gleicher, Michael},
  booktitle={Robotics: Science and Systems},
  pages={26--30},
  year={2018},
  organization={Pittsburgh, PA}
}

@inproceedings{rakita2021collisionik,
  title={Collisionik: A per-instant pose optimization method for generating robot motions with environment collision avoidance},
  author={Rakita, Daniel and Shi, Haochen and Mutlu, Bilge and Gleicher, Michael},
  booktitle={2021 IEEE International Conference on Robotics and Automation (ICRA)},
  pages={9995--10001},
  year={2021},
  organization={IEEE}
}

Also, if you use the Proxima collision avoidance options in OptimaIK, please cite the following paper:

@inproceedings{rakita2022proxima,
  title={PROXIMA: An Approach for Time or Accuracy Budgeted Collision Proximity Queries},
  author={Rakita, Daniel and Mutlu, Bilge and Gleicher, Michael},
  booktitle={Proceedings of Robotics: Science and Systems (RSS)},
  year={2022}
}

Example code can be found here:

extern crate optima;

use nalgebra::DVector;
use optima::inverse_kinematics::OptimaIK;
use optima::optima_tensor_function::robotics_functions::{RobotCollisionProximityGenericParams, RobotCollisionProximityGradientFDMode};
use optima::optimization::{NonlinearOptimizerType, OptimizerParameters};
use optima::robot_modules::robot_geometric_shape_module::RobotLinkShapeRepresentation;
use optima::robot_set_modules::robot_set::RobotSet;
use optima::scenes::robot_geometric_shape_scene::RobotGeometricShapeScene;
use optima::utils::utils_console::OptimaDebug;
use optima::utils::utils_robot::robot_module_utils::RobotNames;
use optima::utils::utils_robot::robot_set_link_specification::{RobotLinkTFGoal, RobotLinkTFSpec, RobotLinkTFSpecCollection};
use optima::utils::utils_se3::optima_se3_pose::{OptimaSE3Pose, OptimaSE3PoseType};

fn main() {
    // Initialize `RobotSet` with base ur5 configuration.
    let robot_set = RobotSet::new_from_robot_names(vec![RobotNames::new_base("ur5")]);

    // Initialize a `RobotGeometricShapeScene`.
    let robot_geometric_shape_scene = RobotGeometricShapeScene::new(robot_set, RobotLinkShapeRepresentation::ConvexShapes, vec![]).expect("error");

    // Spawn the robot joint state that the robot's motion will start at.
    let init_state = robot_geometric_shape_scene.robot_set().spawn_robot_set_joint_state(DVector::from_vec(vec![0.1; 6])).expect("error");

    // Initialize the "motion" version of `OptimaIK`.
    // This is using the OpEn Nonlinear optimizer (same as previous example).
    // This solver can optionally exhibit a collision avoidance objective.  This is controlled
    // by the `RobotCollisionProximityGenericParams` enum.  Here, it is None (i.e., no collision
    // avoidance objective will be present).
    // We also provide the initial joint state and initial time.  In most cases, the initial time
    // will be 0.0.
    let mut optima_ik = OptimaIK::new_motion_ik(robot_geometric_shape_scene, NonlinearOptimizerType::OpEn, RobotCollisionProximityGenericParams::None, &init_state, 0.0);

    // This specifies a robot link transform that the robot should try to achieve.  Note that, in this
    // example, we only provide this single goal, and the solver will exhibit a sequence of
    // state/ time tuples that will, over time, meet this goal.  Alternatively, we could also
    // provide new transform goal for links at each call of `solve_motion_ik` below.  This would
    // be useful for something like shared control or telemanipulation where an operator is
    // providing a new transform goal at each update.
    let mut robot_link_tf_spec_collection = RobotLinkTFSpecCollection::new();
    robot_link_tf_spec_collection.add(RobotLinkTFSpec::Absolute {
        goal: RobotLinkTFGoal::LinkSE3PoseGoal {
            robot_idx_in_set: 0,
            link_idx_in_robot: 9,
            goal: OptimaSE3Pose::new_from_euler_angles(0., 0., 0., 0.5, 0.1, 0.4, &OptimaSE3PoseType::ImplicitDualQuaternion),
            weight: None
        }
    });

    // Optimizer parameters.  We'll just use the default in this case.
    let mut params = OptimizerParameters::default();
    params.set_open_tolerance(0.00001);

    for i in 1..10000 {
        // Solves the motion ik problem at each given time.  The time here advances 1 millisecond
        // on each loop.  Thus, all 5000 loops will result in 5 seconds of outputted motion.
        // The output of each `solve_motion_ik` call will be a `TimedGenericRobotJointState` that
        // will contain a `RobotSetJointState` as well as a time value.
        let res = optima_ik.solve_motion_ik(robot_link_tf_spec_collection.clone(), i as f64 * 0.001, &params, OptimaDebug::False);
        println!("{:?}", res);
    }
}

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

cargo run --example _motion_ik

Optimization

In this Chapter, I will cover brief tutorial topics on how to perform optimization in Optima.

Optima Tensors

Coming Soon

Optima Tensor Functions

Coming Soon

Optima Tensor Function Derivatives

Coming Soon

Nonlinear Optimization

Coming Soon