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.

  • Method

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

  • 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.

  • Takeaways & Limitations

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

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.
  • 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.
  • Smoothing: 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.
  • 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.
  • 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.
  • 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.

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.
  • 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.
  • 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.
  • 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.
  • A polynomial example shows sampled least-squares fitting converging to the linearization of the smooth surrogate.

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.

  • The paper considers three main ways to compute the smoothed value and Jacobian: analytic convolution, sampled function values, and sampled gradients.
  • Analytic computation is available when f and the smoothing distribution are sufficiently structured, while Monte Carlo estimates the smoothed value from sampled evaluations.The sample mean of perturbed evaluations is unbiased for the smoothed value.
  • First-order randomized smoothing estimates Jρ by averaging sampled gradients, but requires f to be locally Lipschitz.Under that condition, the sample mean of perturbed gradients is unbiased.
  • Zeroth-order estimation remains unbiased for discontinuous functions but generally has higher variance than first-order estimation.
  • For ReLU with logistic smoothing, analytic, first-order, and zeroth-order methods converge on average to the smoothed gradient, while zeroth-order estimates are noisier.For the gradient of the smoothed Heaviside, first-order estimation is biased because sampled Heaviside gradients are almost surely zero.
  • First-order and zeroth-order randomized gradients can be interpolated into an α-ordered estimator that trades off their bias and variance.

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.
  • 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.
  • 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.
  • The formulation’s convexity is defined through solving a convex optimization program for the next state, not through convexity of f(x,u) itself.
  • The model’s discretized equations use robot stiffness, object mass, external torques, contact impulses, and configuration changes to compute quasi-dynamic transitions.
  • 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.

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.
  • 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.
  • 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.
  • The implementation uses Drake for collision and mechanical quantities, a third-party conic solver, and custom Eigen-based differentiation through SOCP KKT systems.
  • 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.

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.
  • 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.
  • 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.
  • 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.
  • 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.

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.
  • 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.
  • 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.
  • 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.
  • 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.

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.
  • 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.

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.
  • 2) Initialization:: 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.
  • 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.
  • C. Results & Discussion: 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.
  • C. Results & Discussion: 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.
  • C. Results & Discussion: 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.
  • C. Results & Discussion: 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.

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.

  • The metric uses the linearization of smoothed dynamics rather than exact contact dynamics to obtain more informative local reachability estimates.
  • For unactuated objects, the metric is formed from the corresponding rows of the smoothed dynamics matrix and emphasizes object reachability.
  • Contact-dependent terms summarize achievable object motions across potentially active contacts, while no-contact configurations leave actuated motion direct and object motion unchanged.
  • The metric distinguishes configurations that are equally distant under Euclidean distance: b1 is considered easier to reach than b2 under the local metric.
  • The metric varies across configurations, making a globally uniform metric a poor reachability measure.
  • As the nominal state moves farther from contact, the reachability ellipsoid shrinks and less accessible object configurations become farther from the planner.

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.

  • The enhanced RRT replaces uniform nearest-neighbor selection with a reachability-aware Mahalanobis metric on unactuated objects.
  • Its Extend operation projects the subgoal onto the range of the smoothed dynamics matrix to compute an action efficiently.
  • A single time step is sufficient in practice for Extend to grow the RRT effectively because quasi-dynamic models can steer the system far in one step.
  • The planner computes actions with the smoothed model but propagates nodes using the original contact dynamics, preserving dynamic consistency.

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.
  • ContactSample fixes the nearest node and constructs another configuration that makes contact, improving the local metric and exploration.
  • The current sampler can teleport the robot between configurations, which is acceptable only when the object can maintain static equilibrium during that change.
  • In PlanarHandFixedY, the full enhancements achieve good coverage, whereas removing contact sampling or the local metric significantly hinders tree growth.
  • RRT can still form broom-shaped subgraphs from repeated failed extensions, although the enhanced tree eventually branches into and covers empty configuration-space regions.
  • Trajectory optimization and shortcutting shorten and physically refine the randomized RRT paths after planning.

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.

  • Exact linearizations perform much worse than all tested smoothing schemes, supporting mode smoothing as necessary for many tasks.
  • Analytic and randomized smoothing show similar goal-reaching and packing-ratio performance, while analytic smoothing computes faster because it avoids multiple samples.
  • The local Mahalanobis metric consistently outperforms a globally uniform weighted Euclidean metric across the experiments.
  • Contact sampling substantially improves performance on tasks requiring major actuator resets, but is unnecessary for some tasks.
  • Sim2real evaluation converts CQDC knot-point plans into trajectories and compares real rollouts with simulated trajectories using position and orientation errors.
  • The normalized transfer error accounts for trajectory length and is dimensionless; each system contributes at least 10 evaluated segments.

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.
  • Simulation and Hardware Setup: 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.
  • 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.
  • Sim-to-Real Results: 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.
  • Limitations: Insufficient damping in some 3D grasps can cause objects to roll away from the planned trajectory or off the palm.This limitation arises because point contacts provide little damping when the grasp is not tight.
  • Limitations: Open-loop CQDC plans may fail under second-order dynamics with high velocities and low damping, including missed contacts and dropped objects.The paper suggests low-level feedback stabilization and robustness objectives as future directions.
Loading 2206.10787v2…