Global Planning for Contact-Rich Manipulation via Local Smoothing of Quasi-dynamic Contact Models

In plain words

Robots that touch objects make the physics jump: a finger either pushes a block or it does not, and small changes in position can flip that. Planners that rely on local slopes struggle with such jumps, and reinforcement learning copes partly because it averages over many random tries, which blurs the jumps into a smooth average.

This work asks what that averaging buys, and shows that the same blurring can be computed in two ways: by sampling many nearby tries, or by solving a slightly relaxed version of the contact problem once. The authors also use a contact model that is convex and ignores motion inside each step, which makes the problem easier to solve and to differentiate. With the blurred slopes in hand, a classic random-tree planner can search for long sequences of pushes and grasps, and it finds plans in seconds to minutes on a desktop CPU.

TL;DR

• The contact model is convex, differentiable and quasi-dynamic: it keeps only the steady-state contact forces and drops velocity §3.1 §3.2.
• Two ways to smooth the non-smooth contact dynamics give local linear models: randomized smoothing by sampling, and analytic smoothing through a log-barrier. The authors argue they are equivalent for simple systems and behave alike on complex ones §2.3 §4.3.
• Smoothed models make an RRT usable for contact-rich tasks: a Mahalanobis distance guides nearest-node search, and contact sampling adds nodes that touch the object §6 §7.
• Author-reported: on three trajectory-optimization tasks the analytic and first-order randomized variants beat exact gradients on two tasks tab. 2. The RRT plans six tasks, with runs of 3 to 168 s tab. 3.
• The official code is MIT-licensed. Its planners differ from the paper in several details (see "Paper vs code"). The contact simulator it depends on is in a separate repository.

Problem

Planning through contact has two obstacles. First, contact dynamics are hybrid: many smooth modes separated by switching surfaces. A linear model built from the gradient at one point stops being valid a short distance away, which hurts both gradient-based optimisers and distance-based sampling planners §1. Second, the number of contact modes grows very fast with the number of contacts, so enumerating modes does not scale §1. Model-based methods have smoothed the dynamics analytically, while reinforcement learning has smoothed them by sampling; the paper asks what each route gives §1. Second-order dynamics add transients that make local linearisations myopic over long horizons, so the authors use a quasi-dynamic, convex model §1 §3.1 §10.

Method

Overview. Section numbers in this card are the \section counts in the arXiv source (IEEE prints them as Roman numerals). The paper's contribution has three parts: (1) a smoothing theory with two computation schemes §2 §4; (2) a convex quasi-dynamic contact model whose smoothed linearisations feed (3) two planners: iterative MPC §5 and an RRT with a local metric §6 §7.

1. Smooth surrogate. For a non-smooth map f and a zero-mean elliptical density ρ, the surrogate f_ρ(x) = E_w[f(x + w)] is C¹ when ρ is C¹ §2.1 [Lemma 1 in §2.1]. Table 1 lists common pairs: ReLU with a logistic density gives Softplus, Heaviside with a Cauchy density gives ArcTan, and Heaviside with a logistic density gives Sigmoid tab. 1 §2.1.

   f	ρ	f_ρ
   ReLU	Logistic	SoftPlus
   Heaviside	Cauchy	ArcTan
   Heaviside	Logistic	Sigmoid

2. Linearisation of the surrogate. The model is the affine map g(x; x̄) = J(x̄)(x − x̄) + μ(x̄). Its parameters are J_ρ, the gradient of f_ρ, and μ_ρ = f_ρ(x̄) §2.1 §2.2. Three estimators are given:

   • Analytic: differentiate a closed-form f_ρ when one exists §2.3.
   • First-order randomized: average the Jacobians of f at sampled points. This needs f to be locally Lipschitz §2.3.
   • Zeroth-order randomized: a score-function (Stein) estimate that uses only function values and works for discontinuous f, with higher variance §2.3 [Lemma 3 in §2.1]. Theorem 1 §2.2 shows that, for a Gaussian residual distribution, the least-squares affine fit of f over samples converges to the exact linearisation of f_ρ. The same holds for the dynamics with state and input perturbations §2.4.

