Single-Rigid-Body-Dynamics MPC

• Single-Rigid-Body-Dynamics-Based MPC is a predictive control method that approximates robot dynamics as a single rigid body to efficiently compute control inputs under physical constraints.
• It formulates system behavior using Newton-Euler equations and incorporates terminal and stability constraints to ensure recursive feasibility and stable gait generation.
• The framework robustly handles hybrid dynamics and contact transitions, enabling practical real-time applications in legged, wheeled, and humanoid robotics.

Single-Rigid-Body-Dynamics-Based Model Predictive Control (MPC) refers to a class of optimization-based control strategies where the dynamical system is modeled as a single rigid body, and future behavior is optimized over a prediction horizon to generate control inputs that respect physical constraints and achieve performance objectives. This paradigm is widely adopted in robotics, locomotion, and manipulation due to its balance of dynamic fidelity and computational tractability. The single-rigid-body approximation is especially relevant for legged robots, wheeled-legged platforms, humanoids, and condensed locomotion tasks, allowing for fast online computation in real-time applications.

1. Mathematical Modeling in Single-Rigid-Body MPC

The defining feature of Single-Rigid-Body-Dynamics-based MPC formulations is the lumping of the system's mass and inertia into one central body, often denoted by its position $p$, orientation $\theta$ (quaternion, Euler angles, or dual quaternion), linear velocity $v$, and angular velocity $\omega$. Contacts, manipulation, and actuator inputs are handled as external generalized forces.

Typical equations governing the motion are:

• Translational: $\dot{p} = v$
• Rotational: $\dot{\theta} = T(\theta) \omega$ (where $T(\theta)$ maps angular velocity to the time derivative of orientation)
• Newton-Euler (SRBD):

$$\begin{aligned} \dot{\omega} &= I^{-1}(-\omega \times I\omega + \sum_{i=1}^{n_c} r_{E_i}(q_j) \times \lambda_{E_i}) \ \dot{v} &= g(\theta) + \frac{1}{m} \sum_{i=1}^{n_c} \lambda_{E_i} \end{aligned}$$

• Contacts are encoded either as force/moment inputs (for legged/wheeled systems), or via constraint-based formulations in manipulation contexts.

Key structural assumptions:

• Limb inertia can be neglected, i.e., their dynamic effects do not impact the centroidal motion (Bjelonic et al., 2020); this is valid for lightweight joints.
• For humanoid MPC, linear inverted pendulum (LIP) dynamics or centroidal dynamics models (CDM) are often used, with ZMP (zero moment point) velocities as control inputs (Scianca et al., 2019).

2. Optimization Problem Formulations and Constraints

The MPC optimization seeks to minimize cost functions subject to the physical system's constraints. These costs typically encode trajectory tracking, energy consumption, or deviation from reference footstep locations.

General Form

• Minimize:

$J = \sum_{k=0}^{N-1} \ell(x_k, u_k) + V(x_N)$

• Subject to:

$$x_{k+1} = f(x_k, u_k), \quad x_k \in \mathcal{X}, \quad u_k \in \mathcal{U}$$

where $f(x_k, u_k)$ is given by SRBD equations.

Constraints

• Admissible ZMP region, kinematic workspace, friction cones (Scianca et al., 2019, Li et al., 2021)
• Contact/no-contact variables (binary, in mixed-integer MPC) for hybrid gait selection (Lin et al., 2022)
• Specific constraints for wheels (rolling direction allowed, perpendicular velocity constrained to zero) (Bjelonic et al., 2020)
• Terminal/stability constraints on divergent components (capturability) (Scianca et al., 2019)
• Torque/force limits, actuator bounds (Li et al., 2021)

Hybrid formulations may include contact-implicit representations, where the contact sequence itself is a decision variable, often solved via mixed-integer programming (Lin et al., 2022, Kong et al., 2022). For highly-dynamic scenarios on resource-constrained hardware, structure-exploiting solvers (e.g., ADMM embedded with LQR factorizations) are adopted (Nguyen et al., 2023).

3. Stability, Feasibility, and Internal Dynamics

An essential aspect of SRBD-based MPC is the intrinsic instability of certain simplified models (e.g., LIP). Maintaining ZMP feasibility is not sufficient for internal stability of the center of mass (CoM). The IS-MPC approach (Scianca et al., 2019) enforces an explicit stability constraint on the "unstable" (divergent) component of the dynamics: $$x_u^k = \eta \int_{t_k}^{\infty} e^{-\eta(\tau - t_k)} x_z(\tau)\,d\tau$$ In discrete time, the tail term is introduced for the control horizon truncation, leading to equivalent terminal constraints (e.g., $x_u^{k+C} = x_z^{k+C}$ for truncated tails).

