Projection-Retraction MPPI: Exact Constraint-Manifold Control for Manipulators

Anonymous Authors
Submitted to IEEE Robotics and Automation Letters (RA-L)
Paper arXiv (Coming Soon) Code (Coming Soon)

Overview

PR-MPPI optimizes only over motions that respect the local constraint geometry, then retracts the selected finite-step command back onto the equality manifold.

Project Every Rollout

Each sampled velocity is restricted to the equality tangent space and the active inequality half-spaces before the state is propagated.

Preserve Both Constraint Types

Inequality corrections remain inside the equality tangent space, preventing safety corrections from breaking the closed-chain grasp.

Retract the Command

The executed finite step is restored to the equality manifold to the prescribed numerical tolerance, independently of task weighting.

Abstract

Model Predictive Path Integral (MPPI) control is widely used in manipulation for its gradient-free, parallel handling of non-convex costs. Manipulation tasks, however, often impose constraints that hold throughout the motion: a closed kinematic chain that two grasping arms keep exactly, or joint limits and obstacle clearances that are never crossed. MPPI handles such constraints only through the cost, as soft penalties that hold approximately and fail under a strong task cost. To address this, we propose Projection-Retraction MPPI (PR-MPPI), which enforces the constraints inside the sampled dynamics. At every rollout step, the sampled velocity is projected to satisfy both constraint types: the equality restricts it to a subspace, and each inequality to a half-space within that subspace, so inequality handling never breaks the equality. This projection, however, satisfies the constraints only to first order, and a finite step leaves a small drift off the equality. Therefore, we retract the returned command back onto the constraint, to numerical tolerance and independent of task weighting. We validate PR-MPPI on 14-DoF dual-arm systems. In simulation, the returned commands satisfy the closed-chain equality to numerical tolerance through a joint-limit stress test and randomized obstacle avoidance. On real hardware, the arms of a Unitree H1-2 humanoid reactively avoid a moving obstacle.

Contributions

  • We propose PR-MPPI, a constrained sampling-based MPC framework in which a single velocity-space projection carries both constraint types inside the sampled dynamics: each active inequality is corrected within the equality tangent subspace, so inequality handling cannot break the equality regardless of which margins are active. A retraction step then removes the finite-step drift that the first-order projection leaves on the equality, returning the command to satisfy the equality to numerical tolerance.
  • We establish execution-layer guarantees. Every retracted command satisfies the equality constraint to the prescribed numerical tolerance, independent of the MPPI weighting. Under the stated regularity and feasibility conditions, a finite-step relaxation of the shifted-margin layer is forward invariant, and a safety buffer satisfying the derived finite-step bound preserves the true inequality margins (Remark 1, Proposition 1).
  • We validate PR-MPPI on a 14-DoF dual-arm Franka system in simulation, where the retracted commands satisfy the closed-chain equality to numerical tolerance while the measured trajectories retain the joint-limit and obstacle margins under aggressive task commands, and demonstrate reactive avoidance of a moving obstacle on the dual arms of a Unitree H1-2 humanoid.

Method

PR-MPPI puts equality and inequality constraints inside the sampled dynamics, then restores the executed finite step to the equality manifold.

