CAMP-MPPI: Contact-Aware Manifold Projection MPPI for Hard Dynamics-Level Contact-Wrench Constraints

Anonymous Authors

Abstract

Contact-rich manipulation requires not only kinematic feasibility but also dynamics-level admissibility of interaction wrenches to prevent slip, contact loss, or object instability. Existing hard-constrained Model Predictive Path Integral (MPPI) methods primarily enforce kinematic feasibility, whereas force-aware MPPI formulations typically account for interaction forces through rollout objectives or soft penalties.

This paper presents Contact-Aware Manifold Projection MPPI (CAMP-MPPI), a sampling-based control framework that jointly enforces contact-manifold consistency, kinematic inequalities, and hard dynamics-level wrench constraints within MPPI rollouts. CAMP-MPPI maps heterogeneous physical constraints into a common commanded-acceleration space, composes them into a single admissible set, and projects each sampled control to its closest feasible acceleration under an inertia-weighted metric. When a contact manifold is present, the projected controls are propagated through manifold-consistent dynamics.

Experiments on a Franka manipulator demonstrate stable surface interaction while maintaining admissible contact wrenches and stable passive-object transport under support-wrench constraints. The results show that CAMP-MPPI provides a unified mechanism for enforcing coupled kinematic and dynamics-level feasibility directly within the MPPI sampling process.

Kinematic feasibility alone does not guarantee physically admissible interaction

A motion can stay on a contact surface, or clear an obstacle, and still require a contact or support wrench outside its admissible range. Treating one side as a hard constraint and the other as a cost leaves a failure mode open.

Writing on a curved surface

The pen has to stay on the surface while the normal force stays inside its admissible band.

Partial figure-eight with thick, stamped white marks on a curved black surface — **Surface consistency only**No wrench limit, so contact loading grows and the trace is stamped and heavily pressed (PR-MPPI).

Figure-eight with over-pressed regions and faint gaps — **Wrench as a soft penalty**Loading drifts out of range, leaving over-pressed regions and faint or missing strokes (Torque-Sampling MPPI).

Carrying a tall object on a tray

The tray has to swerve around a hanging obstacle without accelerating hard enough to tip the passive object.

Tall block on a tray touching a hanging white sphere — **Not enough evasion**The object collides with the obstacle.

Tall block tipped over on the tray — **Too aggressive**The required support wrench exceeds its limit and the object tips.

One admissible set, enforced inside every rollout

CAMP-MPPI samples commanded accelerations. At every rollout state it pulls wrench limits and kinematic safety constraints back into that space, projects each sample onto their intersection, and propagates the result through manifold-constrained dynamics.

Raw samples move to the closest admissible acceleration under the inertia metric (dashed ellipse). Their weighted average stays inside the set.

CAMP-MPPI pipeline: wrench and kinematic inequalities are combined into one admissible set in acceleration space; projected accelerations are mapped onto the configuration manifold; constrained rollouts are evaluated over the horizon; a feasibility-preserving weighted update gives the executed control. — **Fig. 2**Contact-wrench and kinematic constraints are mapped into a unified admissible set in commanded-acceleration space, onto which each sampled acceleration is projected under the generalized inertia metric. Projected commands are propagated through manifold-constrained dynamics and scored by the task cost; a feasibility-preserving weighted update of the first-step commands gives the executed control.

Build one admissible set in acceleration space

At a fixed rollout state, the reaction wrench needed to stay on the contact manifold is affine in the commanded acceleration, so each polyhedral wrench limit pulls back to a linear inequality on a. Kinematic safety functions, imposed through a second-order barrier condition, reduce to the same form. Stacking both gives a single convex set.

$𝝀_{i} (𝒂) = 𝑮_{i} 𝒂 + 𝒈_{i} \in 𝒦_{i} ⟺ 𝑪_{i}^{λ} 𝒂 \leq 𝒅_{i}^{λ}$ $𝒜 (𝒒, \dot{𝒒}) = {𝒂 \in ℝ^{n} ∣ 𝑪 𝒂 \leq 𝒅}$
Project each sample, then roll out on the manifold