3. Quasi-dynamic convex contact model (CQDC). Robot joints are impedances modelled as springs with diagonal stiffness K_a. The input u is the commanded joint position. The object's force balance has a regulariser ε that interpolates between exact quasi-static (ε = 0) and Mason's quasi-dynamic model (ε = 1), with step h §3.2. The next configuration is q_+ = q + δq*, where δq* is the solution of a convex second-order cone program. Its objective is ½ δqᵀQ δq + bᵀδq, with Q = blkdiag(ε M_u / h, h K_a) and b = −h[τ_u; K_a(u − q_a) + τ_a]. The constraints are Anitescu's relaxation of Coulomb friction, one cone per contact, and a signed-distance term φ_i §3.2. For planar systems the cones reduce to two linear inequalities per contact, giving a QP §3.2 §6.2 Example 5.

4. Derivatives. The linearisation is A = I + ∂δq*/∂q and B = ∂δq*/∂u. Both come from the implicit function theorem applied to the KKT conditions of the SOCP, with the problem data differentiated by automatic differentiation §3.3. Raw derivatives are discontinuous, so they are averaged over samples (first-order) or taken from the smoothed problem (analytic) §3.3 §4.2.

5. Projection view of penetration. Samples that penetrate an object are not rejected. The dynamics from an infeasible configuration are defined as the projection onto the feasible set, with distance measured by the quadratic cost. Averaging projected samples creates a "stochastic force field" that pushes the object off the surface §4.1 [Example 3 in §4.1].

6. Analytic smoothing (log-barrier). The hard constraints of the SOCP are replaced by −(1/κ) Σ_i log[ (J_n dq + φ)²/μ² − (J_t dq)ᵀ(J_t dq) ], where κ is a barrier weight. The solution converges to the SOCP solution as κ → ∞. The smoothed problem has only a stationarity condition, and the authors solve it with an in-house Newton method §4.2. The barrier acts like a force that grows as the object nears a constraint boundary, and satisfies β(q_a + dq_a) = 1/κ, similar to a bilinear complementarity relaxation §4.3 Example 4.

7. Equivalence of the schemes. For a 1-D frictionless cart against a wall, the analytic solution equals randomized smoothing with an elliptical density that has heavy tails, with σ = h k_a κ §4.3 Example 4. For the frictional examples, the authors compare the two schemes numerically and report qualitatively similar results, without a proof §4.3 fig. 5.

8. Iterative MPC (iMPC). The planner rolls out the nominal inputs, linearises the dynamics, then solves a truncated MPC at every time step with a trust region, applying the first input and advancing the nominal state with the true dynamics. Algorithm 1 repeats this until convergence §5.1 alg. 1. The MPC cost is a quadratic tracking cost on states plus an input penalty §5.1.

   PseudocodeCopy

   # iMPC: iterative MPC with smoothed linearisations  [alg. 1] [§5.1]
   # Input:  initial state x0, nominal inputs u_bar (T, dim_u)
   # Output: optimised inputs u_bar (T, dim_u)
   
   def impc(x0, u_bar, T):
       # Roll out the initial guess to get nominal states  [alg. 1]
       x_bar = rollout(x0, u_bar)                  # (T + 1, dim_x)
       while not converged(x_bar, u_bar):
           # Linearise all steps around the nominal trajectory  [alg. 1]
           A, B, c = smoothed_linearisations(x_bar, u_bar)  # [§4]
           for j in range(T):
               # Solve the truncated, trust-region MPC from x_bar[j]
               # and keep the first input  [§5.1]
               u_bar[j] = mpc(x_bar[j], A[j:], B[j:], c[j:])
               # Advance the nominal state with the true dynamics  [§5.1]
               x_bar[j + 1] = f(x_bar[j], u_bar[j])
       return u_bar

9. Local Mahalanobis metric. For a nominal configuration q̄ with input q̄_a, the local model is q_+ = B δu + c, with δu = u − q̄_a. The distance of a query q from q̄ is the Mahalanobis distance to c with covariance Σ_γ = B Bᵀ + γI §6.1. The sub-level set is an ellipsoid whose axes are the directions the object can reach from q̄. A query that lies outside the range of B is far away, and the distance is dominated by 1/γ §6.1. For contact-rich systems the metric uses the smoothed B_ρ and c_ρ, and only the unactuated (object) rows §6.2 fig. 8.

