PaperlayerView exact revision on arXiv

Source-linked AI summary

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

Tao Pang, H. J. Terry Suh, Lujie Yang, Russ Tedrake

arXiv:2206.10787v2cs.RO

TL;DR

Contact-rich manipulation challenges model-based planning through numerous contact modes, non-smooth dynamics, and non-convex optimization. The paper establishes equivalence between randomized and analytic smoothing, introduces a convex differentiable quasi-dynamic contact model, and combines smoothing with RRT. Across its reported studies, the approach supports effective model-based planning and addresses local-search limitations through global exploration.

  • Problem

    Model-based contact-rich planning must handle hybrid contact modes, non-smooth dynamics, uninformative local gradients, and non-convex optimization.

    Source: §I · p. 1, B. Rethinking Linearization as a… · p. 4

  • Method

    The paper combines randomized and analytic contact-mode smoothing with a convex differentiable quasi-dynamic contact model and RRT-based global exploration.

    Source: B. Forward Dynamics · p. 6, §X · p. 19

  • Results

    The paper reports theoretical equivalence of the smoothing schemes on simple systems, comparable behavior on complex examples, and effective global planning with the proposed model and RRT.

    Source: §X · p. 19

  • Takeaways & Limitations

    Smoothing can provide informative local models while RRT supplies global search, yielding a model-based framework for highly contact-rich manipulation.

    Source: §X · p. 19

Abstract

from arXiv · show

The empirical success of Reinforcement Learning (RL) in the setting of contact-rich manipulation leaves much to be understood from a model-based perspective, where the key difficulties are often attributed to (i) the explosion of contact modes, (ii) stiff, non-smooth contact dynamics and the resulting exploding / discontinuous gradients, and (iii) the non-convexity of the planning problem. The stochastic nature of RL addresses (i) and (ii) by effectively sampling and averaging the contact modes. On the other hand, model-based methods have tackled the same challenges by smoothing contact dynamics analytically. Our first contribution is to establish the theoretical equivalence of the two methods for simple systems, and provide qualitative and empirical equivalence on a number of complex examples. In order to further alleviate (ii), our second contribution is a convex, differentiable and quasi-dynamic formulation of contact dynamics, which is amenable to both smoothing schemes, and has proven through experiments to be highly effective for contact-rich planning. Our final contribution resolves (iii), where we show that classical sampling-based motion planning algorithms can be effective in global planning when contact modes are abstracted via smoothing. Applying our method on a collection of challenging contact-rich manipulation tasks, we demonstrate that efficient model-based motion planning can achieve results comparable to RL with dramatically less computation. Video: https://youtu.be/12Ew4xC-VwA

I. INTRODUCTION

The paper interprets RL’s success in contact-rich manipulation through smoothing and develops a convex, differentiable quasi-dynamic contact model for model-based planning. It combines smoothed contact dynamics with RRT to support global planning across challenging manipulation tasks.

Motivation

  • Contact-rich planning is difficult because hybrid contact modes make dynamics non-smooth, invalidating local Taylor models and complicating gradient-based and sampling-based planning.Existing approaches explicitly enumerate or sample modes, but these methods face scalability limitations.Source: §I · p. 1

Smoothing

  • Randomized smoothing abstracts neighboring contact modes by sampling and averaging, while analytic smoothing constructs smooth approximations from the underlying equations.Both approaches can produce more informative gradients for planning.Source: §I · p. 2
  • The paper establishes theoretical equivalence between randomized and analytic smoothing on simple systems and comparable qualitative and empirical behavior on complex examples.The framework also supports efficient online computation of locally linear models for smoothed dynamics.Source: §I · p. 2

Contact model

  • The proposed convex implicit time-stepping contact model is differentiable, numerically advantageous relative to LCP formulations, and compatible with smoothing.Its quasi-dynamic assumption ignores transient dynamics and kinetic energy to focus on steady-state transitions and long-term prediction.Source: §I · p. 2

Validation

  • Experiments show the quasi-dynamic model approximates second-order dynamics well when systems are highly damped and dominated by frictional forces.The model was tested in a high-fidelity Drake simulator and on hardware.Source: §I · p. 2

Global planning

  • Combining smoothed contact-mode abstraction with RRT enables tractable global motion planning for highly contact-rich manipulation.The method uses local smoothed models to define a distance metric and guide efficient tree expansion.Source: §I · p. 2

2) Linearization of the Smooth Surrogate:

