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

Authors: Tao Pang, H. J. Terry Suh, Lujie Yang, Russ Tedrake · T-RO 2023

Licences

Open Access
Paper: CC BY 4.0

Everything below draws on the full paper under its CC BY 4.0 licence; its figure is credited to the authors.

TL;DR

Smoothing contact dynamics, by sampling as RL does or analytically with a log barrier, gives useful gradients; with a convex quasi-dynamic model, RRT then plans dexterous tasks in about a minute.

Why it matters

The method gives an online, model-based alternative to training reinforcement-learning policies for contact-rich tasks: plans for in-hand rotation, pen placement, plate pickup against a wall and door opening are produced on the order of a minute on a CPU. The authors contrast this with RL tools that rely on offline computation of hours or days, and describe their planner as generalizable with respect to environments and tasks.

Problem

Model-based planning through contact faces three obstacles: the number of contact modes explodes (3D in-hand manipulation can have trillions), contact dynamics are stiff and non-smooth so gradients explode or jump, and the planning problem is non-convex. Reinforcement learning handles contact-rich manipulation by sampling and averaging over contact modes, but relies on offline computation measured in hours or days. Existing sampling-based planners for contact enumerate or sample contact modes explicitly and scale poorly with the number of contacts.

Proposed solution

The authors first show that randomized smoothing (sampling and averaging, as RL does) and analytic smoothing compute local linear models of the same smooth surrogate, equivalently for simple systems. They then build CQDC, a convex, quasi-dynamic, differentiable contact model posed as a second-order cone program with Anitescu's friction relaxation, whose gradients come from the KKT conditions and which is smoothed analytically with a log-barrier relaxation. Finally, they run RRT with a local Mahalanobis distance metric and an extension step built from the smoothed linearizations, add a contact-sampling step that re-places the robot in contact, and refine the resulting path with iterative MPC and shortcutting.

Key insights

Randomized smoothing (as used by RL) and analytic smoothing are two ways of computing local linear models of the same smoothed dynamics; they are equivalent for simple systems and performed similarly on the complex examples.
Analytic smoothing via a log-barrier relaxation needs a single solve per linearization, so it ran faster than randomized smoothing, which averages 100 samples.
The quasi-dynamic model drops velocities and transient dynamics, halving the state and allowing much larger time steps, so each linearization tells the planner more about long-term behaviour.
A local Mahalanobis metric built from smoothed linearizations lets RRT respect contact reachability; a hand-tuned globally uniform weighted Euclidean metric performed much worse.
Contact sampling was essential for tasks that require repositioning the robot (planar pushing, planar hand, Allegro in-hand rotation) but not for plate pickup or pen placement.

Main results

Iterative MPC, minimum cost over 10 iterations and 5 trials: Allegro in-hand rotation 5.78 with analytic smoothing vs 44.68 with exact (unsmoothed) gradients; planar pushing 11.74 vs 31.64.
On the planar hand task, which goes through few contact mode changes, exact gradients also performed well (cost 18.49 vs 26.55 analytic and 17.87 first-order randomized).
With 100 samples on 32 threads, randomized smoothing was 2 to 3 times slower than analytic smoothing in iterative MPC (Allegro rotation: 40.07 s vs 19.59 s).
RRT for 1000 iterations with analytic smoothing took 3.25 s (planar pushing), 31.01 s (Allegro rotation), 24.84 s (pen placement), 12.42 s (door opening) and 117.16 s (plate pickup); first-order randomized smoothing took 7.50 s to 161.64 s.
In every RRT task, using exact linearizations for the metric and extension gave much worse goal distance and space coverage than any smoothing scheme.
All experiments ran on one desktop with an AMD Threadripper 2950 CPU (16 cores, 32 threads) and 32 GB of RAM.

Limitations

Contact sampling teleports the robot between configurations, which is non-physical; it only works when the object stays in static equilibrium without the moved fingers (for example a ball resting on an upward-facing palm), not for a downward-facing hand.
Large extension steps produce non-physical artifacts from Anitescu's convex relaxation of Coulomb friction, so a refinement pass with iterative MPC and shortcutting is needed for physically realistic trajectories.
The quasi-dynamic assumption breaks down for highly dynamic behaviours.
Tree growth can still get stuck, forming "brooms" of many failed child nodes from one parent.
All results in this version are in simulation (implemented in Drake); real-time planning is named as future work.

Field impact

The paper received the T-RO 2024 King-Sun Fu Memorial Best Paper Award (honourable mention) and has 210 citations. It releases a video of the generated plans; this version gives no code link.

Abstract

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

Open Source Robotics