Contact-Aware Motion Planning (CAMP)

• Contact-Aware Motion Planning (CAMP) is a paradigm that models movable objects as dynamic agents by explicitly planning for contacts rather than simply avoiding collisions.
• It formulates the problem as a multi-agent trajectory optimization using complementarity constraints and an augmented Lagrangian method to enforce contact, friction, and non-penetration.
• Empirical validations in NAMO and RAMO tasks demonstrate CAMP’s superior success rates and practical execution in contact-rich, cluttered scenarios.

Contact-Aware Motion Planning (CAMP) denotes a class of motion-planning methods that treat contact as a planned, explicitly modeled interaction rather than a failure mode to be avoided. In its canonical formulation for mobile robots among movable objects, CAMP is an optimization-based paradigm in which contacts between the robot and movable objects are incorporated as complementarity constraints within a spatial–temporal trajectory optimization, and solved with an augmented Lagrangian method (ALM) as an Optimization Problem with Complementarity Constraints (OPCC) (Wang et al., 5 Feb 2025). In the broader literature, related formulations extend the same contact-aware viewpoint to humanoid loco-manipulation, quasistatic contact-aware control, safe whole-arm interaction, aerial motion-force planning, sliding-contact planning, continuum robots, and semantically conditioned acceptable-contact planning (Ciebielski et al., 16 Aug 2025, Pang et al., 2021, Li et al., 2023, Guo et al., 2024, Khoury et al., 2020, Rao et al., 2024, Mangan et al., 17 Apr 2026, Ling et al., 13 Mar 2025, Xie et al., 2023).

1. Conceptual scope and defining shift

The defining conceptual shift in CAMP is that movable objects are modeled as agents with state trajectories, not merely as static obstacles. Classical motion planners for mobile robots, including sampling-based, corridor-based, and trajectory-optimization methods such as GCOPTER, enforce collision avoidance as a hard constraint in configuration space. That produces safe collision-free trajectories, but it severely reduces reachable space in cluttered environments and cannot represent tasks in which contact is inevitable or required, such as pushing a door or moving a chair (Wang et al., 5 Feb 2025).

CAMP instead allows and plans contacts between the robot and movable objects, while still enforcing physical contact laws and non-penetration. In the formulation of "Contact-Aware Motion Planning Among Movable Objects" (Wang et al., 5 Feb 2025), contact is described through complementarity constraints that capture when contact forces are nonzero and when relative motion is constrained. This eliminates the need to pre-specify a discrete contact/no-contact mode schedule: the optimizer decides when and how contact occurs.

Two task classes motivate this formulation. In Navigation Among Movable Objects (NAMO), the robot must reach a goal while movable objects may end in arbitrary final states. In Rearrangement Among Movable Objects (RAMO), movable objects themselves have specified target states. In both cases, CAMP treats the environment as manipulable rather than immutable, which expands the effective reachable space of the robot (Wang et al., 5 Feb 2025).

A common misconception is that contact-aware planning simply relaxes collision checking. The formulation does not permit arbitrary collision. Contact is allowed only with modeled movable objects, subject to quasi-static contact mechanics, friction, dynamics, and non-penetration, while collisions with static obstacles remain constrained through signed-distance-style conditions (Wang et al., 5 Feb 2025).

2. Mathematical formulation

CAMP formulates the robot and movable objects as a multi-agent trajectory optimization. Let there be $N$ movable objects in an $n$-dimensional planar or spatial workspace; the robot plus these objects form $N+1$ agents. The decision variables are discrete waypoint states $\mathbf{q}$ and segment durations $\mathbf{T}=[t_1,\dots,t_M]$, assembled as

$$\mathbf{x} = [\mathbf{q}, \mathbf{T}]^T \in \mathbb{R}^{s \cdot (M+1) + M},$$

where $s = n \cdot (N+1)$ is the state dimension per time sample for all agents (Wang et al., 5 Feb 2025).

Trajectory representation uses MINCO-style polynomial trajectories. For one scalar component of the $i$-th segment,

$$\boldsymbol{c}_i(t) = \sum_{j=0}^{2k-1} c_{i,j} (t - T_{i-1})^{j},$$

with continuity enforced up to derivative order $k$. This parameterization reduces optimization to waypoints and segment times while retaining continuous, differentiable trajectories (Wang et al., 5 Feb 2025).

Movable objects are unactuated, and CAMP assumes a quasi-static, low-speed regime. Their motion is induced by contact forces and possibly drag, with dynamics written as

$n$0

