# Contact-rich Planning for Robotic Manipulation with Quasi-static Contact Models

### Tao Pang

Word cloud of my PhD thesis :D

# Why contact-rich manipulation?

Collision-free Motion Planning

Interact with the world

Smaller

# Why is contact-rich planning hard?

### Contact dynamics is non-smooth!

q^\mathrm{a}
u
q^\mathrm{u}

$$\mathrm{u}$$: un-actuated

control input: commanded ball position

$$\mathrm{a}$$: actuated

q^\mathrm{a}
u
q^\mathrm{u}

# Moving the box to the right

q^\mathrm{u}_+
u

No contact

Contact

\underset{u}{\mathrm{minimize}} \; \frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2
-\frac{\partial }{\partial u} \left(\frac{1}{2} (q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2 \right) = \underbrace{(q^\mathrm{u}_\text{goal} - q^\mathrm{u}_+)}_+ \underbrace {\frac{\partial q_+^\mathrm{u}}{\partial u}}_0

\frac{\partial q^\mathrm{u}_+}{\partial u}
u

No contact

Contact

q^\mathrm{a}
u
q^\mathrm{u}
q^\mathrm{u}_\text{goal}

# Smoothing comes to the rescue!

q^\mathrm{u}_+
u

No contact

Contact

\underset{u}{\mathrm{minimize}} \; \frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2
-\frac{\partial }{\partial u} \left(\frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2\right) = \underbrace{(q^\mathrm{u}_\text{goal} - q^\mathrm{u}_+)}_+ \underbrace {\frac{\partial q_+^\mathrm{u}}{\partial u}}_+

\frac{\partial q^\mathrm{u}_+}{\partial u}
u

No contact

Contact

q^\mathrm{a}
u
q^\mathrm{u}
q^\mathrm{u}_\text{goal}

# Moving the box to the left?

q^\mathrm{u}_+
u

No contact

Contact

\underset{u}{\mathrm{minimize}} \; \frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2
-\frac{\partial }{\partial u} \left(\frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2\right) = \underbrace{(q^\mathrm{u}_\text{goal} - q^\mathrm{u}_+)}_- \underbrace {\frac{\partial q_+^\mathrm{u}}{\partial u}}_+

\frac{\partial q^\mathrm{u}_+}{\partial u}
u

No contact

Contact

Stuck in local minima!

q^\mathrm{a}
u
q^\mathrm{u}
q^\mathrm{u}_\text{goal}

# Global Search with Contact Modes

### Number of modes

• Left not touching
• Left touching
• Right touching
• Right not touching
4

x

Friction cone

4 \times 4
• Not touching
• Sticking
• Sliding left
• Sliding right

y

Number of faces

x

x

y

z

6^\binom{20}{2} = 6^{190}
• Not touching
• Sticking
• 4 directions of sliding

Number of contact pairs.

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

• Locally enumerate mode switches  (fast)
• Globally enumerate Mode Switches (slow)

Good Scalability (Smoothed dynamics)

Limited Scalability (Mode Transitions)

Local Planning

Global Planning

• Our method: Global planning with RRT + Smoothed dynamics [7]

[1] Mordatch et al.

[3] Posa et al.

[6] Marcucci et al.

[4] Aydinoglu and Posa.

[2] Hogan and Rodriguez.

• Sampling mode switches.

[5] Cheng et al.

# What can we learn from RL (reinforcement learning)?

• RL generates impressive policies for specific tasks, but...
• RL requires heavy offline computation,
• The Nvidia policy is trained with "only 8 NVIDIA A40 GPUs" for 60 hours.
• The learned policy does not generalize (e.g. to opening a door).
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, 2018

[2] Nvidia, 2022

### 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 gradients.

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

Terry Suh

# Contributions

• A novel contact dynamics model which is
• quasi-static
• convex
• differentiable
• amenable to smoothing
• A sampling-based global search algorithm guided by the proposed contact model.

A contact-rich planner that can generate plans for complex systems involving dexterous hands / hardware with about 1 minute of online computation on a laptop.

In contrast, RL-based methods needs tens of hours of offline computation on a beefy workstation.

Our planner is enabled by

# 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.
• $$q \coloneqq [q^\mathrm{u}, q^\mathrm{a}]$$: system configuration.
• $$q^\mathrm{a}$$: actuated, imepdance-controlled robots
• $$q^\mathrm{u}$$: un-actuated objects.
• $$u$$: position command to the stiffness-controlled robots [1].
• $$\delta q \coloneqq q_+ - q$$
• $$h$$: step size in seconds.

## $$q_{+} = f(q, 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} \delta q^\star = \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\mathrm{argmin}} \; &\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."
1. Tao Pang, Russ Tedrake, "A Convex Quasistatic Time-stepping Scheme for Rigid Multibody Systems with Contact and Friction", ICRA, 2021.

q_+ = q + \delta q^\star
hK(q^\mathrm{a} + \delta q^\mathrm{a} - u) = -\lambda_1 + \lambda_2

Force balance of the robot.

0 = \lambda_1 - \lambda_3

Force balance of the object.

\phi_i(q + \delta q) \geq 0, \; \forall i

Non-penetration.

\lambda_i \geq 0 \; \forall i

"Contact forces cannot pull."

\phi (q + \delta q) \lambda = 0, \forall i

"Contact force needs contact."

# Quasi-static dynamics is good for manipulation planning!

• Spatial benefit: Smaller state space ($$[q, v]$$ vs. only $$q$$).
• Temporal benefit: Ignoring transients => looking into the future with fewer steps.

• Many manipulation tasks are quasi-static.

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

Second-order Dynamics

Quasi-static Dynamics

# 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.
\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 Matt 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 general systems with 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!
\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},
• $$\mathbf{A}, \mathbf{B}$$ can then be computed by applying the Implicit Function Theorem to the active constraints at optimality.
• Standard practice for many differentiable simulators.
• Allows first-order randomized smoothing.

