Whole-Body Trajectory Optimization for Robot Multimodal Locomotion

Giuseppe L'Erario, Gabriele Nava, Giulio Romualdi, Fabio Bergonti, Valentino Razza, Stefano Dafarra, Daniele Pucci

Installation | Paper | Video

Abstract

The general problem of planning feasible trajectories for multimodal robots is still an open challenge. This paper presents a whole-body trajectory optimisation approach that addresses this challenge by combining methods and tools developed for aerial and legged robots. First, robot models that enable the presented whole-body trajectory optimisation framework are presented. The key model is the so-called robot centroidal momentum, the dynamics of which is directly related to the models of the robot actuation for aerial and terrestrial locomotion. Then, the paper presents how these models can be employed in an optimal control problem to generate either terrestrial or aerial locomotion trajectories with a unified approach. The optimisation problem considers robot kinematics, momentum, thrust forces and their bounds. The overall approach is validated using the multimodal robot iRonCub, a flying humanoid robot that expresses a degree of terrestrial and aerial locomotion. To solve the associated optimal trajectory generation problem, we employ ADAM, a custom-made open-source library that implements a collection of algorithms for calculating rigid-body dynamics using CasADi.

Results

Take-off

The robot starts at equilibrium and the jet thrust is set to zero. We set the initial CoM position at 0.57 m and the final one at 0.7 m. The net-force transitions smoothly from contact forces to jet thrusts. The horizon is 70 knots long.

Weights

Cost Term	$\ddot{r}$	$\dot{r}$	$\dot{h_{\omega}}$	$h_{\omega}$	$\dot{s}$	$s - \bar{s}$	$\dot{f}$	$f$	$\dot{T}$	$T$	$U$
Weight	1e5	1e7	1e4	1e4	1e5	1e6	1e0	5e1	0	0	1e-1

take-off-new.mp4

Ipop output

EXIT: Solved To Acceptable Level.
      solver  :   t_proc      (avg)   t_wall      (avg)    n_eval
       nlp_f  | 829.92ms ( 88.19us) 830.65ms ( 88.26us)      9411
       nlp_g  |  93.37 s (  9.92ms)  93.37 s (  9.92ms)      9411
  nlp_grad_f  | 512.66ms (172.27us) 512.85ms (172.33us)      2976
   nlp_jac_g  | 196.84 s ( 66.14ms) 196.85 s ( 66.14ms)      2976
       total  | 689.38 s (689.38 s) 689.52 s (689.52 s)         1

Take-off and landing

We test our approach as a vertical take-off and landing that shall be generated as a single instance of the optimisation problem. For this reason, we need to increase the number of knots of the optimisation horizon to 200. The initial and final CoM position is set to 0.57 m while at the intermediate knot, we request a CoM height higher than 0.7 m. The robot decreases the contact forces and increases the thrust. The trajectory of the CoM reaches the maximum height and smoothly comes back to the initial position.

Weights

Cost Term	$\ddot{r}$	$\dot{r}$	$\dot{h_{\omega}}$	$h_{\omega}$	$\dot{s}$	$s - \bar{s}$	$\dot{f}$	$f$	$\dot{T}$	$T$	$U$
Weight	1e5	1e7	1e4	1e4	1e5	1e6	1e0	5e1	0	0	1e-1

take-off-and-landing.mp4

Ipopt output

EXIT: Solved To Acceptable Level.
      solver  :   t_proc      (avg)   t_wall      (avg)    n_eval
       nlp_f  |   1.55 s (245.59us)   1.55 s (245.71us)      6319
       nlp_g  | 179.99 s ( 28.48ms) 179.99 s ( 28.48ms)      6319
  nlp_grad_f  |   1.23 s (489.44us)   1.23 s (489.61us)      2521
   nlp_jac_g  | 479.74 s (190.30ms) 479.75 s (190.30ms)      2521
       total  |   1.49ks (  1.49ks)   1.49ks (  1.49ks)         1

Walking to flight transition

We test our approach in a context in which two different locomotion patterns are mixed. The robot starts at an initial CoM position at 0.57 m and transitions from walking to flying locomotion going 0.9 m forward and reaching 0.7 m. The number of knots of the optimisation horizon is equal to 100. The robot moves a few steps forward while increasing the thrust. Eventually, the contacts break and the robot detaches from the ground. Note that the contact sequence in the legged locomotion phase is not predefined and emerges directly from the formulation.

Weights

Cost Term	$\ddot{r}$	$\dot{r}$	$\dot{h_{\omega}}$	$h_{\omega}$	$\dot{s}$	$s - \bar{s}$	$\dot{f}$	$f$	$\dot{T}$	$T$	$U$
Weight	1e5	1e7	1e4	1e4	1e5	1e7	0	1e0	0	2e3	0