Recursive feasibility is analyzed via admissibility intervals: $x_u^{(k,m)} \leq x_u^k \leq x_u^{(k,M)}$ with analytic bounds linked to ZMP region limits.

These constraints guarantee bounded CoM behavior, which is fundamental for the reliable generation of dynamically stable gaits and motions using SRBD simplifications.

4. Advanced Solution Algorithms and Derivative Computations

Real-time deployment demands computationally efficient solvers. Innovations include:

• Inverse dynamics treated as an equality constraint within multiple shooting frameworks, allowing for improved numerical robustness and parallelization (Katayama et al., 2021).
• Recursive Newton-Euler algorithm (RNEA) utilized for computationally fast inverse dynamics and analytic sensitivity computation.
• Analytical formulas for second-order derivatives (Hessians) of SRBD equations, using tensorial extensions of spatial vector algebra. These enable full second-order DDP or SQP algorithms with low run-times, demonstrated on ATLAS humanoid (36 DoF) (Singh et al., 2023).
• Code-generation in C/C++ for optimized computation, facilitating real-time MPC even for complex robots.

For microcontroller/MPC on embedded systems, structure-based solvers such as TinyMPC use LQR precomputation and ADMM splitting to achieve up to $9\times$ speedup and sub-millisecond solve times (Nguyen et al., 2023).

5. Applications in Legged, Wheeled, and Hybrid Robots

SRBD-MPC has been applied across a spectrum of platforms:

• Humanoid gait generation (NAO, HRP-4) with omnidirectional motion commands, real-time ZMP trajectory planning, and recursive feasibility under various control horizons (Scianca et al., 2019).
• Wheeled-legged quadrupeds (e.g., ANYmal with torque-controlled wheels), using joint velocity and force optimization for hybrid driving/gait transitions, and rolling constraints encoded directly in the MPC (Bjelonic et al., 2020).
• Bipedal robots achieving robust performance on rough terrain and dynamic motions (walking, hopping, running) with force-and-moment-based MPC, mapping force/moment solutions to actuator commands via Jacobians (Li et al., 2021).
• Quadrupedal robots with manipulators (Go2 + Kinova arm), using a decomposition strategy coupling SRB models for base locomotion and full-order models for arm manipulation within a layered NMPC/WBC control architecture (Sambhus et al., 29 Jul 2025).
• Deep RL-based policies for adaptive SRB tracking in simulated full-body characters, using quadratic programming solvers and momentum-mapped inverse kinematics for environment adaptation and controller transition, all with high sample efficiency (Kwon et al., 2023).
• Online hybrid non-convex MPC for single rigid body locomotion, solving mixed-integer optimization with no small-angle or pre-defined gait assumptions, employing offline learning of the problem-solution map for rapid online adaptation running at over 50 Hz (Lin et al., 2022).
• Hybrid iLQR MPC for contact-implicit stabilization, including dynamic contact sequence modification and efficient gradient computation via saltation matrix and analytic derivatives, with robust performance under large perturbations (Unitree A1 quadruped) (Kong et al., 2022).

6. Innovations, Practical Challenges, and Future Prospects

Recent research emphasizes several core advancements:

• Relaxation of restrictive modeling assumptions (small-angle linearizations, fixed gaits), expanding reachable dynamic behaviors.
• Contact-implicit formulations and real-time gait adaptation leveraging integer optimization and learning-based warm-starts.
• Finite-horizon tail projection and terminal constraint strategies for stability under SRBD simplifications.
• Parallelization and GPU acceleration for faster rollouts (e.g., Isaac Gym), and structure-exploiting solvers for onboard computation.
• Advanced derivative computation (second-order analytic) and code-generation for deployment on high-DoF systems.

The main challenges remain in ensuring stability and recursive feasibility in the presence of model reduction, handling hybrid dynamics (contact transitions, mode switches), and managing computational complexity for real-time execution on embedded platforms. Future directions point to:

• Systematic derivation of Koopman-based linear representations (dual quaternion observables) for extended global controller performance (Zinage et al., 2021).
• Distributed and decentralized MPC schemes for complex multi-agent or multi-arm systems (Sambhus et al., 29 Jul 2025).
• Integration with learning algorithms for global optimization over hybrid dynamics (Lin et al., 2022).

Single-Rigid-Body-Dynamics-Based MPC continues to be a central paradigm in legged robotics, yielding robust, efficient, and physically grounded control solutions that bridge real-time implementation and dynamic motion fidelity across a wide array of platforms and locomotion regimes.