(Physics) Model-based Planning for Contact-rich Robotic Manipulation

Tao Pang

Nov 2022

Why contact-rich?

Collision-free Motion Planning

Interact with the world

Smaller

What can we learn from RL (reinforcement learning)?

  • RL still requires heavy offline computation.
    • The Nvidia policy is trained with "only 8 NVIDIA A40 GPUs" for 60 hours.
  1. (OpenAI) Andrychowicz et al. "Learning dexterous in-hand manipulation." The International Journal of Robotics Research, 2020.
  2. (Nvidia) Handa et al. "DeXtreme: Transfer of Agile In-hand Manipulation from Simulation to Reality", arXiv preprint, 2022.

  3. H.J. Terry Suh*, Tao Pang*, Russ Tedrake, “Bundled Gradients through Contact via Randomized Smoothing”, RA-L, 2022.

  4. Tao Pang*, H.J. Terry Suh*, Lujie Yang, Russ Tedrake, "Global Planning for Contact-Rich Manipulation via Local Smoothing of Quasi-dynamic Contact Models", under review, 2022.

[1] OpenAI.

[2] Nvidia.

How does RL power through non-smooth contact dynamics? [3][4]

  • RL maximizes a stochastic reward using gradients estimated from sampling.
  • Sampling has a smoothing effect on the reward's landscape.

In model-based planning, we can smooth contact dynamics, but without using samples (faster!).

Why is contact-rich planning hard?

Contact dynamics is non-smooth!

Two solutions

Descend with gradients of smoothed dynamics

Reason explicitly about contact mode transitions

q^\mathrm{a}_+
u

Contact

No Contact

  • But descent methods get stuck in local minima.
  • Therefore they need non-trivial initialization.
  • Good at escaping local minima,
  • But mode enumeration scales poorly (exponentially) with the number of contacts.
x_+ = u
x_+ = 0
u > 0
u \leq 0
q^\mathrm{a}_+
u

smoothed dynamics

original dynamics

\(u = 0\)

Model-based Planning through Contact: a Quick Review

  • Trajectory Optimization through Contact
  1. Mordatch, Igor, Emanuel Todorov, and Zoran Popović. "Discovery of complex behaviors through contact-invariant optimization." ACM Transactions on Graphics (TOG), 2012.
  2. Hogan, François Robert, and Alberto Rodriguez. "Feedback control of the pusher-slider system: A story of hybrid and underactuated contact dynamics." arXiv preprint arXiv:1611.08268, 2016.
  3. Posa, Michael, Cecilia Cantu, and Russ Tedrake. "A direct method for trajectory optimization of rigid bodies through contact." The International Journal of Robotics Research, 2014.
  4. Aydinoglu, Alp, and Michael Posa. "Real-time multi-contact model predictive control via admm." 2022 International Conference on Robotics and Automation (ICRA), 2022.
  5. Cheng, Xianyi, et al. "Contact mode guided motion planning for quasidynamic dexterous manipulation in 3d." 2022 International Conference on Robotics and Automation (ICRA), 2022.
  6. Marcucci, Tobia, et al. "Approximate hybrid model predictive control for multi-contact push recovery in complex environments." 2017 IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids), 2017.
  • Enumerate mode switches locally
  • Enumerate Mode Switches

Good 

Scalability (Smoothed dynamics)

Bad Scalability (Mode Transitions)

Local Planning

Global Planning

Global planning with RRT + Smoothed dynamics?

[1] Mordatch et al.

[3] Posa et al.

[6] Marcucci et al.

[4] Aydinoglu and Posa.

[2] Hogan and Rogriguez.

  • Sampling mode transitions.

[5] Cheng et al.

Our Method: Sim and Hardware Demos.

  • Our planner combines
    • sampling-based global search and
    • smoothed contact dynamics.
  • Our planner takes less than a minute to find most plans shown above, on a single CPU.