walk-to-fly-transition.mp4

Ipopt output

EXIT: Solved To Acceptable Level.
      solver  :   t_proc      (avg)   t_wall      (avg)    n_eval
       nlp_f  | 377.97ms (104.76us) 378.35ms (104.86us)      3608
       nlp_g  |  49.13 s ( 13.62ms)  49.13 s ( 13.62ms)      3608
  nlp_grad_f  | 513.64ms (208.54us) 513.91ms (208.65us)      2463
   nlp_jac_g  | 224.63 s ( 91.20ms) 224.65 s ( 91.21ms)      2463
       total  |   1.65ks (  1.65ks)   1.65ks (  1.65ks)         1

Jumping

To validate the approach for generating multimodal locomotion, the fourth scenario consists of the robot taking a jump. The number of optimisation knots is set to 60. The initial and the final CoM position height equal 0.57 m. We add an additional constraint on the CoM height at the middle-knot $x[N/2] > 0.75m$. The CoM height decreases and then accelerates vertically until the contacts break. After the flight phase, the robot lands and returns to the initial configuration.

Weights

Cost Term	$\ddot{r}$	$\dot{r}$	$\dot{h_{\omega}}$	$h_{\omega}$	$\dot{s}$	$s - \bar{s}$	$\dot{f}$	$f$
Weight	1e0	1e0	1e0	1e0	1e4	2e1	1e-1	1e0

jump.mp4

Ipopt output

EXIT: Solved To Acceptable Level.
      solver  :   t_proc      (avg)   t_wall      (avg)    n_eval
       nlp_f  | 276.99ms ( 78.74us) 277.31ms ( 78.83us)      3518
       nlp_g  |  29.52 s (  8.39ms)  29.52 s (  8.39ms)      3518
  nlp_grad_f  | 156.72ms (153.34us) 156.79ms (153.42us)      1022
   nlp_jac_g  |  58.66 s ( 57.40ms)  58.66 s ( 57.40ms)      1022
       total  | 187.89 s (187.89 s) 187.97 s (187.97 s)         1

Code

The iRonCub model and its meshes are stored in the iRonCub software repository.

You need to install git LFS and enable it. Then, clone the iRonCub software repository:

git clone https://github.com/ami-iit/ironcub_software.git --branch v1.0

in your ~/.bashrc add the following lines, in order to load the robot model and the meshes:

export YARP_ROBOT_NAME=iRonCub-Mk1_1_v1
export IRONCUB_SOFTWARE_SOURCE_DIR=<path-to-ironcub-software>
export YARP_DATA_DIRS=${YARP_DATA_DIRS}:${IRONCUB_SOFTWARE_SOURCE_DIR}/models/iRonCub-Mk1_1/iRonCub/
export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:${IRONCUB_SOFTWARE_SOURCE_DIR}/models/
export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:${IRONCUB_SOFTWARE_SOURCE_DIR}/models/iRonCub-Mk1_1/iRonCub/robots

Clone this repository:

git clone git@github.com:ami-iit/paper_lerario_2022_humanoids_planning-multimodal-locomotion.git

Create an environment, and install the dependencies and the package:

cd paper_lerario_2022_humanoids_planning-multimodal-locomotion
conda config --add channels conda-forge # if you don't have the conda-forge channel already
mamba env create -f environment.yml
pip install .

Run from the root directory of the repository

show_trajectory --help

It will show the commands needed to visualize the trajectories and plots, e.g.:

show_trajectory transition

It will show the visualizations of the trajectories and plots, e.g.:

⚠️ Warning
Note that to replicate the results, you need to install the hsl solvers (here we use ma97) that can be downloaded but not redistributed. Please check here. If you want to speed up the simulation you may install IPOPT 3.13.4 with CoinBrew + HSL solver as explained here!

To run the optimizations, run the scripts in the runs folder, from the root, e.g.:

python runs/problem_take_off.py

Citing this work

If you find the work useful, please consider citing:

@INPROCEEDINGS{10000241,
  author={L'Erario, Giuseppe and Nava, Gabriele and Romualdi, Giulio and Bergonti, Fabio and Razza, Valentino and Dafarra, Stefano and Pucci, Daniele},
  booktitle={2022 IEEE-RAS 21st International Conference on Humanoid Robots (Humanoids)},
  title={Whole-Body Trajectory Optimization for Robot Multimodal Locomotion},
  year={2022},
  volume={},
  number={},
  pages={651-658},
  doi={10.1109/Humanoids53995.2022.10000241}}

Maintainer

This repository is maintained by:

	@Giulero