# Analytic Smoothing

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 - \sum_i\frac{1}{\kappa}\log{\phi_i(q + \delta q)} \end{aligned}
\Leftrightarrow
\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})
\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

### Smoothed Dynamics

\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 + \sum_i I_-\left(\phi_i (q + \delta q) \right) \end{aligned}
I_-(a) = \begin{cases} 0, \; a \geq 0 \\ \infty, \; a < 0 \end{cases}
\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}

# Analytic Smoothing with Friction

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

Finding this trajectory requires powering through local minima!

# Sampling-based Motion Planing is Great at Global Exploration

• Rapidly-Exploring Random Tree (RRT):   iteratively builds a tree that fills the state space. [1]
1. , "Planning Algorithms", Cambridge University Press , 2006.

One iteration of RRT (simplified)

\mathcal{C}

(1) Sample subgoal

\mathcal{C}

(2) Find nearest node

\mathcal{C}

(3) Grow towards

# "Nearest" is tricky to define under dynamics constraint.

• Dynamic reachability is essential for efficient exploration. [1]
1. H.J.T. Suh, J. Deacon, Q. Wang, "A Fast PRM Planner for Car-like Vehicles", self-hosted, 2018.

# A distance "metric" based on smoothed linearization

q_+ = f_\rho(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 + \mu_\rho^\mathrm{u}\; | \; \|\delta u\|\leq \varepsilon\right\}

### Smoothed Contact Dynamics

A node in the RRT tree: $$\bar{q} = [\bar{q}^\mathrm{a}, \bar{q}^\mathrm{u}]$$