Open-loop execution of contact-rich plans on hardware:

A better contact dynamics model for contact-rich planning

  • Rapidly-Exploring Random Tree (RRT):   iteratively builds a tree that fills the state space. [1]
  • A good distance metric should reflect dynamic reachability. [2]
  1. Steven M. LaValle, "Planning Algorithms", Cambridge University Press , 2006.

  2. H.J.T. Suh, J. Deacon, Q. Wang, "A Fast PRM Planner for Car-like Vehicles", self-hosted, 2018.

  3. Tao Pang*, H.J.T. Suh*, Lujie Yang, Russ Tedrake, "Global Planning for Contact-Rich Manipulation via Local Smoothing of Quasi-dynamic Contact Models", under review, 2022.

How do we measure reachability under the non-smooth contact-dynamics constraint?

A convex, quasi-static, differentiable contact model that can be easily smoothed. [3] 

Reachability-informed distance metric using smoothed linearization of our model

Presented Works

Collaborators

  • Tao Pang*, H.J. Terry Suh*, Lujie Yang, Russ Tedrake, "Global Planning for Contact-Rich Manipulation via Local Smoothing of Quasi-dynamic Contact Models", under review.

  • H.J. Terry Suh*, Tao Pang*, Russ Tedrake, "Bundled Gradients through Contact via Randomized Smoothing", RA-L, 2022.

Terry Suh

Lujie Yang

Russ Tedrake

What is quasi-static dynamics?

q^\mathrm{a}
u
K_\mathrm{a}
q^\mathrm{u}
  • Quasi-static: velocity is small so that inertial and Coriolis forces are negligible.
  • \(x \coloneqq q \coloneqq  [q^\mathrm{u}, q^\mathrm{a}]\): system configuration.
    • \(q^\mathrm{a}\): actuated, imepdance-controlled robots
    • \(q^\mathrm{u}\): un-actuated objects.
  • \( u \): robot velocities position command to the stiffness-controlled robots [1].
  • \(\delta q \coloneqq q_+ - q\) 
  • \(h\): step size in seconds.

\( x_{+} = f(x, u)\)

\lambda_1
\lambda_3
\lambda_1
\lambda_2
  • \(\lambda_i\): contact forces/impulses
  • \(\phi_i \): signed distances
\phi_2({q})
\phi_1({q})
\phi_3({q})
\begin{aligned} \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q^\mathrm{a} + \delta q^\mathrm{a} - u)^2 \; \text{s.t.} \\ &\phi_i (q + \delta q)\geq 0, \forall i. \end{aligned}
\Leftrightarrow

(KKT)

  • A convex quadratic program (QP), after linearizing \(\phi_i\).
  • "Minimizing potential energy, subject to non-penetration constraints."
\begin{cases} hK(q^\mathrm{a} + \delta q^\mathrm{a} - u) = -\lambda_1 + \lambda_2, \\ 0 = \lambda_1 - \lambda_3, \\ \phi_i(q + \delta q) \geq 0, \; \forall i, \\ \lambda_i \geq 0 \; \forall i, \\ \phi (q + \delta q) \lambda = 0, \forall i. \end{cases}

Force balance of the robot.

Force balance of the object.

Non-penetration.

"Contact forces cannot pull."

"Contact force needs contact."

  1. Tao Pang, Russ Tedrake, "A Convex Quasistatic Time-stepping Scheme for Rigid Multibody Systems with Contact and Friction", ICRA, 2021.

Quasi-static dynamics is good for manipulation planning!

  • Many manipulation tasks are quasi-static.
  • Spatial benefit: Smaller state space (\([q, v]\) vs. only \(q\)).

  • Temporal benefit: Ignoring transients => looking into the future with fewer steps.

  1. Tao Pang, Russ Tedrake, "A Convex Quasistatic Time-stepping Scheme for Rigid Multibody Systems with Contact and Friction", ICRA, 2021.

