Ungar - A C++ Framework for Real-Time Optimal Control Using Template Metaprogramming

Flavio De Vincenti, Stelian CorosIROS 2023 · DOI

TL;DR

This paper introduces Ungar, a C++ library that leverages template metaprogramming to create efficient, real-time optimal control (MPC) systems for robotics. By shifting complex variable management to compile-time, Ungar allows developers to focus on system dynamics and control strategies without sacrificing runtime performance, simplifying the implementation of sophisticated robotic controllers.

What Problem Does This Paper Address?

Implementing Model Predictive Control (MPC) for complex robotic systems, especially those with high-dimensional states and fast dynamics, presents a significant engineering challenge. The core issue lies in efficiently managing and computing derivatives for the large number of variables involved in nonlinear programming (NLP) problems that underpin MPC. Traditional approaches often require manual indexing and can lead to tedious, error-prone code, while existing libraries might force a single, flat vector representation of variables, obscuring the inherent hierarchical structure of robotic systems and adding runtime overhead. This gap hinders the rapid development and deployment of advanced robotic control strategies.

Methods

Ungar tackles this problem by employing template metaprogramming (TMP). The core idea is to define the structure of decision variables (states, inputs, parameters) using a hierarchical, named approach that mirrors mathematical formulations. This definition is done at compile-time using C++ templates and libraries like Boost.Hana.

Key techniques include:

  • Variable Template Class (var_c): This class allows users to define "leaf" variables (e.g., position, velocity) and "branch" variables that group other variables hierarchically (e.g., a robot's state x composed of position, orientation, etc.).
  • Compile-Time Hierarchy Encoding: All relationships and indexing information are encoded directly into the C++ type system. This means that when you access a sub-variable, the compiler knows exactly where it is in memory.
  • Zero-Cost Access: Because the indexing and structure are known at compile-time, accessing any part of the variable hierarchy incurs no runtime cost, behaving as if the code was manually written for that specific structure.
  • Variable Maps (VariableMap, VariableLazyMap): These classes provide an interface to map the defined variable hierarchies onto contiguous memory buffers (using Eigen's Map). VariableMap offers direct reference access, while VariableLazyMap provides a slightly less performant but faster-to-instantiate option, useful for intermediate code generation.
  • Optional Integrations: Ungar bundles optional wrappers for automatic differentiation (via CppADCodeGen) and an SQP solver (using OSQP), simplifying the integration of a full MPC pipeline.

Datasets

Not discussed in the paper. The paper focuses on the framework itself and demonstrates its use with simulated robotic systems.

Key Insights and Ideas

  • Compile-time is your friend for performance: By pushing complex data structure management and indexing to compile-time, Ungar achieves zero-cost access to variables, which is crucial for real-time control.
  • Hierarchical variable definition simplifies complex systems: The ability to define variables in a structured, named, and hierarchical way (e.g., robot.leg[0].joint.position) makes it much easier to represent and reason about complex robotic systems compared to flat arrays.
  • Bridging the gap between mathematical formulation and efficient code: Ungar provides a "metalanguage" that allows users to write code that closely resembles their mathematical models, while the compiler translates this into highly optimized C++ code.
  • Flexibility for diverse robotic applications: The framework's design allows it to be easily adapted to vastly different robotic systems, from single quadrotors to multi-robot collaborative tasks, by simply changing the variable definitions.

Key Results

The paper demonstrates Ungar's effectiveness through implementations of MPC controllers for:

  • Quadrupedal locomotion: Using a nonlinear SRBD model with unit quaternions for orientation.
  • Collaborative loco-manipulation: Controlling a team of four-legged robots manipulating a shared object, showcasing the ability to handle high-dimensional, coupled systems with deep hierarchies.

The authors also provide compile-time benchmarks comparing VariableMap and VariableLazyMap. For a complex octocopter example with N=390, VariableLazyMap took only 14s to compile, while VariableMap took 39s. This highlights the trade-off between compile-time performance and runtime access efficiency.

Limitations and Future Work

  • Compile-time overhead for very deep hierarchies: While TMP offers performance benefits, extremely deep or nested variable hierarchies can lead to high compile times and, in rare cases, compiler crashes. The authors suggest using VariableLazyMap as a workaround in such scenarios.
  • Future work: The authors plan to optimize Ungar's design to further improve compile times and expand the library with more tools to facilitate the implementation of high-performance MPC controllers.

Summary updated

BibTeX
@inproceedings{vincenti2023ungar,
  author = {Flavio De Vincenti and Stelian Coros},
  title = {{Ungar - A C++ Framework for Real-Time Optimal Control Using Template Metaprogramming}},
  booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
  year = {2023},
  doi = {10.1109/iros55552.2023.10341365},
}
Figure