where $n$1 is the mass matrix, $n$2 contact impulse or force magnitudes, and $n$3 is derived from non-penetration gap functions $n$4 (Wang et al., 5 Feb 2025).

Contact and friction are encoded through complementarity. For two objects $n$5 and $n$6, the formulation uses nonpositive inequalities for tangential relative velocity magnitude and contact friction force magnitude, together with a complementarity equality

$n$7

Intuitively, if tangential relative velocity is nonzero, friction force magnitude drops to zero; if friction force is nonzero, relative tangential velocity must be zero (Wang et al., 5 Feb 2025).

Collision constraints are split into agent–agent and agent–static-obstacle terms. CAMP uses GJK distances on convex hulls for inter-agent separation and ESDF values for static obstacles. Velocity and acceleration limits are enforced through

$n$8

Boundary conditions unify NAMO and RAMO through

$n$9

where the mask matrix $N+1$0 selects which terminal states are fixed (Wang et al., 5 Feb 2025).

The overall optimization minimizes an energy-like integral in the $N+1$1-th derivative, a duration penalty $N+1$2, and a task-specific term $N+1$3, subject to boundary, collision, state-bound, dynamics, and complementarity constraints. This is the OPCC at the core of CAMP (Wang et al., 5 Feb 2025).

3. Solution strategy and planning pipeline

Because complementarity constraints typically violate standard constraint qualifications such as LICQ, CAMP uses an augmented Lagrangian method rather than relying on conventional constrained solvers. Equality constraints $N+1$4 and inequality constraints $N+1$5 are folded into an augmented Lagrangian

$N+1$6

with quadratic penalty terms for equalities and $N+1$7 penalties for inequalities (Wang et al., 5 Feb 2025).

Each ALM iteration performs unconstrained minimization of $N+1$8 with L-BFGS quasi-Newton and Lewis–Overton line search, updates dual variables, and increases the penalty parameter when needed. Feasibility and optimality are checked through infinity-norm gradient and constraint residual criteria. Dense samples along the continuous trajectory are used to evaluate $N+1$9 and $\mathbf{q}$0, and gradients are obtained by chain rule from polynomial coefficients to waypoints and times (Wang et al., 5 Feb 2025).

CAMP is not only a back-end optimizer. It also includes a front-end path-search stage that supplies initial trajectories. In NAMO, a semantic map $\mathbf{q}$1 is masked so that movable objects are treated as free space in $\mathbf{q}$2, and a robot-only A* path $\mathbf{q}$3 is computed from start to goal. Movable objects are initially fixed at their initial states in the initialization. In RAMO, a high-level action set $\mathbf{q}$4 determines which objects are contacted and in what order; object trajectories are segmented accordingly using contact offsets $\mathbf{q}$5 and segment indices $\mathbf{q}$6 (Wang et al., 5 Feb 2025).

This two-stage structure clarifies another misconception: CAMP is not purely a local contact-implicit optimizer. In the reported implementation, discrete search initializes the trajectory, while the continuous back end resolves dynamics, timing, collision constraints, and contact complementarity (Wang et al., 5 Feb 2025).

4. Task regimes and empirical validation

The principal reported validation concerns NAMO and RAMO. For NAMO, seven scenarios from S7M1 to S1M7 were used, varying the number of movable objects from $\mathbf{q}$7 to $\mathbf{q}$8, with 20 random maps per scenario and robot start and goal fixed at $\mathbf{q}$9 and $\mathbf{T}=[t_1,\dots,t_M]$0. This yields 140 maps evaluated for each method. The average success rate is approximately $\mathbf{T}=[t_1,\dots,t_M]$1 for CAMP versus approximately $\mathbf{T}=[t_1,\dots,t_M]$2 for the GCOPTER baseline. As the number of movable objects increases, CAMP’s success rate improves and exceeds $\mathbf{T}=[t_1,\dots,t_M]$3 when $\mathbf{T}=[t_1,\dots,t_M]$4, whereas GCOPTER frequently fails in congested maps because it cannot exploit movability (Wang et al., 5 Feb 2025).

The computational profile reflects the price of contact reasoning. The front-end search time is about $\mathbf{T}=[t_1,\dots,t_M]$5 ms on average. The back-end optimization time is about $\mathbf{T}=[t_1,\dots,t_M]$6 s on average, with worst-case tasks around $\mathbf{T}=[t_1,\dots,t_M]$7 s. GCOPTER is faster in the back end, around $\mathbf{T}=[t_1,\dots,t_M]$8 s mean, but substantially less capable in cluttered contact-rich scenarios (Wang et al., 5 Feb 2025).