q^\mathrm{a}
m^\mathrm{a}
u
K
D

Second-order Dynamics

Quasi-static Dynamics

\begin{aligned} m^\mathrm{a} &= 1.0 \\ D &= 20.0\\ K &= 100.0 \end{aligned}

Quasi-static Dynamics Needs Regularization

\begin{aligned} \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2, \; \text{s.t.} \\ &\phi_i (q + \delta q)\geq 0, \forall i. \end{aligned}

Quasi-static Dynamics

  • The quadratic cost is positive semi-definite.
  • The object can be anywhere between the robot and the wall, similar to having a null space.
\underset{x}{\min} \|Ax - b\|^2
\underset{x}{\min} \|Ax - b\|^2 + \epsilon \|x\|^2

Regularized least squares

Regularized Quasi-static Dynamics

\begin{aligned} \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q^\mathrm{a} + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \epsilon (\delta q^\mathrm{u})^2, \; \text{s.t.} \\ &\phi_i (q + \delta q)\geq 0, \forall i. \end{aligned}
  • The quadratic cost is now positive definite.
  • "Among all possible object motions, give me the one that moves the least."
  • Picking \(\epsilon = m^\mathrm{u} / h\) gives Mason's definition of quasi-dynamic dynamics.
q^\mathrm{a}
u
K_\mathrm{a}
q^\mathrm{u}
\phi_2({q})
\phi_1({q})
\phi_3({q})

What about friction?

\begin{aligned} \underset{\delta q}{\min} \; &\frac{1}{2} {\underbrace{ \begin{bmatrix} \delta q^\mathrm{u} \\ \delta q^\mathrm{a} \end{bmatrix}}_{\delta q}}^\intercal \underbrace{ \begin{bmatrix} \mathbf{M}_\mathrm{u} / h & 0 \\ 0 & h \mathbf{K}_\mathrm{a} \end{bmatrix}}_{\mathbf{Q}} \underbrace{ \begin{bmatrix} \delta q^\mathrm{u} \\ \delta q^\mathrm{a} \end{bmatrix}}_{\delta q} + {\underbrace{ h \begin{bmatrix} - \tau^\mathrm{u} \\ - \mathbf{K}_\mathrm{a} (u - q^\mathrm{a}) - \tau^\mathrm{a} \end{bmatrix}}_{b}}^\intercal \underbrace{ \begin{bmatrix} \delta q^\mathrm{u} \\ \delta q^\mathrm{a} \end{bmatrix}}_{\delta q} \;\text{s.t.} \\ &\underbrace{\phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q}_{\text{Linearized} \; \phi_i(q + \delta q)} \geq 0, \; \forall i \in \{1 \cdots n_{\mathrm{c}}\}. \end{aligned}
  • For a frictionless multi-body system with \(n_\mathrm{c}\) contacts, the dynamics can be solved as a QP:
  • \(\mathcal{K}^\star_i\) is the dual of the \(i\)-th friction cone.
  • Still convex! Convexity is achieved at the expense of "hydroplaning" between relatively-sliding objects.
  • But hydroplaning disappears as \(h \rightarrow 0\).
\begin{aligned} \underset{\delta q}{\min} \; &\frac{1}{2} {\delta q}^\intercal \mathbf{Q} \delta q + b^\intercal \delta q \;\text{s.t.} \\ & \begin{bmatrix} \phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q \\ \mathbf{J}_{\mathrm{t}_i}\delta q \end{bmatrix} \in \mathcal{K}^\star_i, \; \forall i \in \{1 \cdots n_{\mathrm{c}}\}. \end{aligned}
  • Anitescu [1] has a nice convex approximation of Coulomb friction constraints, allowing us to write down the dynamics as a Second-Order Cone Program (SOCP):

Tangential displacements

\begin{aligned} \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \frac{m^\mathrm{u}}{h} (\delta q^\mathrm{u})^2, \; \text{s.t.} \\ &\phi_i (q + \delta q)\geq 0, \; \forall i. \end{aligned}