Each raw sample is moved to the closest admissible acceleration under the generalized inertia metric, in one joint projection rather than sequential corrections. The projected command is propagated through manifold-constrained dynamics, and the set is rebuilt at the next rollout state across the whole horizon.

$𝒂^{⋆} = \underset{𝒂}{arg min} \frac{1}{2} {(𝒂 - 𝒂^{raw})}^{⊤} 𝑴 (𝒂 - 𝒂^{raw}) s.t. 𝑪 𝒂 \leq 𝒅$ $\ddot{𝒒} = 𝒂^{⋆} + 𝑴^{- 1} 𝑱_{c}^{⊤} 𝝀_{req}, 𝝀_{req} = - 𝜦_{c} (𝑱_{c} 𝒂^{⋆} + {\dot{𝑱}}_{c} \dot{𝒒})$
Execute a feasibility-preserving weighted update

At the first step every sample starts from the same measured state, so all projected commands share one convex admissible set and their MPPI-weighted average stays inside it. Manifold consistency of the executed motion comes from the same constrained dynamics.

$𝒂_{0}^{exec} = \sum_{k = 1}^{K} w_{k} 𝒂_{0}^{k, ⋆} ⟹ 𝑪_{0} 𝒂_{0}^{exec} \leq 𝒅_{0}$

How the compared controllers treat each constraint

Controller	Manifold and kinematic constraints	Contact and support wrench
Vanilla MPPI	Rollout cost	Rollout cost
PR-MPPI	Hard, by projection	Not modeled
Torque-Sampling MPPI	Rollout cost	Soft rollout cost
CAMP-MPPI	Hard, one composite set	Hard, one composite set

Hardware experiments

Two real-world tasks, each coupling a geometric or kinematic requirement with a dynamics-level wrench limit. Every method runs 10 trials per task.

Robot: Franka Research 3
Rollouts: K = 1024 on an RTX 4070
Horizon: H = 15 writing, H = 36 tray
Control rate: 100 Hz, under 10 ms per cycle
Baselines: Vanilla, PR, Torque-Sampling MPPI

Curved-surface writing

The robot traces a figure-eight on a curved surface. It must stay on the contact manifold while the required normal wrench stays inside the admissible band around a nominal 3 N.

Clean, complete figure-eight trace — Completion 100 %, load feasibility 100 %.

Over-pressed figure-eight with faint gaps — Completion 84 %, load feasibility 88.4 %.

Partial, heavily pressed trace — Completion 39 %, load feasibility 52.6 %.

Short, faint, off-path trace — Completion 11 %, load feasibility 26.2 %.

Table I. Performance and contact-load feasibility on curved-surface writing (10 trials)

Method	Path RMSE [mm]	Manifold RMS [mm]	Load feas. [%]	Max viol. [N]	Completion [%]	Comp. time [ms]
Vanilla MPPI^†	16.11 ± 14.11	1.675 ± 10.31	26.2 ± 7.1	27.1 ± 18.9	11 ± 0.6	3.02 ± 0.02
PR-MPPI^†	0.686 ± 0.109	0.590 ± 0.098	52.6 ± 5.6	7.4 ± 18.3	39 ± 1.8	3.18 ± 0.34
Torque-Sampling MPPI^†	2.249 ± 0.380	1.211 ± 0.720	88.4 ± 3.2	2.9 ± 1.6	84 ± 6.7	4.33 ± 0.03
CAMP-MPPI	0.735 ± 0.134	1.087 ± 0.011	100.0 ± 0.0	0.0 ± 0.0	100.0 ± 0.0	6.68 ± 0.11

^† Path RMSE and manifold RMS are computed over the executed portion only, so small geometric errors from incomplete runs should be read together with completion.

Normal force over 21 seconds. After contact at about 1.5 s, the required normal wrench stays between 2 and 4 N and the measured force stays near 3 N.

The required wrench stays inside the band