The paper reframes smoothing as choosing an affine approximation over a distribution, showing that the resulting model is the exact linearization of a smooth surrogate. This connects convolution-based smoothing, gradient estimators, and least-squares fitting.

  • The linearization of the smooth surrogate fρ is defined using its value and Jacobian at the nominal point.The surrogate value is an expectation of perturbed function evaluations, while its Jacobian differentiates that expectation.Source: 2) Linearization of the Smooth Surrogate · p. 3
  • Jρ can be computed either by averaging gradients of perturbed f or through a score-function estimator that does not require gradients of f.The first characterization uses reparametrization; Stein’s lemma provides the alternative likelihood-ratio form.Source: 2) Linearization of the Smooth Surrogate · p. 3
  • For a zero-mean Gaussian perturbation, the best affine model under a weighted least-squares objective equals the exact linearization of fρ.The result follows because the regression objective is convex and its stationary solution matches the smoothed value and Jacobian.Source: B. Rethinking Linearization as a… · p. 4
  • This least-squares view explains why smoothing can produce a more informative local model than pointwise linearization for non-smooth functions.The affine model minimizes residual error over a distribution rather than at a single nominal point.Source: B. Rethinking Linearization as a… · p. 4
  • A polynomial example shows sampled least-squares fitting converging to the linearization of the smooth surrogate.Source: B. Rethinking Linearization as a… · p. 4

C. Computation of the Locally Linear Model

The paper presents analytic, first-order randomized, and zeroth-order randomized routes for computing locally linear models. These routes differ in whether they use analytic derivatives, sampled gradients, or function evaluations, with distinct regularity and variance properties.

1) Analytic Smoothing:

The CQDC model uses convex, quasi-dynamic contact dynamics to obtain numerically advantageous and differentiable forward predictions. Its formulation represents contact through a convex conic optimization problem while retaining a controlled approximation to frictional behavior.

  • The CQDC model is a convex, quasi-dynamic, differentiable contact model used for the paper’s contact-rich planning tasks.It is introduced as the forward dynamics formulation for subsequent planning experiments.Source: §III · p. 6
  • Quasi-dynamic modeling removes velocity and acceleration terms, reducing state dimension and enabling longer-horizon prediction with fewer steps.The simplification is suitable for many manipulation tasks but cannot describe highly dynamic behaviors such as spinning a pen between fingers.Source: A. Why Plan with Quasi-dynamic Models? · p. 6
  • Anitescu’s convex relaxation enforces Coulomb friction exactly in sticking and separation, while sliding introduces a small gap that converges to zero as the step size decreases.Source: B. Forward Dynamics · pp. 6–7
  • The formulation’s convexity is defined through solving a convex optimization program for the next state, not through convexity of f(x,u) itself.Source: B. Forward Dynamics · p. 6
  • The model’s discretized equations use robot stiffness, object mass, external torques, contact impulses, and configuration changes to compute quasi-dynamic transitions.Source: B. Forward Dynamics · p. 6
  • The quasi-dynamic equations and friction constraints are the KKT conditions of a convex SOCP, solvable with conic optimization methods.The SOCP becomes a QP without friction or with linearly representable friction cones.Source: B. Forward Dynamics · p. 7

C. Derivatives of Forward Dynamics

The paper differentiates CQDC forward dynamics by differentiating through the KKT conditions of its convex contact program. Because the resulting derivatives are discontinuous, randomized smoothing is needed for useful local approximations.

  • CQDC derivatives are obtained by expressing forward dynamics in standard form and differentiating the optimal solution with respect to state and control.Source: C. Derivatives of Forward Dynamics · p. 7
  • The derivative of the contact solution with respect to optimization data is computed using the implicit function theorem applied to the SOCP’s KKT conditions.Automatic differentiation or structure-specific methods then propagate derivatives to state and control.Source: C. Derivatives of Forward Dynamics · p. 7, D. Implementation · p. 7
  • Because CQDC derivatives are discontinuous in state and control, a single derivative is not a good local approximation; multiple gradients must be averaged with first-order randomized smoothing.Source: C. Derivatives of Forward Dynamics · p. 7
  • The implementation uses Drake for collision and mechanical quantities, a third-party conic solver, and custom Eigen-based differentiation through SOCP KKT systems.Source: D. Implementation · p. 7
  • The implementation restricts collision geometries to sphere-based pairs so contact points and normals vary smoothly with configuration.The authors note that smoothing collision geometries could alleviate this limitation.Source: D. Implementation · pp. 7–8

IV. SMOOTHING OF CONTACT DYNAMICS

The paper combines randomized or analytic smoothing with convex quasi-dynamic contact models to obtain usable local approximations and gradients across contact modes.

