RMPflow-CLF: Stable Controller Composition

• RMPflow-CLF is a computational framework that unifies Riemannian Motion Policies and Control Lyapunov Function theory to enable stable, concurrent composition of controllers for robotic systems.
• It employs per-subtask CLF-based quadratic programs to integrate heterogeneous nominal controllers while enforcing global stability through a structured pullback mechanism.
• Empirical results validate its effectiveness in real-time tasks like collision avoidance and formation keeping, demonstrating robustness and practical applicability in multi-robot settings.

RMPflow-CLF is a computational framework for the stable, concurrent composition of controllers in multi-objective robotic systems. It unifies Riemannian Motion Policies (RMPs) and Control Lyapunov Function (CLF) theory to enable the combination of heterogeneous subtask policies, each with potentially different geometries, dynamics, and user heuristics, while providing global stability guarantees. RMPflow-CLF generalizes the original RMPflow approach to admit arbitrary per-subtask nominal controllers via a CLF-based quadratic program (QP), thus accomodating various design heuristics under a provably stable, coordinate-free fusion mechanism (Li et al., 2019).

1. Riemannian Motion Policies and RMPflow

An RMP on an $m$-dimensional manifold $M$ with local coordinate $x\in\mathbb{R}^m$ is specified by acceleration $a(x,\dot x)$ and a state-dependent, symmetric positive-definite "inertia" matrix $M(x,\dot x)\in \mathbb{R}_+^{m\times m}$. Two representations are used:

• Canonical form: $(a(x, \dot x), M(x, \dot x))^M$
• Natural form: $[f(x, \dot x), M(x, \dot x)]^M$ where $f(x, \dot x) = M(x, \dot x) a(x, \dot x)$.

Often, $M(x, \dot x)$ is induced by a Riemannian metric $G(x, \dot x)$ with a curvature-correction: $$\Xi_G(x, \dot x) := \frac{1}{2} \sum_{i} \dot x_i\, \partial_x g_i(x, \dot x), \quad M(x, \dot x) = G(x, \dot x) + \Xi_G(x, \dot x)$$ where $g_i$ is the $i$-th column of $G$.

RMPflow organizes subtasks in an RMP-tree, with nodes as manifolds, the root as the robot configuration space $\mathcal{C}$, leaves as subtask spaces $T_k$, and edges as smooth maps $\psi_e$ with Jacobians $J_e$. Forward pass (pushforward) computes child states, and backward pass (pullback) fuses leaf RMPs at each node via: $$f_u = \sum_{j=1}^N J_{e_j}^\top [f_{v_j} - M_{v_j} \dot J_{e_j} \dot x], \quad M_u = \sum_{j=1}^N J_{e_j}^\top M_{v_j} J_{e_j}$$ The canonical acceleration at the root is $a_r = M_r^\dagger f_r$.

2. CLF-Based Stability Conditions and Inductive Fusion

RMPflow-CLF introduces explicit CLF treatment for per-leaf controllers. On each leaf $T_k$, for potential $\Phi_k(z_k)$ and metric $G_k(z_k, \dot z_k)$, define the Lyapunov candidate: $$V_k(z_k, \dot z_k) = \frac{1}{2} \dot z_k^\top G_k(z_k, \dot z_k)\dot z_k + \Phi_k(z_k)$$ A leaf force $f_k$ must satisfy the CLF constraint: $$\dot z_k^\top f_k \leq -\dot z_k^\top \left[ \nabla_{z_k} \Phi_k + \xi_{G_k}(z_k, \dot z_k) \right] - \alpha_k(\|\dot z_k\|)$$ with $\alpha_k$ a class-$\mathcal{K}$ function. This ensures $\dot V_k \leq -\alpha_k(\|\dot z_k\|)$.

By induction through tree pullback, if each leaf enforces the CLF decay, then the sum of Lyapunov functions across all leaf tasks,

$$V_r(q, \dot q) = \sum_k V_k(\psi_{r\rightarrow k}(q), J_{r\rightarrow k}\dot q)$$