10. RRT with smoothing. The RRT of alg. 2 is changed in three places.

    • Nearest uses the smoothed object metric d^u_{ρ,γ} instead of a uniform Euclidean distance §7.1.
    • Extend takes one step only. It solves a least-squares problem for the input that moves the object toward the subgoal, using the smoothed B_ρ^u, and then rolls out the true dynamics §7.2 alg. 3.
    • ContactSample, used in place of Extend with some probability, fixes the object and sets the robot to a contact configuration: inverse kinematics for planar pushers and the planar hand, and an EigenGrasp-based closing motion for the Allegro hand §7.3. After the tree reaches the goal, the plan is shortened and refined: contact segments are re-optimised with iMPC on a finer step, and gaps are joined by a collision-free RRT §7.5.
    PseudocodeCopy

    # Single-step Extend with the smoothed object model  [alg. 3] [§7.2]
    # Input:  parent node, subgoal (full configuration)
    # Output: child configuration
    
    def extend(parent, subgoal, step):
        # Least-squares input that moves the object toward the subgoal
        # using only the object rows of the smoothed B  [alg. 3 line 1]
        delta_u = lstsq(
            parent.B_u,
            subgoal_u - parent.c_u,
        )
        # Fixed-length step along the unit direction  [alg. 3 line 2]
        delta_u = delta_u / norm(delta_u)
        u_new = parent.u_bar + step * delta_u
        # The actual, non-smoothed dynamics give the child  [§7.2]
        return f(parent.q, u_new)

    PseudocodeCopy

    # Nearest node under the local Mahalanobis metric  [§6.2] [§7.1]
    # Input:  subgoal (object coordinates), tree nodes, gamma
    # Output: index of the nearest node
    
    def nearest(subgoal_u, nodes, gamma):
        best_index = None
        best_distance = inf
        for i, node in enumerate(nodes):
            # Covariance from the smoothed object rows of B  [§6.2]
            cov_u = node.B_u @ node.B_u.T + gamma * eye
            error = subgoal_u - node.c_u
            distance = error @ inv(cov_u) @ error
            if distance < best_distance:
                best_index = i
                best_distance = distance
        return best_index

Hyperparameters

Most numerical values are not stated in the paper. The source has no appendix, and the values below from the code are shown as code defaults or example settings, not as paper values.

Name	Value	Where	Notes
Samples per linearisation (first-order, iMPC)	100	§5.2	parallel on all threads; code default irs_mpc_params.py
iMPC outer iterations	20	tab. 2 caption	planar hand script 20 run_impc2.py; Allegro script 15 run_impc2.py
Trials per method	5	tab. 2 fig. 6	mean and std in fig. 6; std not in tab. 2
Hardware	16-core CPU, 32 GB RAM	§5.2	desktop, all CPU
Iteration budget, RRT	1000	tab. 3 caption	planar hand example max_size 1000 run_rrt.py
Horizon T	not stated		planar hand, planar pushing and Allegro scripts all use T = 20 run_impc2.py
Time step h	not stated in paper		planar hand h = 0.1 planar_hand_setup.py; Allegro h = 0.01 run_impc2.py
Log-barrier weight κ, initial	not stated	§4.2	planar hand 500 run_impc2.py; planar pushing 100 run_planar_pushing2.py; Allegro 200 run_impc2.py
Log-barrier weight κ, final	not stated	§4.2	planar hand 1000 at iteration T run_impc2.py; Allegro 6000 over 15 iterations run_impc2.py
Randomized std schedule	not stated	§4.1	examples start at 0.2 and divide by i^0.8 run_impc2.py
iMPC cost weights Q, Q_final, R	not stated	§5.1	planar hand Q = (10, 1e-3), Q_final = 100 Q, R = 10 run_impc2.py
iMPC trust region	not stated	§5.1	planar hand inputs bounded to ±2h around the nominal run_impc2.py
Quasi-dynamic regulariser ε	"as small as possible"	§3.2	value not stated; the simulator is not in this repo
Metric regularisation γ	10⁻⁶ in fig. 8	fig. 8	class default 1e-6 rrt_params.py; planar hand RRT 1e-4 run_rrt.py
Extend step ε	not stated	§7.2	planar hand 0.35 run_rrt.py; class default 0.3 rrt_params.py
Goal-as-subgoal probability	not stated	§7	planar hand 0.3 run_rrt.py; class default 0.5 rrt_params.py
Termination tolerance	not stated		planar hand 0.01 run_rrt.py
ContactSample probability	not stated	§7.3	planar hand RRT script sets it to 0 run_rrt.py
Randomized std for RRT samples	not stated		class default 0.1 rrt_params.py
Randomized samples for RRT	not stated		class default 100 rrt_params.py
Analytic barrier weight in RRT	not stated		planar hand 100 run_rrt.py