PR-MPPI framework: rollout projection, cost evaluation, execution projection, and retraction
PR-MPPI framework. Every sampled input passes through \(\Pi_{\mathrm{act}}\) before rollout integration. The weighted nominal sequence is filtered again along its own predicted states, and its first finite step is retracted onto \(\mathcal{M}\). The lower panels show tangent projection followed by inequality correction within that tangent space.

  1. Define the constraint geometry

    The equality defines a manifold. With \(\bar h(q)=h(q)-h_{\mathrm{safe}}\), a safety buffer shifts each inequality into a CBF guard condition.

    \(\textbf{Equality Constraint:}\; \mathcal{M}=\{q\mid c(q)=0\},\; J_c(q)u=0\) \(\textbf{Inequality Constraint:}\; J_h(q)u\ge-\Gamma\bar h(q)\)

  2. Project every sampled velocity

    Before propagation, each sample is projected onto the equality tangent space and the inequality half-spaces. The solve starts with violating or near-boundary margins, then adds the most violated omitted half-space until every residual is nonpositive.

    \(\begin{aligned} \Pi_{\mathrm{act}}(q,\tilde u) ={}& \arg\min_u && \tfrac12\lVert u-\tilde u\rVert^2 \\ & \hspace{0.65em}\mathrm{s.t.} && J_c(q)u=0 \\ & && J_h(q)u\ge-\Gamma\bar h(q) \end{aligned}\)

  3. Update and filter the nominal horizon

    MPPI averages the filtered rollout sequences. Because averaging removes their individual guarantees, the entire nominal horizon is filtered again while propagating its own predicted states.

    \(U^*=\sum_{k=1}^{K}w_k\tilde U^{+(k)}\) \(u_t^\star=\Pi_{\mathrm{act}}(q_t,u_t^*),\qquad t=0,\ldots,T-1\)

  4. Retract the executed step

    The first projected step is tangent-feasible but leaves \(\mathcal{O}(\Delta t^2)\) equality drift. Gauss–Newton retraction removes it to the prescribed tolerance.

    \(q^+=R_{\mathcal{M}}(q_0+\Delta t\,u_0^\star),\qquad \text{such that}\quad \lVert c(q^+)\rVert<\varepsilon_{\mathrm{tol}}\)

Result. At active-set termination, every inequality half-space is satisfied and the correction remains in the equality tangent space; retraction then makes the commanded equality residual a chosen numerical tolerance.

Simulation Experiments

Joint-Limit Stress Test

A dual-arm Franka system lowers a level tray from \((0.45,0,1.0)\,\mathrm{m}\) toward an IK target at \((0.45,0,0.55)\,\mathrm{m}\). Both fourth joints start at \(-2.01\,\mathrm{rad}\), while the target \(-2.41\,\mathrm{rad}\) lies beyond the imposed lower bound \(-2.35\,\mathrm{rad}\). A \(0.05\,\mathrm{rad}\) safety margin shifts the guard boundary to \(-2.30\,\mathrm{rad}\), activating both joint-limit constraints nearly simultaneously.

DQ-MPPI
PR-MPPI Full
Left Joint Limit
Right Joint Limit
Tilting Angle
Tracking Error
Method Chain trans. (m) Chain rot. (rad) Tilt (rad) Max. bound viol. (rad) Compute time (ms)
DQ-MPPI0.004 ± 0.0000.004 ± 0.0010.061 ± 0.0470.082238.948 ± 19.473
PR-MPPI Full0.003 ± 0.0000.003 ± 0.0000.002 ± 0.0000.00019.351 ± 1.216

Full PR-MPPI uses no joint-limit violation penalty in this experiment. Both fourth joints settle near the shifted guard boundary without crossing the true bound, while the retracted command satisfies every equality channel below \(10^{-9}\).

Randomized Obstacle Avoidance

The level tray moves from \((0.5,0,0.5)\,\mathrm{m}\) to \((0.5,0,1.0)\,\mathrm{m}\) past a spherical obstacle of radius \(0.05\,\mathrm{m}\). Thirty paired obstacle placements sample \(x\in[0.40,0.60]\,\mathrm{m}\) and \(y\in[-0.10,0.10]\,\mathrm{m}\), with every controller evaluated at the same placements. Full PR-MPPI uses a \(0.02\,\mathrm{m}\) tray-obstacle safety margin and no tray-obstacle collision penalty.

MC-MPPI
DQ-MPPI
PR-MPPI Full
PR-MPPI Execution-Only Inequality
MethodSuccess / TotalStuckTray dropCollision
MC-MPPI13 / 301700
DQ-MPPI18 / 301110
PR-MPPI Full30 / 30000
PR-MPPI execution-only inequality29 / 30100

Projecting the obstacle inequality inside the rollouts changes which trajectories receive low cost: Full PR-MPPI averages over motions that already pass the obstacle and completes all 30 trials. The execution-only ablation remains safe but has one central-placement stall and roughly twice the goal-dwell-time spread.

Real-World Experiments

Reactive Avoidance of a Moving Obstacle

The two 7-DoF arms of a Unitree H1-2 humanoid hold a loaded tray under the same closed-chain grasp while a person advances a hand-held obstacle from varying directions. The current obstacle position is supplied at every control cycle and held fixed over the rollout horizon, so PR-MPPI replans reactively without an obstacle-motion prediction model.