Feasibility

  • Infinite-support sampling can produce infeasible penetrating states, which are handled by projecting them onto the nearest feasible configuration under a quadratic work cost.Linearizing the non-penetration constraint turns this projection into a quadratic program equivalent to the frictionless CQDC dynamics.Source: A. Randomized Smoothing of Contact… · p. 8

Projection interpretation

  • Projected penetrating configurations create an expected stochastic force field that pushes objects away from the feasible-set boundary.Before smoothing, the projection-based dynamics can have zero gradients in the penetrating regime; after smoothing, the gradient becomes positive.Source: A. Randomized Smoothing of Contact… · p. 8

Smoothing methods

  • Randomized smoothing replaces contact dynamics with averaged behavior by repeatedly solving the dynamics and averaging q+, A, and B.Analytic smoothing instead converts hard constraints into log-barrier costs and solves the resulting unconstrained convex program.Source: B. Analytic Smoothing of Contact… · pp. 8–9

Analytic smoothing

  • The log-barrier weight κ controls analytic smoothing, with the constrained solution recovered as κ approaches infinity.For moderate κ, the barrier acts like a force field whose strength increases near the constraint boundary, producing force-at-a-distance behavior.Source: B. Analytic Smoothing of Contact… · p. 9

Local models

  • Smoothed CQDC dynamics support direct computation of local gradients through implicit differentiation of the optimization problem’s optimality conditions.The implementation uses Newton’s method and was reported to work robustly across the numerical experiments.Source: B. Analytic Smoothing of Contact… · p. 9

C. Equivalence of Smoothing Schemes

Randomized and analytic smoothing are theoretically equivalent for simple systems and qualitatively similar on more complex systems, while smoothed local models integrate directly with iterative planning.

Equivalence

  • For simple systems, the paper derives sampling distributions whose randomized smoothing exactly matches log-barrier-based analytic smoothing.For complex systems, Gaussian randomized smoothing and analytic smoothing produce qualitatively similar dynamics.Source: C. Equivalence of Smoothing Schemes · pp. 9–10

Frictionless example

  • In the frictionless example, analytic smoothing replaces the contact constraint with a log-barrier whose optimality conditions define the smoothed dynamics.The resulting impulse is inversely proportional to distance from the wall and is analogous to bilinear complementarity relaxation.Source: C. Equivalence of Smoothing Schemes · p. 9

Frictional example

  • For frictional systems, the paper evaluates the two smoothing schemes numerically because deriving an analytic sampling distribution is difficult.The reported smoothed dynamics exhibit a similar trend under the two schemes.Source: C. Equivalence of Smoothing Schemes · p. 10

Scope

  • The paper expects subtle mathematical differences between analytic and randomized smoothing beyond simple examples despite similar empirical behavior.This qualifies the equivalence claim for general settings.Source: C. Equivalence of Smoothing Schemes · p. 10

Planning integration

  • The resulting local linear models can improve iterative gradient-based planners, including iMPC, by replacing exact Taylor linearizations with smooth-surrogate linearizations.In iMPC, smoothing is gradually reduced so the surrogate converges toward the true dynamics.Source: §V · p. 10, A. Iterative MPC with Smoothing · p. 10

B. Experiment Setup

The experiments evaluate iMPC smoothing schemes on two planar contact-manipulation systems and a three-dimensional Allegro in-hand rotation system.

Systems

  • The study tests planar pushing, planar hand reorientation, and Allegro in-hand rotation with increasing contact and actuation complexity.The systems contain respectively 2, 13, and 20 collision geometries.Source: 1) Systems Description · pp. 10–11

Evaluation

  • Performance is compared across analytic, randomized first-order, randomized zero-order, and exact-gradient iMPC using five-run means and standard deviations.The comparison is presented as performance over iterations.Source: Figure 6 · p. 11

2) Initialization:

The trajectory-optimization experiments use simple initializations and show that smoothing improves performance on tasks requiring multiple contact-mode changes, while randomized smoothing costs more computation.

2) Initialization:

  • A constant nominal input trajectory provides empirically good convergence for the iMPC experiments.The initialization sets every commanded robot position to the same value.Source: 2) Initialization · p. 11
  • Smoothed iMPC can start from a robot configuration near contact, whereas the unsmoothed baseline requires an initialization that already makes contact to avoid zero gradients.The near-contact condition is qualified as not being “too far away” from the objects.Source: 2) Initialization · p. 11