Results

Author-reported. Setup: iMPC on three tasks with 20 iterations, five trials, cost = minimum over iterations, and a 16-core desktop §5.2 tab. 2. The RRT runs use six tasks with 1000 iterations §8.1 tab. 3. No success rates are reported.

Trajectory optimisation (iMPC), minimum cost and wall-clock time tab. 2:

Method	PlanarPushing cost	Time (s)	PlanarHand cost	Time (s)	AllegroHand cost	Time (s)
Analytic (A.)	11.74	2.17	26.55	5.20	5.78	19.59
Randomized first-order (RF.)	11.73	4.64	17.87	23.09	8.42	40.07
Randomized zero-order (RZ.)	12.86	4.61	18.29	11.93	28.21	34.05
Exact (no smoothing)	31.64	1.88	18.49	5.91	44.68	12.92

The paper says smoothing beats exact gradients on PlanarPushing and AllegroHand, while PlanarHand is solved well by exact gradients too §5.3. Among the smoothing schemes, zero-order is the weakest on PlanarPushing and AllegroHand §5.3 tab. 2. Randomized smoothing runs about 2 to 3 times slower than analytic with N = 100 and 32 threads §5.3.

RRT planning, running time in seconds for 1000 iterations tab. 3:

Method	PlanarPushing	PlanarHand	AllegroHand	AllegroPlate	AllegroPen	AllegroDoor
Analytic (A.)	3.25	10.32	31.01	117.16	24.84	12.42
Randomized first-order (RF.)	7.50	21.34	80.12	161.64	86.12	34.55
Randomized zero-order (RZ.)	7.40	21.00	82.00	168.11	81.23	33.80

The RRT figures fig. 11 plot the distance to goal and a packing ratio over iterations, averaged over five runs. The text reports, without numbers, that the Exact linearisation performs much worse than any smoothing scheme on all experiments, that the Euclidean "Global" metric is much worse than the local metric, and that contact sampling is essential on the planar pusher and planar hand §8.2. The paper claims that analytic and randomized smoothing perform alike on most tasks, with analytic faster §8.2.

Headline claims: the Figure 1 caption puts each online plan at roughly one minute of wall-clock time fig. 1 §10; the approach is comparable to RL with much less computation [abstract] §10. The conclusion contrasts this with RL tools that use offline computation of hours or days §10. No RL numbers are given in the paper.

Sim-to-real §9: plans are run open loop in Drake and on an iiwa bimanual bucket rig. 2-D tasks show small mean error. The 3-D Allegro tasks show larger error, and the paper attributes the gaps to a phase lag and missed contacts §9.2 fig. 12 fig. 13. No numbers are given in the text.

Paper vs code

Repository: github.com/pangtao22/planning_through_contact, owned by author Tao Pang and linked from the paper's project page. MIT-licensed, commit 74e4559. The readme says the contact simulator (quasistatic_simulator, branch tro2023) is a separate repository, so the CQDC solver, ε, and the smoothing routines in C++ are not in this checkout readme.md.

• iMPC linearisation timing. Paper Alg. 1 linearises all steps once per outer iteration before the inner loop alg. 1. The code computes A_t, B_t, c_t inside the inner loop, after each step, using the newly rolled-out state, so later steps see partly updated matrices irs_mpc.py.

• Input penalty. The paper penalises ‖u_t‖²_R §5.1. The code penalises input increments u_t − u_{t−1}, with u_{−1} taken from the initial state irs_mpc.py.

• Offset c_ρ. The paper's c_ρ is the expectation of the smoothed next state §2.4. When rollout_forward_dynamics_mode is set, the code takes c from the unsmoothed nominal dynamics instead irs_mpc.py. All three example scripts set this option run_impc2.py.

