Features of optimal control solvers

Table of Contents

• List of optimal control solvers
• Common features among solvers
• robotoc::OCPSolver
  • Features
  • Methematical formulation
  • Solution algorithm
• robotoc::UnconstrOCPSolver
  • Features
  • Methematical formulation
  • Solution algorithm
• robotoc::UnconstrParNMPCSolver
  • Features
  • Methematical formulation
  • Solution algorithm
• References

List of optimal control solvers

The following three solvers are available:

• robotoc::OCPSolver: Optimal control solver for robot systems with rigid contacts and/or a floating base. This solver can optimize the trajectory and switching times simultaneously.
• robotoc::UnconstrOCPSolver: Optimal control solver for robot systems without rigid contacts or a floating base. More efficient than robotoc::OCPSolver.
• robotoc::UnconstrParNMPCSolver: Optimal control solver for robot systems without rigid contacts or a floating base. Possibly very efficient when the number of available CPU cores is very large.

Common features among solvers

The common features in mathematical formulation, algorithms, and implementation among these three solvers are as follows:

• Direct multiple shooting method [1, 2]:
  • has a nice convergence property.
  • fully leverages parallel computation of the KKT system with OpenMP.
• Primal-dual interior point method [3, 4]:
  • can treat many inequality constraints including nonlinear ones very efficiently.
  • allows infeasible initial guess of the solution.
  • enables us to perform very fast suboptimal MPC by fixing the barrier parameter.
• Gauss-Newton method:
  • improves computational speed and numerical conditioning.
• Structure-exploting algorithms to compute Newton steps [2, 4, 5]:
  • can compute the Newton steps with linear time complexity with respect to the length of the horizon.
• Very fast rigid body kinematics and dynamics computation thanks to Pinocchio.
  • Pinocchio provides very efficient forward kinematics, kinematics Jacobians, the recursive Newton Euler algorithm (RNEA), and derivatives of the RNEA [6].
  • Pinocchio also gives us an interface to construct a robot model (robotoc::Robot) via URDF files.
• Versatile cost function and constraints interface:
  • new cost compoments can be easily defined by inheriting robotoc::CostFunctionComponentBase.
  • The cost function can be easily be constructed by collecting cost components into robotoc::CostFunction
  • new constraint compoments can be easily defined by inheriting robotoc::ConstraintComponentBase or robotoc::ImpactConstraintComponentBase.
  • The constraints can be easily be constructed by collecting constraint components into robotoc::Constraints

robotoc::OCPSolver

Features

robotoc::OCPSolver is an optimal control solver for robot systems with rigid contacts and/or a floating base. This is the main solver of robotoc. The unique features of robotoc::OCPSolver are:

• Multiphase optimal control problem construction through the contact sequence interface:
  • robotoc::ContactSequence interface enables us to formulate the complicated optimal control problems involving changes of dynamics and state jumps due to rigid contacts.
• Lifted contact dynamics [7]:
  • Relaxes the high nonlinearity in optimization problems typically caused by inequality constraints treated by the interior point methods.
• Riccati recursion for switching time optimization (STO) problems [8]:
  • Computes the Newton steps including the switching times with linear time complexity with respect to the length of the horizon. Moreover, the Riccati-recursion-based Hessian modification improves numerical stability.
    
• Constraint transformation of pure-state constraints for efficient Riccati recursion [9]:
  • The pure-state equality constraints representing the switching constraints are treated very efficiently while preserving accuracy and guaranteeing optimality by the constraint transformation.

Methematical formulation

TODO

Solution algorithm

TODO

robotoc::UnconstrOCPSolver

Features

robotoc::UnconstrOCPSolver is an optimal control solver for robot systems without rigid contacts or a floating base. This is recommended than robotoc::OCPSolver for such systems. The unique features of robotoc::UnconstrOCPSolver are:

• Inverse dynamics based formulation [10]:
  • Inverse dynamics-based formulation enables very fast computation compared with the forward dynamics-based formulation. You can check this fact easily by comparing robotoc::UnconstrOCPSolver and robotoc::OCPSolver.
• Riccati recursion to compute Newton steps [2, 4]:
  • Computes the Newton steps with linear time complexity with respect to the length of the horizon.

Methematical formulation

TODO

Solution algorithm

TODO

robotoc::UnconstrParNMPCSolver

Features

robotoc::UnconstrParNMPCSolver is an optimal control solver for robot systems without rigid contacts or a floating base. This is recommended than robotoc::UnconstrOCPSolver if the available number of CPU cores are very large. The unique features of robotoc::UnconstrParNMPCSolver are:

• Inverse dynamics based formulation [10]:
  • Inverse dynamics-based formulation enables very fast computation compared with the forward dynamics-based formulation.
• Highly parallelizable Newton-type method [5]:
  • This method can parallelize all steps of Newton-step computation while the Riccati recursion needs serial computation in solving the KKT system. Note that the convergence speed can decrease because this method introduces an approximation for parallelization.

Methematical formulation

TODO

Solution algorithm

TODO

References

[1] H. Bock and K. Plitt, "A multiple shooting algorithm for direct solution of optimal control problems," in 9th IFAC World Congress, pp.1603–1608, 1984.
[2] J. B. Rawlings, D. Q. Mayne, and M. Diehl, Model Predictive Control: Theory, Computation, and Design, 2nd edition, Nob Hill Publishing, 2017.
[3] J. Nocedal and S. J. Wright, Numerical Optimization, 2nd edition, Springer, 2006.
[4] C. Rao, S. J. Wright, and J. B. Rawlings, "Application of interior-point methods to model predictive control," Journal of Optimization Theory and Applications, vol. 99, no. 3, pp. 723–757, 1998.
[5] H. Deng and T. Ohtsuka, "A parallel Newton-type method for nonlinear model predictive control," Automatica, Vol. 109, pp. 108560, 2019.
[6] J. Carpentier and N. Mansard, "Analytical derivatives of rigid body dynamics algorithms," Robotics: Science and Systems, 2018.
[7] S. Katayama and T. Ohtsuka, "Lifted contact dynamics for efficient optimal control of rigid body systems with contacts," 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (to appear), https://arxiv.org/abs/2108.01781, 2022
[8] S. Katayama and T. Ohtsuka, "Structure-exploiting Newton-type method for optimal control of switched systems," https://arxiv.org/abs/2112.07232, 2021.
[9] S. Katayama and T. Ohtsuka, "Efficient Riccati recursion for optimal control problems with pure-state equality constraints," 2022 American Control Conference (ACC), pp. 3579-3586, 2022
[10] S. Katayama and T. Ohtsuka, "Efficient solution method based on inverse dynamics for optimal control problems of rigid body systems," 2021 IEEE International Conference on Robotics and Automation (ICRA), pp.2070-2076, 2021.