guarantees non-increasing global energy ($$\dot V_r \leq -\sum_k\alpha_k(\|J_{r\rightarrow k}\dot q\|) \leq 0$$), yielding convergence via LaSalle’s principle to joint equilibria (Li et al., 2019).

3. Per-Leaf Quadratic Programs and Nominal Controller Integration

To reconcile CLF stability with user-supplied heuristics, RMPflow-CLF formulates, for each leaf, a one-constraint quadratic program: $$f_k^* = \arg\min_{f_k}\ \|f_k - M_k u_k^d\|_{P_k}^2 \quad \text{subject to} \quad \dot z_k^\top f_k \leq -\dot z_k^\top [\nabla\Phi_k + \xi_{G_k}] - \alpha_k(\|\dot z_k\|)$$ Here, $u_k^d$ is an arbitrary nominal control, $P_k \succ 0$ a user-defined weight. The closed-form projection solution ensures that the CLF constraint is minimally enforced, modifying the user’s control only as necessary. This mechanism allows inclusion of heuristics, demonstrations, or sampling-based plans without explicit derivation of geometric dynamic systems (GDS) curvature (Li et al., 2019).

4. Complete Control Cycle and Computational Feasibility

The RMPflow-CLF algorithm proceeds as follows:

1. Initialize $(q, \dot q)$ at the root.
2. Forward pass (pushforward): Propagate state via edge maps and Jacobians.
3. Leaf evaluation: For each leaf, solve the CLF-QP to compute $f_k^*$ and update $M_k$.
4. Backward pass (pullback): Aggregate per-branch RMPs using the pullback formula.
5. Resolve: Compute root acceleration $u = a_r = M_r^\dagger f_r$.
6. Actuate: Apply $u$ to the robot.

Each cycle requires $O(K)$ analytic QP solves and $O(\text{edges})$ Jacobian operations. This yields real-time performance for $K$ in the tens to hundreds, making the approach practical for multi-subtask and multi-robot systems (Li et al., 2019).

5. Empirical Validation and Performance

Empirical results demonstrate that RMPflow-CLF:

• Generates richer behaviors than strict GDS-based controllers due to greater flexibility in allowed nominal fields (e.g., spiral, time-varying) while retaining identical stability guarantees.
• Outperforms conventional flow-GDS in challenging settings such as multi-robot collision avoidance, notably by resolving symmetry-induced deadlocks via heuristic vector fields.
• Validated on hardware (the Robotarium testbed) in formation-keeping and leader-following, enabling smooth execution of complex leader paths while maintaining provable stability and formation integrity (Li et al., 2019).

6. Extensions via Lyapunov Function Shaping (RMPfusion)

RMPfusion extends RMPflow-CLF by introducing learnable, state-dependent weights $w_i(q)$ for each subtask Lyapunov function: $$\widetilde V_i(q,\dot q) = w_i(q) V_i(q,\dot q), \quad V_{\text{fusion}}(q,\dot q) = \sum_{i=1}^K w_i(q) V_i(q,\dot q)$$ Modified pullback forces, including explicit corrections, ensure cancellation of cross-terms arising from $\dot w_i$: $$f_r = \sum_{i=1}^K \left[ w_i J_i^\top(f_i - M_i \dot J_i) + h_i \right]$$ where $h_i$ cancels the $\nabla_q w_i$-related terms. For positive, smooth weights with proper inertia, $V_{\text{fusion}}$ provides global Lyapunov stability, even as the weight functions are trained, for example by imitation learning. The structured policy class supports gradient-based end-to-end optimization and guarantees safety at every iteration, regardless of weight network initialization (Mukadam et al., 2019).

7. Theoretical Guarantees and Applicability

RMPflow-CLF preserves coordinate-free concurrency: each subtask controller is developed independently on its own geometry, with global stability ensured through tree-structured CLF constraints and energy-like descent guarantees. The approach accommodates user-chosen controller heuristics, is computationally efficient for real-time implementation, and supports extension to learned Lyapunov shaping for improved adaptivity. The central theoretical results—inductive CLF composition and closed-form QP solutions—anchor the method's robustness and generality in multi-objective, high-DOF robotic control (Li et al., 2019, Mukadam et al., 2019).