• Smoothed A. Default use_A = False replaces the smoothed A with a structural matrix: identity, with actuated columns zeroed, and B's actuated rows set to identity irs_mpc.py. The planar pushing script sets use_A = True run_planar_pushing2.py.

• Mode per example. The paper does not say which smoothing mode each example used. The planar hand script uses zero-order randomized (k0Pyramid) run_impc2.py, planar pushing uses first-order randomized run_planar_pushing2.py, and the Allegro script uses analytic (k1AnalyticIcecream) run_impc2.py.

• Barrier schedule. The paper increases κ every iteration without a formula §5.1. The code uses κ_i = κ_0 (κ_f/κ_0)^{i/T}, with T the horizon in the planar hand script run_impc2.py, and divides by the iteration budget in the Allegro script run_impc2.py.

• Iteration counts. Table 2 uses 20 iterations for every method tab. 2. The Allegro script runs 15 run_impc2.py and the planar pushing script 10 run_planar_pushing2.py.

• Initial guess. The paper uses a constant input trajectory §5.2. The planar hand script uses a constant enveloping grasp, not q_a0 directly run_impc2.py.

• Extend step. Paper Alg. 3 always moves a fixed ε along δu* (the cap in the source is commented out) alg. 3. The code moves min(‖δu*‖, ε) [code: irs_rrt/irs_rrt_projection.py#L154-L157].

• Metric constant. The paper defines d = ½ (q − μ)ᵀ Σ⁻¹ (q − μ) §6.1. The code drops the ½ irs_rrt.py. This does not change the nearest node, but it changes any numeric threshold on the distance.

• Zero-order RRT cannot run as shipped. populate_node_parameters compares the smoothing mode with a set using ==, so the zero-order branch is never taken irs_rrt.py. The zero-order RRT routine is marked "buggy" and raises reachable_set.py. Table 3's RZ row cannot be reproduced from this code.

• Contact sampling off in the planar hand RRT script. grasp_prob = 0 disables ContactSample run_rrt.py. The paper's ablations say contact sampling is essential on the planar hand §8.2.

• Termination metric. The docstring says termination uses the global metric, but the code uses rrt_params.distance_metric, which is local_u in the planar hand script rrt_base.py irs_rrt.py.

• RRT state sampling. The RRT samples inputs only. The state is tiled from the node reachable_set.py. The paper describes a low-variance state distribution §4.1.

• Consistent. Extend rolls out the true dynamics, as the paper says irs_rrt_projection.py §7.2. The metric uses the smoothed object rows only irs_rrt.py.

Pitfalls

• Smoothing is not free. Randomized smoothing needs N = 100 full dynamics solves per linearisation in this setup, while analytic needs one §5.3. Randomized smoothing with the first-order (Jacobian) estimator fails at discontinuities, because the Jacobian of a Heaviside is zero almost everywhere §2.3 [Example 2 in §2.3]. Zero-order estimates are unbiased but noisier, which shows in AllegroHand tab. 2.

• Penetrating samples define the dynamics. A sampled state inside an object is projected to the surface by the same SOCP solve; no extra check is needed §4.1. Skipping that and rejecting such samples changes the smoothed model.

• Stochastic force field. Averaging projected samples pushes the object away from walls even when the exact dynamics would not. A planner that reads the smoothed gradient will see this force §4.1 Example 3.

• The analytic κ has no value. The paper gives only the relation β(q + δq) = 1/κ and says κ is chosen by "physical intuition" §5.1 §4.3. The example values in code (100 to 500 initially) are the only guide.

• Equivalence is not proven for complex systems. The formal equivalence is for a 1-D frictionless cart §4.3 Example 4. Frictional examples are compared numerically fig. 5 and the paper says subtle differences remain §4.3.

• Epsilon must be small, but how small is unstated. ε sets the quasi-dynamic character. The paper says to take it "as small as possible without causing numerical issues" §3.2. The solver in code is not in this repository.

• Metric singularity. Σ_γ = B Bᵀ + γI is singular when γ is zero and B loses rank. The paper says the sub-level set then degenerates §6.1. The code takes a plain matrix inverse of this covariance, so the regulariser should stay positive reachable_set.py.

• Contact sampling teleports. ContactSample moves the robot to a new configuration without a path. The paper says this is acceptable only when the object stays in equilibrium while the robot moves §7.3. The plans are therefore not directly executable.

• Plans are open loop. Quasi-dynamic plans lag second-order execution and can drop the object. The paper says tracking needs a feedback controller §9.2.

• Costs hidden by the table. Table 2 reports the minimum over iterations, not the final cost, and gives no spread tab. 2. Table 3 gives no spread either. The RRT plots show averages over five runs fig. 11.

• Wall-clock claims. "Order of a minute" is the paper's phrase fig. 1 §10. Table 3 shows 3 to 168 s per run, and the paper has no RL timing to compare with.

• Citations. Equations are cited by section, not by number, because the equation numbering was not checked against the compiled PDF. Algorithms alg. 1 to alg. 3 are numbered in the source's float order.

• Source version. The text is arXiv v2 (2023-02-28). The paper was first posted 2022-06-22. The code commit 74e4559 is from the repository as cloned.

Lineage

• Builds on the iMPC method of the authors' earlier work "Bundled Gradients through Contact via Randomized Smoothing" (Suh, Pang and Tedrake, RA-L) §5.1. That paper has no card here, so it is named, not summarised.
• Related to reachability-guided sampling for planning under differential constraints (Shkolnik, Walter and Tedrake, ICRA 2009) §6. No card here.
• Relaxation of Coulomb friction follows Anitescu (2006) §3.2. No card here.
• Compared against reinforcement learning in the abstract and conclusion, with no numbers given.
• Superseded by: not known from content already in this wiki.

Related

ConceptTrajectory optimizationChoosing a robot motion over time that minimises a cost while respecting limits. Four variants: a GPU spline optimiser with torque limits in the loop, a multirotor MPC tracker, smoothed contact models for iterative MPC and RRT, and sampling-based joint grasp and motion optimisation. They differ in which limits sit inside the loop and how many candidates they optimise.ConceptRandomized smoothingRandomized smoothing averages a function over random noise. Used as a classifier, it gives certified predictions against bounded training-set or test-time perturbations, at low author-reported accuracy on image tasks. Used on a contact model, it gives usable slopes for planning, with no certificate. Numbers are author-reported.ConceptDifferentiable simulationA simulator whose gradients drive a fit or a training loss. PhysDreamer fits stiffness to a generated video. Physics-aware training takes gradients from a digital model of a real device. TrafficSim trains a learned rollout. Smoothed contact models feed planners. Choose by whether you can simulate the system or must measure it. Results are author-reported, from four cards.ConceptMahalanobis distanceA distance using the inverse covariance, so correlated or differently scaled features are not treated alike. Four uses: neighbour choice before a local clinical model, two OOD scores (labelled class centroids as in ViM; unlabelled self-supervised features as in SSD), and nearest-node search in contact-rich motion planning.Paper · 2026-05-06Evidence-based anomaly detection in clinical domainsFlags a patient-management decision as anomalous when a Bayesian network or Naive Bayes model, learned from the k past patients most similar to the target (weighted Mahalanobis distance), gives it low predictive probability. Author-reported on 100 PORT pneumonia admission decisions: best AUC-ROC 0.81, and 0.5 precision at 0.53 recall for the BBN model.Paper · 2026-03-05cuRoboV2: Dynamics-Aware Motion Generation with Depth-Fused Distance Fields for High-DoF RobotsGPU motion stack for high-DoF robots: B-spline trajectories optimised under torque limits (differentiable RNEA), a dense ESDF built from a block-sparse TSDF with PBA+, and LM-then-L-BFGS collision-free IK. Author-reported: 99.7% success under 3 kg payload (cuRobo 77.1%), 99.6% collision-free IK on a 48-DoF humanoid, up to 10x faster ESDF than nvblox.

Source and license

Adapted from “Global Planning for Contact-Rich Manipulation via Local Smoothing of Quasi-dynamic Contact Models” by Tao Pang, H. J. Terry Suh, Lujie Yang, Russ Tedrake (arXiv:2206.10787), used under CC BY 4.0. Changes: condensed and restructured for implementation, with tables, equations and algorithms taken from the paper and its code, each cited to its source. Not affiliated with or endorsed by the authors. Code citations refer to https://github.com/pangtao22/planning_through_contact at commit 74e4559346a8 (MIT). Any quoted code is shown under that license.