C. Results & Discussion

  • For PlanarPushing and Allegro-Rotation, all smoothing schemes achieve much lower costs than exact gradients, whereas exact linearization also performs well for PlanarHand.The paper relates this difference to the number of contact-mode changes required by each task.Source: 1) Exact vs. Smoothing · pp. 11–12
  • Analytic and first-order randomized smoothing perform similarly, while zeroth-order smoothing performs worse in higher dimensions.The authors attribute the zeroth-order result to the estimator’s high variance.Source: 2) Analytic vs. Randomized Smoothing · p. 12
  • With 100 samples and 32 threads, randomized smoothing is 2 to 3 times slower than analytic smoothing.Analytic smoothing requires one smoothed-dynamics evaluation, whereas randomized smoothing averages N sampled evaluations.Source: 3) Running (wall-clock) Time · p. 12
  • Trajectory optimization is prone to local minima when goals are far from the initial configuration, motivating a more global planner based on RRT.Contact makes local linearizations uninformative and causes existing reachability approaches to scale poorly with the number of contacts.Source: §VI · p. 12
  • The proposed approach uses smoothed local models to construct an informative reachability-consistent distance metric for global contact planning.The metric becomes large when no actuation can reach the queried state in one step.Source: §VI · p. 12, A. The Local Mahalanobis Metric · p. 12

B. Metric on Smoothed Dynamics and Unactuated Objects

The local Mahalanobis metric is built from smoothed dynamics to measure contact-aware reachability, especially for unactuated objects. Its ellipsoidal geometry captures which object configurations are easier to reach and discourages exploration away from contact.

VII. RRT THROUGH CONTACT

The contact-aware RRT combines a smoothed reachability metric, local-model extension, and contact sampling to search through contact dynamics. Its one-step extension uses smoothed models for action selection but the original dynamics for dynamically consistent propagation.

C. Contact Sampling

Contact sampling improves RRT exploration by creating contact-making configurations when ordinary tree growth reaches poorly accessible states. It is especially useful when progress requires substantially resetting actuator configurations, though the current sampler can introduce non-physical teleportation.

  • Nodes far from contact hinder RRT growth because they are distant under the local metric and often produce unfavorable extensions.Source: C. Contact Sampling · p. 14
  • ContactSample fixes the nearest node and constructs another configuration that makes contact, improving the local metric and exploration.Source: C. Contact Sampling · p. 14
  • The current sampler can teleport the robot between configurations, which is acceptable only when the object can maintain static equilibrium during that change.Source: C. Contact Sampling · p. 14
  • In PlanarHandFixedY, the full enhancements achieve good coverage, whereas removing contact sampling or the local metric significantly hinders tree growth.Source: D. Effectiveness of Proposed… · p. 15, Figure 9 · p. 15
  • RRT can still form broom-shaped subgraphs from repeated failed extensions, although the enhanced tree eventually branches into and covers empty configuration-space regions.Source: D. Effectiveness of Proposed… · p. 15
  • Trajectory optimization and shortcutting shorten and physically refine the randomized RRT paths after planning.Source: E. Final Path Refinement · p. 15

B. Results & Discussion

Experiments show that smoothing, local reachability metrics, and contact sampling each contribute to contact-rich RRT performance. The resulting plans are also evaluated for transfer from the quasi-dynamic model to higher-fidelity simulation and hardware.

3) Simulation Setup:

The experiments compare CQDC trajectories with second-order simulation and hardware execution across planar and 3D contact-rich systems. Results show good sim-to-real transfer in 2D, larger discrepancies in 3D, and limitations when open-loop plans encounter high velocities or insufficient damping.

Simulation and Hardware Setup

  • CQDC trajectories were transferred to Drake while keeping collision geometries, controller stiffness, and friction coefficients consistent.Drake uses full second-order dynamics and an accurate contact solver.Source: 3) Simulation Setup · p. 18
  • The IiwaBimanual hardware setup replaces the simulated object with a bucket actuated by two Kuka iiwa arms.The goal is to rotate the bucket by 180 degrees, comparing initial and final poses after open-loop execution.Source: 4) Hardware Setup · p. 18, Figure 13 · p. 18

Sim-to-Real Results

  • 2D systems show low error and good sim-to-real transfer, whereas 3D systems exhibit larger error.The evaluated systems include PlanarPushing, PlanarHand, IiwaBimanual, AllegroHand, AllegroPlate, AllegroPen, and AllegroDoor.Source: B. Results & Discussion · p. 18
  • The persistent phase difference reflects CQDC’s first-order dynamics versus the second-order dynamics generating the real trajectories.The real trajectory tends to lag during acceleration and lead during deceleration; on favorable segments, this produces harmless oscillations in the simulated trajectory.Source: B. Results & Discussion · p. 18

Limitations

Related papers

Loading 2206.10787v2…