Whole-Body MPC for Robotic Systems

• Whole-body MPC is an optimal control approach that plans and regulates high-dimensional robotic configurations using a unified, bilevel optimization framework.
• It employs continuous Bezier-curve trajectory parameterizations to enable smooth transitions, reduce computation time, and enforce kinematic constraints.
• The framework integrates predictive admittance control and hard collision constraints to achieve responsive, compliant interaction in dynamic, real-world environments.

A whole-body model-predictive controller (MPC) is an optimal control system that plans and regulates the evolution of a high-dimensional robotic system’s entire body configuration over a finite prediction horizon. In contrast to traditional reduced-order or task-space formulations, whole-body MPC directly considers complex multi-body dynamics, high degrees of freedom (DoF), and multi-contact constraints, often aiming to coordinate locomotion, manipulation, and balance in a unified optimization framework. Modern whole-body MPCs exploit advances in model parameterization, convexification, hierarchical representations, warm-start strategies, and real-time optimization to tackle the otherwise prohibitive computational complexity of full dynamical planning for legged and mobile manipulators.

1. Bilevel and Hierarchical Model Predictive Control Architectures

Contemporary whole-body MPC frameworks frequently adopt multilayer controller structures to modularize the generation of long-horizon kinematic or task-space references and their refinement under high-dimensional whole-body constraints. An archetypical approach is the bilevel MPC framework (Du et al., 30 Oct 2024), in which:

• The top-level MPC (commonly called “MPC-T”) plans object- or task-oriented trajectories—such as the SE(3) pose of dual-arm end-effectors—over an extended horizon using reduced-order parameterization and simplified feasibility approximations.
• The lower-level whole-body MPC (“MPC-W”) shortens the horizon but increases model fidelity to synthesize entire joint trajectories, explicitly accounting for all full-body kinematic, dynamic, and actuation constraints, as well as real-time force–compliance objectives.
• Feedback between the levels ensures that the refined whole-body trajectory closely tracks the object/task reference while remaining collision-free, feasible for the mechanical structure, and compliant during environmental interaction.

This division provides tractable long-horizon planning while maintaining feasibility and high responsiveness to disturbances at the whole-body level.

2. Bezier-Curve and Continuous Trajectory Parameterizations

A critical advancement in whole-body MPC is the use of continuous and smooth trajectory representations, most notably via Bezier-curve parameterization. In this scheme (Du et al., 30 Oct 2024):

• Each trajectory (task space or joint space) is described using a small number of Bezier control points, e.g.

$$q(t) = B_q(Q, \bar{t}) = \sum_{j=0}^{N_q} \binom{N_q}{j} \bar{t}^j (1 - \bar{t})^{N_q-j} Q_j$$

where $Q_j$ are the control points for the joint position, and $\bar{t}$ is the normalized time on $[0,1]$.

• Analytical differentiation yields closed-form expressions for velocities and accelerations as functions of differences between control points, permitting exact enforcement of velocity and acceleration bounds at the control point level.
• The same formalism is extended to orientation planning (in SE(3)), using either quaternion-based Bezier curves or auxiliary representations (ψ) mapped to unit quaternions, ensuring globally smooth rotational interpolation without explicit orthogonalization constraints at every knot.
• This approach obviates the need for dense time discretization, dramatically lowering the decision-variable count (e.g., from 224 to 96 in task-space MPC) and computation time (from 60.8 ms to 6.2 ms per loop in the referenced application), and ensures smooth transitions at all states.

A plausible implication is that Bezier-parameterized MPCs deliver more robust model-state transitions and superior tracking performance, especially as the dynamic and geometric complexity of the manipulation/loco-manipulation tasks increases.

3. Predictive Admittance Control and Consistency of Hybrid Control Modes

For online interaction with dynamic or uncertain environments, whole-body MPCs incorporate predictive admittance control. In this paradigm (Du et al., 30 Oct 2024):

• The MPC-W stage optimizes both trajectory and the compliant force response by incorporating admittance constraints of the form:

$$F_{\text{opt}} = F_{\text{act}} + \Lambda \ddot{\tilde{p}} + K \tilde{p} + D \dot{\tilde{p}}$$

where $\tilde{p} = p_{\text{opt}} - p_{\text{ref}}$ is the deviation between planned and reference end-effector position.