For RAMO, scenarios include four static obstacles and one movable object, either a cylinder or a cube, with object start $\mathbf{T}=[t_1,\dots,t_M]$9 and target $$\mathbf{x} = [\mathbf{q}, \mathbf{T}]^T \in \mathbb{R}^{s \cdot (M+1) + M},$$0. The cylinder can move in any direction; the cube only along the $$\mathbf{x} = [\mathbf{q}, \mathbf{T}]^T \in \mathbb{R}^{s \cdot (M+1) + M},$$1- and $$\mathbf{x} = [\mathbf{q}, \mathbf{T}]^T \in \mathbb{R}^{s \cdot (M+1) + M},$$2-axes. CAMP reports $$\mathbf{x} = [\mathbf{q}, \mathbf{T}]^T \in \mathbb{R}^{s \cdot (M+1) + M},$$3 success for cylinder RAMO and $$\mathbf{x} = [\mathbf{q}, \mathbf{T}]^T \in \mathbb{R}^{s \cdot (M+1) + M},$$4 for cube RAMO. No collision-free baseline is given because classical planners do not natively solve RAMO with physical contact (Wang et al., 5 Feb 2025).

Real-world experiments use an omnidirectional mobile base equipped with curved acrylic panels for predictable contact surfaces and yellow LEDs outlining the contour for visual detection; trajectories are measured with a motion capture system. In a NAMO-like narrow-passage scenario, GCOPTER plans a long collision-free detour, whereas CAMP pushes the blocking object aside and takes a shorter path. In RAMO scenarios, CAMP generates contact-rich trajectories for both cylinder and cube objects, and executed trajectories match planned ones closely (Wang et al., 5 Feb 2025).

Tracking-error summaries further support executability. For the robot, mean position error is approximately $$\mathbf{x} = [\mathbf{q}, \mathbf{T}]^T \in \mathbb{R}^{s \cdot (M+1) + M},$$5, standard deviation approximately $$\mathbf{x} = [\mathbf{q}, \mathbf{T}]^T \in \mathbb{R}^{s \cdot (M+1) + M},$$6–$$\mathbf{x} = [\mathbf{q}, \mathbf{T}]^T \in \mathbb{R}^{s \cdot (M+1) + M},$$7, and maximum approximately $$\mathbf{x} = [\mathbf{q}, \mathbf{T}]^T \in \mathbb{R}^{s \cdot (M+1) + M},$$8–$$\mathbf{x} = [\mathbf{q}, \mathbf{T}]^T \in \mathbb{R}^{s \cdot (M+1) + M},$$9. Mean velocity error is approximately $s = n \cdot (N+1)$0–$s = n \cdot (N+1)$1. For movable objects in RAMO, mean position error is approximately $s = n \cdot (N+1)$2–$s = n \cdot (N+1)$3, with maxima approximately $s = n \cdot (N+1)$4–$s = n \cdot (N+1)$5, and mean velocity error approximately $s = n \cdot (N+1)$6–$s = n \cdot (N+1)$7. The reported interpretation is that these errors are acceptable for contact-rich manipulation with passive object dynamics (Wang et al., 5 Feb 2025).

5. Relation to adjacent contact-aware paradigms

CAMP has since appeared in several adjacent forms. In humanoid loco-manipulation, contact modes themselves become symbolic states and actions. "Task and Motion Planning for Humanoid Loco-manipulation" formulates an integrated TAMP problem over symbolic contact states $s = n \cdot (N+1)$8, symbolic actions $s = n \cdot (N+1)$9, continuous state $i$0, control $i$1, and phase durations $i$2, with Monte Carlo Tree Search over contact-mode sequences and whole-body trajectory optimization under centroidal dynamics, friction constraints, and actuation limits (Ciebielski et al., 16 Aug 2025). This is mode-explicit rather than complementarity-based, but it preserves the CAMP premise that contact is part of the decision space.

A second line emphasizes execution rather than global planning. "Easing Reliance on Collision-free Planning with Contact-aware Control" proposes a quasistatic QP controller that continues to execute a trajectory despite unexpected collisions while enforcing inequality bounds on contact forces, and demonstrates smoother contact release than null-space projection on a KUKA iiwa arm (Pang et al., 2021). "Embracing Safe Contacts with Contact-aware Planning and Control" couples a contact-aware controller with a BiTRRT extension that reasons about elastic-band deformation and safe whole-arm contact, with soft and rigid contacts handled by a quasi-static model and online stiffness estimation (Li et al., 2023). These works suggest that CAMP can reside at the execution layer, the global-planning layer, or both.