\newcommand{\DfDx}[2]{\frac{\partial {#1}}{\partial {#2}}} q_+ = \underbrace{\mathbf{B}_\rho(\bar{q}, \bar{q}^\mathrm{a})}_{\DfDx{f_\rho}{u}(\bar{q},\bar{q}^\mathrm{a})} \underbrace{\delta u}_{(u - \bar{q}^\mathrm{a})} + \underbrace{\mu_\rho}_{f_\rho(\bar{q}, \bar{q}^\mathrm{a})}

Take the rows corresponding to the object:

\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) \\ \end{aligned}

Smoothed

Smoothed Input Linearization:

$$\varepsilon$$ sub-level set

\mathcal{C}^\mathrm{u}

configuration space of the object.

\newcommand{\DfDx}[2]{\frac{\partial {#1}}{\partial {#2}}} q_+^\mathrm{u} = \mathbf{B}^\mathrm{u}_\rho(\bar{q}, \bar{q}^\mathrm{a}) \delta u + \mu_\rho^\mathrm{u}

un-actauted(objects)

# $$d_\rho^\mathrm{u}(\cdot; \bar{q})$$ locally reflects dynamic reachability

\mathcal{R}_{\rho, \varepsilon}^\mathrm{u} (\bar{q})\coloneqq \left\{\mathbf{B}_\rho^\mathrm{u}(\bar{q},\bar{q}^{\mathrm{a}}) \delta u + \mu^\mathrm{u}_\rho\; | \; \|\delta u\|\leq \varepsilon\right\}
\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) \\ \end{aligned}

$$\varepsilon$$ sub-level set

# RRT through contact (so far)

One iteration of RRT through Contact

(1) Sample subgoal $$\blacktriangle$$

\mathcal{C}^\mathrm{u}
\mathcal{C}^\mathrm{u}

(2) Find nearest node

### Dynamically-consistent Extension

\newcommand{\DfDx}[2]{\frac{\partial {#1}}{\partial {#2}}} q_+^\mathrm{u} = \mathbf{B}^\mathrm{u}_\rho \delta u + \mu_\rho^\mathrm{u}

is only valid locally.

\begin{aligned} \delta u^\star = \underset{\delta u}{\mathrm{argmin}} &\|\mathbf{B}^\mathrm{u}_\rho \delta u + \mu_\rho^\mathrm{u} - q^\mathrm{u}_\blacktriangle \|^2, \; \text{s.t.} \\ & \|\delta u\| \leq \varepsilon \end{aligned}
f(\bar{q}, \bar{q}^\mathrm{a} + \delta u^\star)
\bar{q}
\mathcal{C}^\mathrm{u}

(3) Grow towards $$\blacktriangle$$

# Introducing contact sampling

One iteration of RRT through contact, with contact sampling

(1) Sample a different grasp ($$q^\mathrm{a}$$) for one of the nodes, giving a new distance metric

\mathcal{C}^\mathrm{u}

### Contact sampling allows global exploration of $$q^\mathrm{a}$$ on the contact manifold!

\mathcal{C}^\mathrm{u}

(2) Find nearest node

\mathcal{C}^\mathrm{u}

(3) Grow towards

# Contact-sampler is system-specific

• Treat each finger as a robot arm and solve IK.
• With the hand open, pick a random direction to close the fingers until contact.

# 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: global exploration of robot configuration near the contact manifold.
• Dynamic-consistent extension: edges respect non-smooth quasi-static dynamics.

### 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) \\ \end{aligned}

# What's next?

### Locomotion

[Boston Dynamics]
PV = nRT

### Contact-rich Manipulation

Quasi-static contact dynamics?

• Full second-order dynamics?
• Second-order centroidal dynamics?

# Quasi-static dynamics is good for manipulation planning!

• Spatial benefit: Smaller state space ($$[q, v]$$ vs. only $$q$$).
• Temporal benefit: Ignoring transients => looking into the future with fewer steps.

• Many manipulation tasks are quasi-static.

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

Second-order Dynamics

Quasi-static Dynamics

q^\mathrm{a}_+
u

# RRT through contact (so far)

One iteration of RRT through Contact

\mathcal{C}^\mathrm{u}

(3) Grow towards

But only take small actions.

: q^\mathrm{u}_\mathrm{goal}
: q^\mathrm{u}_\mathrm{start}

Normalized ellipsoid volume

q^\mathrm{u} \coloneqq [x^\mathrm{u}],\; q^\mathrm{a} \coloneqq [x^\mathrm{a}, y^\mathrm{a}].

(1) Sample subgoal

\mathcal{C}^\mathrm{u}
\mathcal{C}^\mathrm{u}

(2) Find nearest node

# Introducing contact sampling

One iteration of RRT through contact, with contact sampling

(1) Sample a different grasp ($$q^\mathrm{a}$$) for one of the nodes, giving a new distance metric

\mathcal{C}^\mathrm{u}

### Contact sampling allows global exploration of $$q^\mathrm{a}$$ on the contact manifold.

q^\mathrm{u} \coloneqq [x^\mathrm{u}],\; q^\mathrm{a} \coloneqq [x^\mathrm{a}, y^\mathrm{a}].
: q^\mathrm{u}_\mathrm{goal}
: q^\mathrm{u}_\mathrm{start}

Normalized ellipsoid volume

\mathcal{C}^\mathrm{u}

(2) Find nearest node

\mathcal{C}^\mathrm{u}

(3) Grow towards

But only take small actions.

# The modified RRT works well for contact-rich tasks!

Planning wall-clock time (seconds)

# Why is contact-rich planning hard?

### (b)

q^\mathrm{u}_+ - q^\mathrm{u}
u - q^\mathrm{u}

No contact

Contact

q^\mathrm{u}_+ - q^\mathrm{u}
u - q^\mathrm{u}

No contact

Contact

# Global Search with Contact Modes

q^\mathrm{u}_+
u

No contact

Contact

\underset{u}{\mathrm{minimize}} \; \frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2
-\frac{\partial }{\partial u} \left(\frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2\right) = (q^\mathrm{u}_\text{goal} - q^\mathrm{u}_+) \frac{\partial q_+^\mathrm{u}}{\partial u}

u

No contact

Contact

q^\mathrm{u}_+
\frac{\partial q_+^\mathrm{u}}{\partial u} = 0
\frac{\partial q_+^\mathrm{u}}{\partial u} = 1
\frac{\partial q_+^\mathrm{u}}{\partial u} = 1
\frac{\partial q_+^\mathrm{u}}{\partial u} = 0
q^\mathrm{a}
u
q^\mathrm{u}
q^\mathrm{a}
u
q^\mathrm{u}

## (b)

q^\mathrm{u}_+
u

No contact

Contact

u

No contact

Contact

q^\mathrm{u}_+
\frac{\partial q_+^\mathrm{u}}{\partial u} = 0
\frac{\partial q_+^\mathrm{u}}{\partial u} = 1
\frac{\partial q_+^\mathrm{u}}{\partial u} = 1
\frac{\partial q_+^\mathrm{u}}{\partial u} = 0

# Why is contact-rich planning hard?

### Two solutions

Descend with gradients of smoothed dynamics

Reason explicitly about contact mode transitions

q^\mathrm{a}_+
u

Contact

No Contact

• Non-linear optimization can scale to complex systems.
• But descent methods get stuck in local minima.
• Good at escaping local minima,
• But mode enumeration scales poorly (exponentially) with the number of contacts.
q^\mathrm{u}_+
u

No contact

Contact

Push Left

Do not Push Left

Push Right

Do not Push Right