After contact is established, the normal wrench required by CAMP-MPPI remains within the 2–4 N admissible band, and the measured normal force tracks the nominal 3 N with an RMSE of 0.30 N. The controller keeps the geometric contact constraint and admissible loading throughout the trace.

Fig. 4. Wrench admissibility and measured force tracking during curved-surface writing.

Tray transport past an obstacle

A tall passive object rides on a tray through an obstacle-constrained workspace. The controller needs enough evasive motion to keep its clearance, but not so much tray acceleration that the support wrench tips the object.

Hardware

Motion snapshot: the tray passes below the obstacle and the object stays upright — Upright 10, collision 0, tipped 0.

Motion snapshot: the object moves into the obstacle — Upright 0, collision 8, tipped 2.

Motion snapshot: the evasive motion tips the object — Upright 0, collision 0, tipped 10.

Motion snapshot: the object tips on the tray — Upright 0, collision 3, tipped 7.

Simulation

CAMP-MPPI

Success

Torque-Sampling MPPI

Fail

PR-MPPI

Fail

Vanilla MPPI

Fail

Table II. Support-wrench feasibility, task outcomes, and computation time on tray transport (10 trials)

Method	Wrench feas. [%]	Max viol. [mm]	Viol. time [%]	Peak \|a_h\|/a_max [%]	Upright / Coll. / Tip. [trials]	Comp. time [ms]
Vanilla MPPI	80.2 ± 2.4	41.1 ± 9.7	19.8 ± 4.0	137 ± 11.9	0 / 3 / 7	5.23 ± 0.33
PR-MPPI	82.4 ± 4.0	96.7 ± 12.4	17.6 ± 4.3	160 ± 19.6	0 / 0 / 10	5.81 ± 0.31
Torque-Sampling MPPI	86.2 ± 6.2	35.1 ± 13.5	18.8 ± 6.2	125 ± 26.1	0 / 8 / 2	6.76 ± 0.58
CAMP-MPPI	100.0 ± 0.0	0.0 ± 0.0	0.0 ± 0.0	97 ± 1.9	10 / 0 / 0	8.25 ± 0.39

Task outcomes count upright completions, collisions, and tipping failures.

Left: obstacle clearance and tipping violation over time for the four methods. Right: per-trial minimum clearance versus maximum tipping violation; only CAMP-MPPI trials fall in the region where both constraints are satisfied. — **Fig. 6**Constraint satisfaction on tray transport. Left: minimum obstacle clearance and tipping-constraint violation over time; red lines mark the admissible limits. Right: trial-level minimum clearance against maximum tipping violation. CAMP-MPPI stays in the region where both constraints hold, while the baselines violate collision avoidance, the tipping limit, or both.

Takeaway

Enforcing dynamics-level wrench admissibility directly inside the MPPI sampling process, together with geometric and kinematic constraints, removes the trade-off that soft or partial constraint handling leaves open. Future work will address contact-mode transitions, uncertainty-aware wrench constraints, and more general multi-contact interactions.

BibTeX

@inproceedings{campmppi,
  title     = {{CAMP-MPPI}: Contact-Aware Manifold Projection {MPPI} for
               Hard Dynamics-Level Contact-Wrench Constraints},
  author    = {Anonymous Authors},
  booktitle = {IEEE International Conference on Robotics and Automation (ICRA)},
  note      = {Under review},
  year      = {2027}
}

CAMP-MPPI Contact-Aware Manifold Projection MPPI for Hard Dynamics-Level Contact-Wrench Constraints

Abstract

Kinematic feasibility alone does not guarantee physically admissible interaction

Writing on a curved surface

Carrying a tall object on a tray

One admissible set, enforced inside every rollout

Build one admissible set in acceleration space

Project each sample, then roll out on the manifold

Execute a feasibility-preserving weighted update

How the compared controllers treat each constraint

Hardware experiments

Curved-surface writing

The required wrench stays inside the band

Tray transport past an obstacle

Takeaway

BibTeX