• By parameterizing $\tilde{p}(t)$ as a Bezier curve, the optimization only solves for its control points, enabling simultaneous force and position policy synthesis in short horizons, compatible with a hybrid position/velocity-control robot architecture.
• This representation ensures that position, velocity, and force commands remain mutually consistent and continuous, crucial for hybrid robots (e.g., velocity-controlled mobile bases with position-controlled arms).
• The closed-form structure also enables the analytic handling of initial/final conditions, as well as the embedding of zero terminal velocity/acceleration requirements via the assignments of the final Bezier points.

4. Hard Constraints, Collision Avoidance, and Real-Time Replanning

Modern whole-body MPC frameworks—particularly under Bezier parameterization—seamlessly integrate strict constraints critical for real-world manipulation:

• Initial and terminal equality constraints are enforced by mapping boundary states directly to the first/last control points of the Bezier curve.
• Velocity and acceleration limits are converted into simple bounds on the Bezier derivatives at control points, with the convex-hull property guaranteeing that the entire trajectory obeys these constraints.
• Collision avoidance is captured via hard constraints at multiple path samples; for static/dynamic obstacles, the MPC enforces a minimal allowable separation at characteristic points (e.g., the midpoint of coordinated dual-arm end-effectors or mobile base positions).

This inherent convexity and analytic tractability equip the controller for fast replanning—critical as the robot encounters environmental changes or unexpected disturbances. Replanning is achieved by re-solving the bilevel MPC loops with updated obstacle and robot state information, keeping real-time computation feasible even for high-DoF systems.

5. Computational Performance and Hardware Results

The described framework (Du et al., 30 Oct 2024) demonstrates significant reduction in both computational time and memory requirements compared to classic discretized MPC techniques. Empirically,

• In simulation, Bezier-mapped MPC reduces computation times by an order of magnitude, shrinks trajectory tracking errors and their variance (especially for dense knot settings), and maintains accurate model-state transitions following sharp disturbances.
• Real-world experiments on dual-arm mobile manipulators confirm that the controller supports compliant multi-contact manipulation, dynamic obstacle avoidance, narrow-space navigation, and push recovery in both static and dynamic scenes.
• Both long-horizon (object/task) and short-horizon (whole-body) MPC components demonstrate loop times in the sub-10 ms regime, providing sufficient bandwidth for real-time feedback on high-complexity robots.

In addition to efficiency, these techniques yield more robust and smooth execution—reducing oscillations, enforcing all constraints, and enabling adaptive interaction control in practical, safety-critical applications.

6. Mathematical Formulation and Algorithmic Summary

Fundamental equations and algorithmic elements in the reported framework include:

Stage	Parameterization & Constraints	Optimization Variable
MPC-T (Task)	Bezier curves (pos, rot in SE(3)); initial/final/vel/acc bounds; collision avoidance	Control points for dual-arm end-effectors (position, orientation)
MPC-W (Whole-body)	Bezier curves (joint-space); analytic derivatives for velocities/accelerations; predictive admittance constraints; obstacle avoidance	Control points for joints and admittance action; mobile base and arms

Core optimization problem (continuous trajectory form):

$$\min_{E} \int_0^T L_\varepsilon(\varepsilon(t)) dt \quad \text{s.t.}\; g_\varepsilon(\varepsilon(t)) \leq 0,\ \forall t \in [0, T]$$

With trajectory parameterized by a Bezier polynomial:

$$\varepsilon(t) = B_\varepsilon(E, \bar{t}) = \sum_{j=0}^{N_\varepsilon} {N_\varepsilon \choose j} \bar{t}^j (1 - \bar{t})^{N_\varepsilon-j} E_j$$

Controllers optimize for both the pose and the compliance force profiles by jointly optimizing the relevant control point vectors.

7. Significance and Perspectives

The integration of continuous, convex trajectory representations and predictive admittance models into a bilevel whole-body MPC markedly enhances computational tractability, real-time response, and robustness in high-dimensional robots. The approach alleviates the barrier of dense time discretization and cumbersome constraint enforcement, facilitating deployment on highly redundant mobile manipulators interacting with dynamic, uncertain environments.

Validated on both simulated and physical platforms, these advances are poised to accelerate the deployment of safe, efficient, and compliant dual-arm mobile systems in industrial, care, and service domains, as well as pave the way for future work in scalable, learning-augmented, or multi-agent whole-body motion planning.