The same sampling–projection–retraction pipeline used in simulation is deployed online with only the platform kinematics replaced. The arms reshape their configuration inside the equality tangent space to create clearance, while retraction returns grasp-consistent commands and the measured tray remains level.

Observed limitation. When the obstacle approaches along a direction for which the closed-chain equality forbids the required retreat, the projected margin gradient \(N\nabla\bar{h}\) becomes small. In that configuration no admissible motion can increase clearance without relaxing the grasp; selectively relaxing the equality is left for future work.

Appendix: Implementation Details

Cost Function

\(S(\boldsymbol U)=\displaystyle\sum_{t=0}^{T-1}\!\left[\ell_{\mathrm{track}}(\boldsymbol q_t)+\ell_{\mathrm{coll}}(\boldsymbol q_t)+\ell_{\mathrm{jl}}(\boldsymbol q_t)+w_u\lVert\boldsymbol u_t\rVert_2^2\right]+\phi_{\mathrm{track}}(\boldsymbol q_T)\) \(\ell_{\mathrm{track}}=\begin{cases}w_q\lVert\boldsymbol q-\boldsymbol q_g\rVert_2^2, & \text{Exp. 1},\\ w_p\lVert\boldsymbol p_o-\boldsymbol p_g\rVert_2^2+w_R\!\left(1-\langle\boldsymbol\rho_o,\boldsymbol\rho_g\rangle^2\right), & \text{Exp. 2}.\end{cases}\) \(\ell_{\mathrm{coll}}=\displaystyle\sum_{d_i\in\mathcal D_{\mathrm{coll}}}w_i[\delta_i-d_i(\boldsymbol q)]_+^2,\qquad \mathcal D_{\mathrm{coll}}=\{d_{\mathrm{tray,obs}},d_{\mathrm{arm,obs}},d_{\mathrm{arm,arm}},d_{\mathrm{self}},d_{\mathrm{tray,arm}}\}\) \(\ell_{\mathrm{jl}}=w_{\mathrm{jl}}\displaystyle\sum_j[\delta_{\mathrm{jl}}-d_{\mathrm{jl},j}(\boldsymbol q)]_+^2,\qquad [x]_+=\max(x,0)\)

The terminal term uses the same tracking residual with terminal weights. Zero-weight terms are inactive (Full PR-MPPI: \(w_{\mathrm{jl}}=0\) in Exp. 1 and \(w_{\mathrm{tray,obs}}=0\) in Exp. 2); the equality \(\boldsymbol c(\boldsymbol q)=0\) is enforced geometrically rather than added as a penalty.

Cost termExp. 1Exp. 2
Running tracking\(w_q=30{,}000\)\(w_p=60,\;w_R=50\)
Terminal tracking\(w_{q,T}=300{,}000\)\(w_{p,T}=500,\;w_{R,T}=50\)
Control effort\(w_u=0.02\)\(w_u=0.02\)
Joint limit\(w_{\mathrm{jl}}=0\)\(w_{\mathrm{jl}}=1{,}000\)
Tray–obstacle0 (off)0 (Full), 1,800 (Exec-only)
Arm–obstacle0 (off)1,800
Arm–arm1,0001,000
Intra-arm self150150
Tray–arm0 (off)1,000

MPPI

  • Rollouts: \(K=1000\)
  • Horizon: \(T=30\) steps
  • Time step: \(\Delta t=1/30\,\mathrm{s}\)
  • Exploration: \(\sigma=0.03\,\mathrm{rad/s}\)

Constraint Layer

  • Eight-dimensional closed-chain equality
  • Relative-pose constraint plus tray roll and pitch
  • CBF gain: \(\gamma_i=5\)
  • Projection in rollouts and before execution

CUDA Acceleration

  • Fused CUDA kernels accelerate rollout projection and inequality correction
  • Batched constraint filtering across \(K\) rollouts
  • Reduced Python dispatch and intermediate tensor overhead
  • NVIDIA RTX 4060 Ti, 8 GB VRAM

BibTeX

@article{anonymous2026prmppi,
  title={Projection-Retraction MPPI: Exact Constraint-Manifold Control for Manipulators},
  author={Anonymous Authors},
  journal={IEEE Robotics and Automation Letters (RA-L)},
  note={Submitted},
  year={2026},
  url={https://anonymous.4open.science/w/prmppi/index.html}
}