Other formulations specialize contact-awareness to distinct embodiments and contact models. "Flying Calligrapher" plans tangential motion trajectories and time-varying normal force for aerial contact tasks using a contact-aware trajectory optimizer and hybrid motion-force control, reporting an IoU of $i$3 and end-effector position and force RMSE of $i$4 cm and $i$5 N (Guo et al., 2024). "Efficient Sampling of Transition Constraints for Motion Planning under Sliding Contacts" introduces sliding-contact constraints and a three-mode transition sampler for fixed/sliding/no-contact planning on mesh surfaces (Khoury et al., 2020). "Towards Contact-Aided Motion Planning for Tendon-Driven Continuum Robots" uses contact to induce multiple effective curvatures in a single-segment TDCR and reports about $i$6 success over 525 queries, while baselines do not exceed $i$7 (Rao et al., 2024). "Contact-Aware Planning and Control of Continuum Robots in Highly Constrained Environments" evaluates contact quality in vascular navigation, reports $i$8 hardware success in three anatomical environments, and shows that penalizing end-of-continuum-segment contact improves manipulability (Mangan et al., 17 Apr 2026). "Robust Model-Based In-Hand Manipulation with Integrated Real-Time Motion-Contact Planning and Tracking" combines contact-implicit MPC at a high level with tactile force-motion tracking at a low level for dexterous in-hand manipulation (Jiang et al., 8 May 2025).

A separate strand recasts CAMP in semantic terms. "Language-Conditioned Path Planning" learns a language-conditioned collision function $i$9 that distinguishes desired or acceptable contact from forbidden contact (Xie et al., 2023), while IMPACT uses a Vision-LLM to infer dense 3D contact-tolerance cost maps and reports improved success in cluttered settings over 3620 simulation and 200 real-world trials (Ling et al., 13 Mar 2025). These works do not model contact mechanics in the style of OPCC, but they extend the contact-aware premise by differentiating contacts semantically rather than only geometrically or dynamically.

The acronym itself is not unique. In wireless-connected robotic networks, CAMP denotes communication-aware motion planning rather than contact-aware motion planning (Zhang et al., 2015). The two usages are unrelated except at the level of shared emphasis on task-relevant environment interaction.

6. Limitations, misconceptions, and open directions

The principal limitations of the optimization-based CAMP formulation follow directly from its modeling assumptions. The 2025 mobile-robot formulation assumes quasi-static contact, Coulomb friction, rigid bodies, and simplified limit surfaces, and therefore may not capture high-speed dynamics, impacts, deformable contacts, or complex compliance. Many agents and contact points increase the optimization dimension and the number of complementarity constraints, which makes ALM with dense sampling computationally heavy for large-scale problems or real-time requirements. The approach also assumes a known semantic map distinguishing static and movable objects, so unstructured environments with unknown inertia or friction can reduce predictive accuracy (Wang et al., 5 Feb 2025).

A second misconception is that CAMP automatically subsumes all contact-rich planning. The literature instead shows several design choices. Some methods are contact-implicit and continuous, such as OPCC solved by ALM (Wang et al., 5 Feb 2025). Others are mode-explicit and symbolic, as in humanoid loco-manipulation (Ciebielski et al., 16 Aug 2025). Some focus on control under contact uncertainty (Pang et al., 2021), some on safe whole-arm interaction (Li et al., 2023), some on motion-force coupling (Guo et al., 2024), and some on semantic tolerances rather than mechanics (Ling et al., 13 Mar 2025, Xie et al., 2023). A plausible implication is that CAMP is best understood as a family of planning and control formulations unified by explicit contact reasoning, not by a single mathematical template.

The forward-looking themes in the cited work are consistent. For optimization-based CAMP, the reported directions include improving friction and contact models beyond quasi-static assumptions, enhancing scalability with more advanced OPCC solvers or warm-start strategies, and extending from mobile robots to manipulators and legged systems (Wang et al., 5 Feb 2025). Related work suggests richer whole-body dynamics with actuation constraints (Ciebielski et al., 16 Aug 2025), tighter integration of planning and tactile feedback (Jiang et al., 8 May 2025), perceptive or uncertain environments (Ciebielski et al., 16 Aug 2025), and learning-based contact-model adaptation or semantic guidance (Wang et al., 5 Feb 2025, Ling et al., 13 Mar 2025). Taken together, these directions suggest a trajectory from contact-aware motion planning as a specialized optimization trick toward contact-aware autonomy as a general principle for robots operating in crowded, deformable, and semantically structured environments.