RAPTOR: RAPid and Robust Trajectory Optimization for Robots

Introduction

Dynamic locomotion for humanoid robots presents significant analytical and computational challenges due to the extensive number of linkages and degrees of freedom. This complexity results in a vast search space for feasible gaits which translates into a time-consuming process when optimizing over trajectories. In addition, the process often involves numerous hyperparameters and requires a good initial guess or a warm-start strategy, further complicating the development process. Existing methods struggle to integrate the latest hardware designs, such as actuated ankles with closed-loop mechanisms, which offer increased stability, but introduce additional constraints into the dynamics that can be challenging to represent in a computationally tractable fashion. This work introduces a generalized gait optimization framework that directly generates smooth and physically feasible trajectories. The proposed method demonstrates faster and more robust convergence than existing techniques and explicitly incorporates closed-loop constraints. The method is implemented as an open-source C++ codebase that significantly reduces computation times, facilitating dynamic locomotion for full-size humanoids. Note that RAPTOR also works for other fully actuated systems such as robotic manipulators.

To be more specific, we parameterize the trajectories for the robot states as, for example, polynomials. The decision variable of the optimization is then the coefficients of the polynomial. We sample a certain number of discrete points on the trajectory and evaluate the constraints, such as joints limits, torque limits, or collision avoidance. We use Ipopt as our optimization solver. For some of the constraints, we implement the analytical hessian so that Ipopt can converge faster.

Demos

Requirements

• Ubuntu >= 22.04
• Eigen 3.4
• GSL: solve close-loop kinematics
• Pinocchio >= 3.0: compute inverse dynamics and its gradient
• Ipopt: for nonlinear optimization

A more detailed instruction is provided here. We recommend users to install the requirements through docker.

Overview

• Trajectories/ : This folder contains implementation of multiple primitives of smooth trajectories.
• KinematicsDynamics/ : This folder contains implementation to compute forward kinematics and inverse dynamics of a robot.
• Constraints/ : This folder contains implementation of multiple constriants that could be useful for trajectory optimization, such as torque limits or collision avoidance.
• Costs/ : This folder contains implementation of multiple costs that could be useful for trajectory optimization, such as minimizing the total torque/power consumption or minimizing the path length.
• Optimization/ : This folder contains a base class that provides interfaces to ipopt.
• Examples/ : This folder contains several examples of trajectory optimization problem implementations, including gait optimization examples for multiple walking robots like Digit-v3, Talos, Unitree-G1, and a lot of examples related to collision avoidance or system identification for Kinova-gen3.

A more detailed instruction on how to code your own optimization problem is provided here.

Getting Started

Run the following command to compile all the code.

mkdir build
cd build
cmake ..
make -j4

You can find a README file with more details in each example within the Example/ folder. We provide the following examples:

• Kinova-gen3

  • CollisionAvoidanceInverseKinematics/: Solves an inverse kinematics problem for a given desired end-effector transformation matrix while considering joint limits and collision avoidance.
  • CollisionAvoidanceTrajectory/: The robot arm, without any payload, plans a trajectory to reach a target configuration while avoiding obstacles and satisfying torque limits.
  • Armour: A better re-implementation that integrates our previous work ARMOUR and WAITR, which is a provably-safe receding-horizon trajectory optimization framework.
  • SystemIdentification/: Contains multiple examples related to system identification of a robotic arm.

• Digit

  • Single-step periodic gait optimization.
  • Multi-step periodic gait optimization.
  • Detailed explanations on incorporating closed-loop linkages as kinematic constraints in optimization.

• Talos

  • Single-step periodic gait optimization.
  • Single-step gait optimization starting from a fixed initial configuration.
  • Multi-step periodic gait optimization.
  • Serves as a template for migrating the optimization framework to other humanoid robots.
  • Follow the procedure in the README of the folder to optimize gaits for your own humanoid robot.

• Unitree-G1

  • Single-step periodic gait optimization
  • Multi-step periodic gait optimization.
  • A simple example migrated from code in Talos.

Bibtex

To cite RAPTOR in your academic research, please use the following bibtex entry:

@article{zhang2024rapidrobusttrajectoryoptimization,
  title={Rapid and Robust Trajectory Optimization for Humanoids},
  author={Bohao Zhang and Ram Vasudevan},
  journal={arXiv preprint arXiv:2409.00303},
  year={2024}
}

Related Projects

RAPTOR also includes a better integrated implementation of our previous projects:

Autonomous Robust Manipulation via Optimization with Uncertainty-aware Reachability

Wrench Analysis for Inertial Transport using Reachability

Safe Planning for Articulated Robots Using Reachability-based Obstacle Avoidance With Spheres

Credits

Bohao Zhang ([email protected]): Project leader.

Zichang Zhou ([email protected]): System identification.

Jiyang Wang ([email protected]): Linearized reachability-based contact constraints.

This work is developed in RoahmLab, University of Michigan, Ann Arbor