Dynamics of the Toy Problem

  1. Anitescu, Mihai. "Optimization-based simulation of nonsmooth rigid multibody dynamics." Mathematical Programming 105.1 (2006): 113-143.

Differentiability

\begin{aligned} \delta q^\star \coloneqq \underset{\delta q}{\text{argmin}.} \; &\frac{1}{2} {\delta q}^\intercal \mathbf{Q} \delta q + b^\intercal \delta q \;\text{s.t.} \\ & \begin{bmatrix} \phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q \\ \mathbf{J}_{\mathrm{t}_i}\delta q \end{bmatrix} \in \mathcal{K}^\star_i, \; \forall i \in \{1 \cdots n_{\mathrm{c}}\}. \end{aligned}

Convex, Quasi-Static, Differentiable Dynamics (an SOCP)

q_+ = f(q, u) = q + \delta q^\star(q, u)
\newcommand{\DfDx}[2]{\frac{\partial {#1}}{\partial {#2}}} \mathbf{A} \coloneqq \DfDx{f}{q} = \mathbf{I} + \DfDx{\delta q^\star}{q}, \; \mathbf{B} \coloneqq \DfDx{f}{u} = \DfDx{\delta q^\star}{u},
  • The KKT optimality condition of the dynamics SOCP defines equality constraints (including active contact constraints), from which the derivatives of \(\delta q^\star\) w.r.t. \((\mathbf{Q}, b, \phi_i, \mathbf{J}_{\mathrm{n}_i}, \mathbf{J}_{\mathrm{t}_i})\) can be computed using the Implicit Function Theorem (IFT).
  • \(\mathbf{A}, \mathbf{B}\) can then be computed using the gradients from IFT and the chain rule.
  • Standard practice for many differentiable simulators, even though many of them do not find the active constraints by solving convex programs.

Smoothing

  • The contact dynamics \(f(q, u) \in C^0\) has discontinuous gradients, which are bad for planning methods that use gradients.
  • Smoothing with "force at a distance" is a common method to alleviate this problem.

Optimality Condition

\begin{aligned} \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \frac{m^\mathrm{u}}{h} (\delta q^\mathrm{u})^2 - \frac{1}{\kappa}\sum_i\log{\phi_i(q + \delta q)} \end{aligned}
\Leftrightarrow
\begin{aligned} \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \frac{m^\mathrm{u}}{h} (\delta q^\mathrm{u})^2, \; \text{s.t.} \\ &\phi_i (q + \delta q)\geq 0, \forall i. \end{aligned}
\Leftrightarrow

KKT

Original Dynamics

\begin{cases} hK(q^\mathrm{a} + \delta q^\mathrm{a} - u) = -\lambda_1 + \lambda_2, \\ (m^\mathrm{u} / h) \delta q^\mathrm{u} = \lambda_1 - \lambda_3, \\ \phi_i(q + \delta q) \geq 0, \; \forall i, \\ \lambda_i \geq 0, \; \forall i, \\ \phi (q + \delta q) \lambda = 0, \forall i. \end{cases}
q^\mathrm{a}
u
K_\mathrm{a}
q^\mathrm{u}
\lambda_1
\lambda_3
\lambda_1
\lambda_2
\phi_2({q})
\phi_1({q})
\phi_3({q})

Smoothed Dynamics

\begin{cases} hK(q^\mathrm{a} + \delta q^\mathrm{a} - u) = -\lambda_1 + \lambda_2, \\ (m^\mathrm{u} / h) \delta q^\mathrm{u} = \lambda_1 - \lambda_3, \\ \phi_i(q + \delta q) \geq 0, \; \forall i, \\ \lambda_i \geq 0, \; \forall i, \\ \phi_i (q + \delta q) \lambda = 1 / \kappa, \forall i. \end{cases}

Smoothing, the general case.

\begin{aligned} \underset{\delta q}{\text{min}.} \; &\frac{1}{2} {\delta q}^\intercal \mathbf{Q} \delta q + b^\intercal \delta q \;\text{s.t.} \\ & \begin{bmatrix} \phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q \\ \mathbf{J}_{\mathrm{t}_i}\delta q \end{bmatrix} \in \mathcal{K}^\star_i, \; \forall i \in \{1 \cdots n_{\mathrm{c}}\}. \end{aligned}
\underset{\delta q}{\text{min}.} \; \frac{1}{2} {\delta q}^\intercal \mathbf{Q} \delta q + b^\intercal \delta q - \frac{1}{\kappa} \sum_{i=1}^{n_\mathrm{c}} \log \left[ \frac{(\phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q)^2}{\mu_i^2} - (\mathbf{J}_{\mathrm{t}_i}\delta q)^\intercal (\mathbf{J}_{\mathrm{t}_i}\delta q) \right]

Put constraints in a generalized log for \(\mathcal{\kappa}_i^\star\)

\Rightarrow
  • An unconstrained convex program.
  • Can be solved with Newton's method.
  • Also differentiable.

Convex, Quasi-Dynamic, Differentiable Dynamics (an SOCP)

Notation: 

q_+ = f(q, u)

Original dynamics

q_+ = f_\rho(q, u)

Smoothed dynamics

Smoothed gradients enable trajectory optimization for dexterous hands, but...

Task: Turning the ball by 30 degrees.

What about 180 degrees?

Finding this trajectory requires powering through local minima!

Sampling-based Motion Planing is Great at Global Exploration, but...

  • Rapidly-Exploring Random Tree (RRT):   iteratively builds a tree that fills the state space. [1]
  • Dynamic reachability is essential for efficient exploration. [2]
  1. Steven M. LaValle, "Planning Algorithms", Cambridge University Press , 2006.

  2. H.J.T. Suh, J. Deacon, Q. Wang, "A Fast PRM Planner for Car-like Vehicles", self-hosted, 2018.

How do we think about reachability under the contact-dynamics constraint?

A distance metric that reflects reachability

q_+ = f_\rho(q, u),
\newcommand{\DfDx}[2]{\frac{\partial {#1}}{\partial {#2}}} \mathbf{A}_\rho \coloneqq \DfDx{f_\rho}{q}\big|_{q,u}, \\ \mathbf{B}_\rho \coloneqq \DfDx{f_\rho}{u}\big|_{q,u}.
\mathcal{R}_{\rho, \varepsilon}^\mathrm{u} (\bar{q})\coloneqq \left\{\mathbf{B}_\rho^\mathrm{u}(\bar{q},\bar{q}^{\mathrm{a}}) \delta u + f_\rho^\mathrm{u}(\bar{q},\bar{q}^\mathrm{a})\; | \; \|\delta u\|\leq \varepsilon\right\}

Smoothed Contact Dynamics

\(\varepsilon \) sub-level set

Nominal state

un-actauted(objects)

Smoothed

\begin{aligned} d^\mathrm{u}_{\rho}(q; \bar{q}) &\coloneqq \frac{1}{2} (q^\mathrm{u} - \mu^\mathrm{u}_\rho)^\intercal {\left[\mathbf{B}_{\rho}^{\mathrm{u}} {\mathbf{B}_{\rho}^{\mathrm{u}}}^\intercal\right]}^{-1} (q^\mathrm{u} - \mu^\mathrm{u}_\rho) \\ \mu_\rho^\mathrm{u} &\coloneqq f_\rho^\mathrm{u}(\bar{q}, \bar{q}^\mathrm{a}) \end{aligned}

At state \(\bar{q}\), what doe an \(\varepsilon\)-ball in action space get transformed into under the smoothed dynamics?

RRT through contact

Three modifications to the vanilla RRT are made:

  • Nearest node on tree is found using \( d^\mathrm{u}_{\rho}(q; \bar{q}) \).
  • Contact sampling: robot configuration is occasionally reset to one that has a good distance metric, which typically corresponds to grasps.
  • Dynamic-consistent extension: an action is computed using the smooth linearized dynamics to reach \(q\).

Distance Metric

RRT tree for a simple system with contacts

Ablation study: what trees look like after growing a fixed number of nodes.

\begin{aligned} d^\mathrm{u}_{\rho}(q; \bar{q}) &\coloneqq \frac{1}{2} (q^\mathrm{u} - \mu^\mathrm{u}_\rho)^\intercal {\left[\mathbf{B}_{\rho}^{\mathrm{u}} {\mathbf{B}_{\rho}^{\mathrm{u}}}^\intercal\right]}^{-1} (q^\mathrm{u} - \mu^\mathrm{u}_\rho) \\ \mu_\rho^\mathrm{u} &\coloneqq f_\rho^\mathrm{u}(\bar{q}, \bar{q}^\mathrm{a}) \end{aligned}

The modified RRT works well for contact-rich tasks!

Planning wall-clock time (seconds)

Preliminary: stabilizing contact-rich plans

Open-loop

Closed-loop

\begin{aligned} \underset{q^\mathrm{u}_+, u}{\min} &\|q_+^\mathrm{u} - q_+^\mathrm{u, ref}\|_\mathbf{Q} + \|u - u^\mathrm{ref} \|_\mathbf{R}, \text{s.t.}\\ & q^\mathrm{u}_+ = f^\mathrm{u}(q^\mathrm{ref}, u^\mathrm{ref}) + \mathbf{B}_\rho^\mathrm{u} (u - u^\mathrm{ref}) \end{aligned}

Naive QP controller based on smoothed linearization:

Quasi-static dynamical systems: how to fix it.

q^\mathrm{a}
m^\mathrm{a}
u
K
D

A Impedance-controlled 1D robot and an object.

m^\mathrm{u}
q^\mathrm{u}
  1. Pang, Tao, and Russ Tedrake. "A convex quasistatic time-stepping scheme for rigid multibody systems with contact and friction." 2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021.
  2. Pang, Tao, and Russ Tedrake. "A robust time-stepping scheme for quasistatic rigid multibody systems." 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.

\( x_{+} = f(x, u)\)

  • \(x \coloneqq q \coloneqq  [q^\mathrm{u}, q^\mathrm{a}]\): system configuration.
    • \(q^\mathrm{a}\): actuated, imepdance-controlled robots
    • \(q^\mathrm{u}\): un-actuated objects.
  • \( u \): robot velocities. position command to the impedance-controlled robots. 
\begin{cases} \end{cases}

Contact force and \(q_+\) can now be uniquely* determined from \(u\), even for grasping.

Force-acceleration vs. Impulse-Momentum

q^\mathrm{a}
m^a \ddot{q}^\mathrm{a} + D \dot{q}^\mathrm{a} + K(q^\mathrm{a} - u) = 0
m^\mathrm{a}
u
``\Sigma F = ma"

Force-acceleration

K
D
  1. Stewart, David E. "Rigid-body dynamics with friction and impact." SIAM review 42.1 (2000): 3-39.
``h\Sigma F = m\Delta v"
m^\mathrm{a} (\delta v) + hD v^\mathrm{a} + hK(q^\mathrm{a} + \delta q^\mathrm{a} - u) = 0

Impulse-momentum

0
0
0
0
\underset{\delta q^\mathrm{a}}{\min} \; \frac{1}{2} hK (q^\mathrm{a} + \delta q^\mathrm{a} - u)^2

is the optimality condition of 

But impacts can lead to infinite forces (e.g. Painleve's Paradox) [1].

Time step

Stabilizing contact-rich plans

Open-loop hardware

Open-loop Drake