Thesis Defense (XX)

H.J. Terry Suh, MIT

Leveraging Structure for Efficient and Dexterous Contact-Rich Manipulation

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

Part 2. Local Planning / Control via Dynamic Smoothing

Part 1. Understanding RL with Randomized Smoothing

Introduction

[TRO 2023, TRO 2025]

Introduction

Why contact-rich manipulation?

What has been done, and what's lacking?

What is manipulation?

How hard could this be? Just pick and place!

Rigid-body manipulation: Move object from pose A to pose B

Manipulation is NOT pick & place

[ZH 2022]

[SKPMAT 2022]

[PSYT 2022]

[ST 2020]

Manipulation: An Important Problem

Matt Mason, "Towards Robotic Manipulation", 2018

Manipulation: An Important Problem

Matt Mason, "Towards Robotic Manipulation", 2018

Manipulation is a core capability for robots to broaden the spectrum of things we can automate in this world.

DexAI

TRI Dishwasher Demo

Flexxbotics

ETH Zurich, Robotic Systems Lab

What is Contact-Rich Manipulation?

Why should there be an inherent value in studying

manipulation that is rich in contact?

What is Contact-Rich Manipulation?

Why should there be an inherent value in studying

manipulation that is rich in contact?

Matt Mason, "Towards Robotic Manipulation", 2018

Manipulation

What is Contact-Rich Manipulation?

Matt Mason, "Towards Robotic Manipulation", 2018

Manipulation

What is Contact-Rich Manipulation?

Matt Mason, "Towards Robotic Manipulation", 2018

Manipulation

What is Contact-Rich Manipulation?

Matt Mason, "Towards Robotic Manipulation", 2018

Manipulation

Where do I make contact?
Where should I avoid contact?
With what timing?
What do I do after making contact?

What is Contact-Rich Manipulation?

Matt Mason, "Towards Robotic Manipulation", 2018

Manipulation

Where do I make contact?
Where should I avoid contact?
With what timing?
What do I do after making contact?

As possibilities become many, strategic decisions become harder.

What is Contact-Rich Manipulation?

Manipulation

Manipulation that strategically makes decisions about contact after considering multiple possible ways of selecting contact, especially when such possibilities are many.

Contact-Rich Manipulation

H.J. Terry Suh, PhD Thesis

Matt Mason, "Towards Robotic Manipulation", 2018

What is Contact-Rich Manipulation?

Manipulation

Manipulation that strategically makes decisions about contact after considering multiple possible ways of selecting contact, especially when such possibilities are many.

Contact-Rich Manipulation

H.J. Terry Suh, PhD Thesis

Matt Mason, "Towards Robotic Manipulation", 2018

Doesn't necessarily have to behaviorally result in many contacts.

What is Contact-Rich Manipulation?

Manipulation

Manipulation that strategically makes decisions about contact after considering multiple possible ways of selecting contact, especially when such possibilities are many.

Contact-Rich Manipulation

H.J. Terry Suh, PhD Thesis

Matt Mason, "Towards Robotic Manipulation", 2018

Doesn't necessarily have to behaviorally result in many contacts.
Shouldn't artificially restrict possible contacts from occuring.

Case 1. Whole-Body Manipulation

Do not make contact

(Collision-free Motion Planning)

Make Contact

Human

Robot

Slide inspired by Tao Pang's Thesis Defense & TRI Punyo Team

Case 2. Dexterous Manipulation

Human

Robot

Wonik Allegro Hand

Why Contact-Rich Manipulation?

A capable embodied intelligence must be able to exploit its physical embodiment to the fullest.

Manipulation that strategically makes decisions about contact after considering multiple possible ways of selecting contact, especially when such possibilities are many.

Contact-Rich Manipulation

H.J. Terry Suh, PhD Thesis

[TRO 2023, TRO 2025]

Introduction

Why contact-rich manipulation?

What has been done, and what's lacking?

[TRO 2023, TRO 2025]

Introduction

What has been done, and what's lacking?

Why contact-rich manipulation?

Fundamental Challenges

Cost

Fundamental Challenges

Flatness and Stiffness spells difficulty for local gradient-based optimizers.

Cost

Smooth Analysis

But GIVEN a contact event, the landscape is smooth.

Cost

[Salisbury Hand, 1982]

[Murray, Li, Sastry]

What can we do given a contact mode?

Smooth Planning & Control

Combinatorial Optimization

Cost

Can we try to globally search for the right "piece"?

Hybrid Dynamics & Combinatorial Optimization

Contact dynamics as a "hybrid system" with continuous and discrete components.

[Sreenath et al., 2009]

Hybrid Dynamics & Combinatorial Optimization

[Hogan et al., 2009]

[Graesdal et al., 2024]

Beautiful results in locomotion & manipulation

Problems with Mode Enumeration

System

Number of Modes

\begin{aligned} N = 2 \end{aligned}

\begin{aligned} N = 3^{\binom{9}{2}} \end{aligned}

No Contact

Sticking Contact

Sliding Contact

Number of potential active contacts

Problems with Mode Enumeration

System

Number of Modes

\begin{aligned} N = 3^{\binom{20}{2}} \approx 4.5 \times 10^{90} \end{aligned}

The number of modes scales terribly with system complexity

\begin{aligned} N = 2 \end{aligned}

\begin{aligned} N = 3^{\binom{9}{2}} \end{aligned}

No Contact

Sticking Contact

Sliding Contact

Number of potential active contacts

Contact Relaxation & Smooth NLP

[Posa et al., 2015]

[Mordatch et al., 2012]

Reinforcement Learning

[OpenAI et al., 2019]

[Handa et al., 2024]

Reinforcement Learning

[OpenAI et al., 2019]

Summary of Previous Approaches

Classical Mechanics

Contact Scalability

Efficiency

Global Scope

Summary of Previous Approaches

Classical Mechanics

Hybrid Dynamics &

Combinatorial Optimization

Contact Scalability

Efficiency

Global Scope

Summary of Previous Approaches

Classical Mechanics

Hybrid Dynamics &

Combinatorial Optimization

Contact Relaxation &

Smooth Nonlinear Programming

Contact Scalability

Efficiency

Global Scope

Summary of Previous Approaches

Classical Mechanics

Hybrid Dynamics &

Combinatorial Optimization

Contact Relaxation &

Smooth Nonlinear Programming

Reinforcement Learning

Contact Scalability

Efficiency

Global Scope

Summary of Previous Approaches

Classical Mechanics

Hybrid Dynamics &

Combinatorial Optimization

Contact Relaxation &

Smooth Nonlinear Programming

Reinforcement Learning

Contact Scalability

Efficiency

Global Scope

How can we achieve all three criteria?

Introduction

What has been done, and what's lacking?

Why contact-rich manipulation?

Part 3. Global Planning for Contact-Rich Manipulation

Part 2. Local Planning / Control via Dynamic Smoothing

Part 1. Understanding RL with Randomized Smoothing

Introduction

Preview of Results

Our Method

Contact Scalability

Efficiency

Global Planning

60 hours, 8 NVidia A100 GPUs

5 minutes, 1 Macbook CPU

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

Part 2. Local Planning / Control via Dynamic Smoothing

Part 1. Understanding RL with Randomized Smoothing

Introduction

[TRO 2023, TRO 2025]

Part 1. Understanding RL with Randomized Smoothing

Why does RL

perform well?

Why is it considered inefficient?

Can we do better with more structure?

[RA-L 2021, ICML 2022]

[TRO 2023, TRO 2025]

Part 1. Understanding RL with Randomized Smoothing

Why does RL

perform well?

Why is it considered inefficient?

Can we do better with more structure?

[RA-L 2021, ICML 2022]

How does RL search through these difficult problems?

Original Problem

\begin{aligned} \min_x F(x) \end{aligned}

[ICML 2022]

Cost

\begin{aligned} F(x) \end{aligned}

How does RL search through these difficult problems?

Randomized Smoothing

\begin{aligned} \min_x \mathbb{E}_w F(x+w) \end{aligned}

[ICML 2022]

\begin{aligned} F(x) \end{aligned}

\begin{aligned} \mathbb{E}_wF(x+w) \end{aligned}

How does RL search through these difficult problems?

Randomized Smoothing

\begin{aligned} \min_x \mathbb{E}_w F(x+w) \end{aligned}

[ICML 2022]

\begin{aligned} F(x) \end{aligned}

\begin{aligned} \mathbb{E}_wF(x+w) \end{aligned}

Noise regularizes difficult landscapes,

allevates flatness and stiffness,

abstracts contact modes.

[TRO 2023, TRO 2025]

Part 1. Understanding RL with Randomized Smoothing

Why does RL

perform well?

Why is it considered inefficient?

Can we do better with more structure?

[RA-L 2021, ICML 2022]

How does RL search through these difficult problems?

Randomized Smoothing

\begin{aligned} \min_x \mathbb{E}_w F(x+w) \end{aligned}

[ICML 2022]

\begin{aligned} F(x) \end{aligned}

\begin{aligned} \mathbb{E}_wF(x+w) \end{aligned}

But how do we take gradients of a stochastic function?

\nabla_x \mathbb{E}_w F(x+w)

Estimation of Gradients with Monte Carlo

\begin{aligned} & \nabla_x\mathbb{E}_{w\sim\rho}F(x+{w}) \\ = & \min_{\mathbf{J},\mu}\mathbb{E}_{w\sim\rho}\|F(x + w) - (\mathbf{J}w +\mu)\|^2 \\ \approx & \frac{1}{N}\sum^N_{i=1} \|F(x + w_i) - (\mathbf{J}w_i +\mu)\|^2 \end{aligned}

Zeroth-Order Gradient Estimator

Stein Gradient Estimator
REINFORCE
Score Function / Likelihood Ratio Estimator

Estimation of Gradients with Monte Carlo

Zeroth-Order Gradient Estimator

Stein Gradient Estimator
REINFORCE
Score Function / Likelihood Ratio Estimator

Estimation of Gradients with Monte Carlo

Zeroth-Order Gradient Estimator

Stein Gradient Estimator
REINFORCE
Score Function / Likelihood Ratio Estimator

Estimation of Gradients with Monte Carlo

Zeroth-Order Gradient Estimator

Stein Gradient Estimator
REINFORCE
Score Function / Likelihood Ratio Estimator

But what if we had access to gradients?

Leveraging Differentiable Physics

First-Order Randomized Smoothing

\begin{aligned} & \frac{\partial}{\partial x}\mathbb{E}_{w\sim\rho}F(x+{w}) \\ = & \mathbb{E}_{w\sim\rho}\frac{\partial F}{\partial x}(x + w) \\ \approx & \frac{1}{N}\sum^N_{i=1} \frac{\partial F}{\partial x}(x + w_i) \end{aligned}

Gradient Sampling Algorithm

From John Duchi's Slides on Randomized Smoothing, 2014

Leveraging Differentiable Physics

First-Order Randomized Smoothing

Gradient Sampling Algorithm

From John Duchi's Slides on Randomized Smoothing, 2014

So which one should we use?

Comparison of Efficiency

Bias

Variance

First-Order Estimator

Zeroth-Order Estimator

\nabla_x \mathbb{E}_w F(x+w)

Comparison of Efficiency

Analytic Expression

First-Order Gradient Estimator

Zeroth-Order Gradient Estimator

Requires differentiability over dynamics
Generally lower variance.

Least requirements (blackbox)
High variance.

Possible for only few cases

Structure

Efficiency

\nabla_x \mathbb{E}_w F(x+w)

[TRO 2023, TRO 2025]

Part 1. Understanding RL with Randomized Smoothing

Why does RL

perform well?

Why is it considered inefficient?

Can we do better with more structure?

[RA-L 2021, ICML 2022]

Bias

Variance

First-Order Estimator

Zeroth-Order Estimator

Comparison of Efficiency

Analytic Expression

First-Order Gradient Estimator

Zeroth-Order Gradient Estimator

Requires differentiability over dynamics
Generally lower variance.

Least requirements (blackbox)
High variance.

Possible for only few cases

Structure

Efficiency

\nabla_x \mathbb{E}_w F(x+w)

Can we transfer this promise to RL?

Leveraging Differentiable Physics

Do Differentiable Simulators Give Better Gradients?

Leveraging Differentiable Physics

Do Differentiable Simulators Give Better Gradients?

Bias of First-Order Estimation

\begin{aligned} \nabla_x \mathbb{E}_w F(x + w) & = \mathbb{E}_w F(x + w) = \frac{1}{N}\sum^N_{i=1}\nabla_x F(x + w_i) \end{aligned}

Bias of First-Order Estimation

Variance of First-Order Estimation

First-order Estimators CAN have more variance than zeroth-order ones.

Variance of First-Order Estimation

\phi

- No force when not in contact

- Spring-damper behavior when in contact

Variance of First-Order Estimation

\phi

- No force when not in contact

- Spring-damper behavior when in contact

Variance of First-Order Estimation

\phi

- No force when not in contact

- Spring-damper behavior when in contact

Prevalent for models that approximate contact through spring-dampers!

Gradients

Gradients

Bias

Variance

Common lesson from stochastic optimization:

1. Both are unbiased under sufficient regularity conditions

2. First-order generally has less variance than zeroth order.

Bias

Variance

Bias

Variance

1st Pathology: First-Order Estimators CAN be biased.

2nd Pathology: First-Order Estimators can have MORE

variance than zeroth-order.

Stiffness still hurts us in improving performance and efficiency

[TRO 2023, TRO 2025]

Part 1. Understanding RL with Randomized Smoothing

Why does RL

perform well?

Why is it considered inefficient?

Can we do better with more structure?

[RA-L 2021, ICML 2022]

Bias

Variance

First-Order Estimator

Zeroth-Order Estimator

Bias

Variance

First-Order Estimator

Zeroth-Order Estimator

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

Part 2. Local Planning / Control via Dynamic Smoothing

Part 1. Understanding RL with Randomized Smoothing

Introduction

[TRO 2023, TRO 2025]

Part 2. Local Planning / Control via Dynamic Smoothing

Can we do better by understanding dynamics structure?

How do we build effective local optimizers for contact?

Utilizing More Structure

\lambda = k\min(\phi,0) + d\min(\phi,0)v

\phi

- No force when not in contact

- Spring-damper behavior when in contact

Tackling stiffness requires us to rethink about the models we use to simulate contact

Intuitive Physics of Contact

Where will the box move next?

Intuitive Physics of Contact

How did we know? Did we really have to integrate stiff springs?

Intuitive Physics of Contact

How did we know? Did we really have to integrate stiff springs?

Constraint-Driven Simulations can simulate longer horizon behavior

Intuitive Physics of Contact

Did we think about velocities at all? Or purely reason about configuration?

Quasistatic Modeling & Optimization-Based Sim.

Optimization-Based Simulation

Quasistatic Modeling

[Stewart & Trinkle, 2000]

[Mujoco, Todorov 2012]

[SAP Solver (Drake), CPH, 2022]

[Dojo, HCBKSM 2022]

[Howe & Cutkosky 1996]

[Lynch & Mason 1996]

[Halm & Posa 2018]

[Pang & Tedrake 2021]

CQDC: A Quasi-dynamic Simulator for Manipulation

Dr. Tao Pang

Robot commands as

State only comprises of configurations

- Actuated configurations (Robot)

- Unactuated configurations (Object)

q^\mathrm{u}

q^\mathrm{a}

- Position command to an stiffness controller

Convex

Quasidynamic

Differentiable

Contact Model

CQDC: A Quasi-dynamic Simulator for Manipulation

\begin{aligned} \min_{{\color{red}q^\mathrm{u}_+}} &\;\;\frac{1}{2}({\color{red}q^\mathrm{u}_+} - q^\mathrm{u})^\top \mathbf{M}_\mathrm{u} ({\color{red}q^\mathrm{u}_+} - q^\mathrm{u}) \\ \text{s.t.} & \;\; \phi(q^\mathrm{u},{\color{blue}q^\mathrm{a}_\text{cmd}}) \geq 0 \end{aligned}

\begin{aligned} q^\mathrm{u} \end{aligned}

\begin{aligned} {\color{red}q^\mathrm{u}_+} \end{aligned}

Non-Penetration

Minimum Energy Principle

CQDC: A Quasi-dynamic Simulator for Manipulation

\begin{aligned} \min_{q+} \quad \frac{1}{2}q_+^\top \mathbf{P} q_+ + b^\top q \\ \text{subject to} \quad \mathbf{J}_i q_+ + c_i \in \mathcal{K}_i \end{aligned}

Second-Order Cone Program (SOCP)

We can use standard SOCP solvers to solve this program

Uses Drake as Backbone

Allows us to separate complexity of "contact" from complexity of "highly dynamic".

CQDC Simulator

Sensitivity Analysis

Spring-damper modeling

Quasi-static Dynamics

\begin{aligned} \frac{\partial x_T}{\partial x_0}=\frac{\partial x_T}{\partial x_{T-1}}\cdots \frac{\partial x_1}{\partial x_0} \end{aligned}

\begin{aligned} \frac{\partial x_T}{\partial x_0} \end{aligned}

Directly obtained by sensitivity analysis

Jumping from one equilibrium to another lets us compute long horizon gradients temporally, allowing us to be less stiff.

Figures from Tao Pang's Defense, 2023

Constrained Optimization

\begin{aligned} \min_{{\color{red}q^\mathrm{u}_+}} &\;\;\frac{1}{2}({\color{red}q^\mathrm{u}_+} - q^\mathrm{u})^\top \mathbf{M}_\mathrm{u} ({\color{red}q^\mathrm{u}_+} - q^\mathrm{u})\;\;\text{s.t.} \;\; \phi(q^\mathrm{u},{\color{blue}q^\mathrm{a}_\text{cmd}}) \geq 0 \end{aligned}

Constrained Optimization

\begin{aligned} \min_{{\color{red}q^\mathrm{u}_+}} &\;\;\frac{1}{2}({\color{red}q^\mathrm{u}_+} - q^\mathrm{u})^\top \mathbf{M}_\mathrm{u} ({\color{red}q^\mathrm{u}_+} - q^\mathrm{u})\;\;\text{s.t.} \;\; \phi(q^\mathrm{u},{\color{blue}q^\mathrm{a}_\text{cmd}}) \geq 0 \end{aligned}

\infty

Log-Barrier Relaxation

\infty

\begin{aligned} \min_{{\color{red}q^\mathrm{u}_+}} &\;\;\frac{1}{2}({\color{red}q^\mathrm{u}_+} - q^\mathrm{u})^\top \mathbf{M}_\mathrm{u} ({\color{red}q^\mathrm{u}_+} - q^\mathrm{u}) -\log \phi(q^\mathrm{u},{\color{blue}q^\mathrm{a}_\text{cmd}}) \end{aligned}

Log-Barrier Relaxation

\infty

Constraints have an inversely proportional effect to distance.

\begin{aligned} \min_{{\color{red}q^\mathrm{u}_+}} &\;\;\frac{1}{2}({\color{red}q^\mathrm{u}_+} - q^\mathrm{u})^\top \mathbf{M}_\mathrm{u} ({\color{red}q^\mathrm{u}_+} - q^\mathrm{u}) -\log \phi(q^\mathrm{u},{\color{blue}q^\mathrm{a}_\text{cmd}}) \end{aligned}

Barrier-Smoothed Dynamics

Initial Configuration

1-Step Dynamics

(Low Smoothing)

1-Step Dynamics

(High Smoothing)

Barrier smoothing creates force from a distance

Benefits of Smoothing

[ICML 2022]

\begin{aligned} F(x) \end{aligned}

\begin{aligned} \mathbb{E}_wF(x+w) \end{aligned}

Recall smoothing achieves abstraction of contact modes

Benefits of Smoothing

True Dynamics

Local Model

Benefits of Smoothing

Smoothed Dynamics

Local Model

Benefits of Smoothing

Smoothed Dynamics

Local Model

Barrier Smoothing can also abstract contact modes!

But does it have the exact same effect as randomized?

Randomized-Smoothed Dynamics

Initial Configuration

1-Step Dynamics

Sampling

Equivalence of Smoothing Schemes

\rho(w) = \sqrt{\frac{4 c}{(w^\top c w + 4)^3}}

The two smoothing schemes are equivalent!

(There is a distribution that corresponds to barrier smoothing)

Randomized Smoothing

Barrier Smoothing

Analytic Expression

First-Order Gradient Estimator

Zeroth-Order Gradient Estimator

Requires differentiability over dynamics
Generally lower variance.

Least requirements (blackbox)
High variance.

Possible for only few cases

Structure

Efficiency

Comparison of Efficiency

[TRO 2023, TRO 2025]

Part 2. Local Planning / Control via Dynamic Smoothing

Can we do better by understanding dynamics structure?

How do we build effective local optimizers for contact?

Back to Dynamics

\begin{aligned} q^\mathrm{u}_{next} = f(q,u) \end{aligned}

True Dynamics

How do we build a computationally tractable local model for iterative optimization?

True Dynamics

Back to Dynamics

\begin{aligned} q^\mathrm{u}_{next} = \frac{\partial f}{\partial u}\delta u + f(q,u) \end{aligned}

Is this a good local model?

Local Model

First-Order Taylor Approximation

Gradients too myopic, suffers from flatness

Back to Dynamics

Local Model

Is this a good local model?

Gradients are more useful due to smoothing

First-Order Taylor Approximation

on Smoothed Dynamics

\begin{aligned} q^\mathrm{u}_{next} = \frac{\partial f_{\color{red}\rho}}{\partial u}\delta u + f_{\color{red}\rho}(q,u) \end{aligned}

Back to Dynamics

Local Model

Is this a good local model?

First-Order Taylor Approximation

on Smoothed Dynamics

Gradients are more useful due to smoothing

Still violates some fundamental characteristics of contact!

\begin{aligned} q^\mathrm{u}_{next} = \frac{\partial f_{\color{red}\rho}}{\partial u}\delta u + f_{\color{red}\rho}(q,u) \end{aligned}

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} = f(q, u + \delta u) \end{aligned}

Dynamics

\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

What does this imply about reachable sets?

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} = \frac{\partial f_{\rho}}{\partial u}{\color{green}\delta u} + f_{\rho}(q,u) \end{aligned}

Linear Map

\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

What does this imply about reachable sets?

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} = \frac{\partial f_{\rho}}{\partial u}{\color{green}\delta u} + f_{\rho}(q,u) \end{aligned}

Linear Map

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} \end{aligned}

We can make ellipsoidal approximations of the reachable set

\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

We can make ellipsoidal approximations of the reachable set

True Samples

Samples from Local Model

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} = \frac{\partial f_{\rho}}{\partial u}{\color{green}\delta u} + f_{\rho}(q,u) \end{aligned}

What does this imply about reachable sets?

Linear Map

True Samples

Samples from Local Model

What does this imply about reachable sets?

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} = \frac{\partial f_{\rho}}{\partial u}{\color{green}\delta u} + f_{\rho}(q,u) \end{aligned}

Linear Map

What are we missing here?

True Samples

Samples from Local Model

What does this imply about reachable sets?

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} = \frac{\partial f_{\rho}}{\partial u}{\color{green}\delta u} + f_{\rho}(q,u) \end{aligned}

Linear Map

Qualification on Contact Impulses

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} = \frac{\partial f_{\rho}}{\partial u}{\color{green}\delta u} + f_{\rho}(q,u) \end{aligned}

Dynamics (Primal)

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} \end{aligned}

\begin{aligned} {\color{magenta}\lambda} = \frac{\partial \lambda_{\rho}}{\partial u}{\color{green}\delta u} + \lambda_{\rho}(q,u) \end{aligned}

Contact Impulses (Dual)

\begin{aligned} {\color{magenta}\lambda} \end{aligned}

\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

The Friction Cone

*image taken from Stephane Caron's blog

Unilateral

(Can't pull)

Coulomb Friction

Friction Cone

Qualification on Contact Impulses

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} = \frac{\partial f_{\rho}}{\partial u}{\color{green}\delta u} + f_{\rho}(q,u) \end{aligned}

Dynamics (Primal)

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} \end{aligned}

\begin{aligned} {\color{magenta}\lambda} = \frac{\partial \lambda_{\rho}}{\partial u}{\color{green}\delta u} + \lambda_{\rho}(q,u) \end{aligned}

Contact Impulses (Dual)

\begin{aligned} {\color{magenta}\lambda} \end{aligned}

*image taken from Stephane Caron's blog

\begin{aligned} {\color{magenta}\lambda}\in \{\mu\lambda_n \geq \|\lambda_t\|_2\} \end{aligned}

Contact is unilateral

\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

Feasible Trust Region

Dynamics (Primal)

Contact Impulses (Dual)

\begin{aligned} {\color{magenta}\lambda}\in \{\mu\lambda_n \geq \|\lambda_t\|_2\} \end{aligned}

Trust Region Size

\begin{aligned} \|{\color{green}(\delta q, \delta u)}\|_2\leq \varepsilon \end{aligned}

\begin{aligned} \color{green} \mathcal{T}(q, u) =\{(\delta q, \delta u)\} \end{aligned}

Feasible Trust Region

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} = \frac{\partial f_{\rho}}{\partial u}{\color{green}\delta u} + f_{\rho}(q,u) \end{aligned}

\begin{aligned} {\color{magenta}\lambda} = \frac{\partial \lambda_{\rho}}{\partial u}{\color{green}\delta u} + \lambda_{\rho}(q,u) \end{aligned}

Motion Set

\begin{aligned} \color{green} \mathcal{T}(q, u) =\{(\delta q, \delta u)\} \end{aligned}

Feasible Trust Region

\begin{aligned} \color{blue} \mathcal{M}(q,u)=\{q^\mathrm{u}_{next}\} \end{aligned}

Motion Set

Dynamics (Primal)

Contact Impulses (Dual)

\begin{aligned} {\color{magenta}\lambda}\in \{\mu\lambda_n \geq \|\lambda_t\|_2\} \end{aligned}

Trust Region Size

\begin{aligned} \|{\color{green}(\delta q, \delta u)}\|_2\leq \varepsilon \end{aligned}

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} = \frac{\partial f_{\rho}}{\partial u}{\color{green}\delta u} + f_{\rho}(q,u) \end{aligned}

\begin{aligned} {\color{magenta}\lambda} = \frac{\partial \lambda_{\rho}}{\partial u}{\color{green}\delta u} + \lambda_{\rho}(q,u) \end{aligned}

Contact Impulse Set

\begin{aligned} \color{green} \mathcal{T}(q, u) =\{(\delta q, \delta u)\} \end{aligned}

Feasible Trust Region

\begin{aligned} \color{magenta} \mathcal{C}(q,u)=\{\lambda\} \end{aligned}

Contact Impulse Set

Dynamics (Primal)

Contact Impulses (Dual)

\begin{aligned} {\color{magenta}\lambda}\in \{\mu\lambda_n \geq \|\lambda_t\|_2\} \end{aligned}

Trust Region Size

\begin{aligned} \|{\color{green}(\delta q, \delta u)}\|_2\leq \varepsilon \end{aligned}

\begin{aligned} {\color{blue}q^\mathrm{u}_{next}} = \frac{\partial f_{\rho}}{\partial u}{\color{green}\delta u} + f_{\rho}(q,u) \end{aligned}

\begin{aligned} {\color{magenta}\lambda} = \frac{\partial \lambda_{\rho}}{\partial u}{\color{green}\delta u} + \lambda_{\rho}(q,u) \end{aligned}

Now we have a close match with real samples

Motion Set

No Constraints

True Samples

Motion Set

No Constraints

True Samples

Closer to ellipse when there is a full grasp

The gradients take into account manipulator singularities

Motion Set

No Constraints

True Samples

Motion Set

No Constraints

True Samples

Can also add additional convex constraints (e.g. joint limits)

Finding Optimal Action: One Step

\begin{aligned} \min_{\delta u} \quad & \|q^\mathrm{u}_{goal} - q^\mathrm{u}_+\|^2 + \|\delta u\|^2 \\ \text{s.t.} \quad & q_+ \in \mathcal{M}_\varepsilon(q,u) \end{aligned}

Get to the goal

Minimize effort

Motion Set Constraint

\begin{aligned} \color{blue} \mathcal{M}(q,u)=\{q^\mathrm{u}_{next}\} \end{aligned}

\begin{aligned} \color{red} q^\mathrm{u}_{goal} \end{aligned}

\begin{aligned} \color{blue} q \end{aligned}

\begin{aligned} \delta u \end{aligned}

\begin{aligned} \min_{\delta u_{0:T-1},\delta q_{0:T}} \quad & \|q^\mathrm{u}_{goal} - q^\mathrm{u}_T\|^2 + \sum^{T-1}_{t=0}\|u_{t+1} - u_t\|^2 \\ \text{s.t.} \quad & q_{t+1}\in \mathcal{M}_\varepsilon(q_t,u_t) \\ & q_0 = q_{initial} \\ & u_t = \bar{u}_t + \delta u_t \quad\quad\forall t\in\{0,\cdots,T-1\} \end{aligned}

Get to the goal

Minimize effort

Multi-Horizon Optimization

Motion Set Constraint

Trajectory Optimization: Step 1

q_0

q_1

q_2

q_3

q_4

q_5

q_6

u_0

u_1

u_2

u_3

u_4

u_5

Roll out an input trajectory guess to obtain an initial trajectory.

u_{0:T-1}

q_\mathrm{goal}

Trajectory Optimization: Step 2

q_0

q_1

q_2

q_3

q_4

q_5

q_6

u_0

u_1

u_2

u_3

u_4

u_5

Linearize around each point to obtain a local dynamics model around the trajectory

(q_t,u_t)

q_\mathrm{goal}

Trajectory Optimization: Step 3

q_0

q_1

q_2

q_3

q_4

q_5

q_6

u_0

u_1

u_2

u_3

u_4

u_5

q_\mathrm{goal}

\delta q_1^\star

u_0 + \delta u_0^\star

u_1 + \delta u_1^\star

q_1 + \delta q_1^\star

\delta q_2^\star

\delta q_3^\star

\delta q_4^\star

\delta q_5^\star

u_2 + \delta u_2^\star

u_3 + \delta u_3^\star

u_4 + \delta u_4^\star

u_5 + \delta u_5^\star

q_2 + \delta q_2^\star

q_3 + \delta q_3^\star

q_4 + \delta q_4^\star

q_5 + \delta q_5^\star

q_6 + \delta q_6^\star

\delta q_6^\star

Use our subproblem to solve for a new input trajectory

u_{0:T-1}^\star = u_{0:T-1} + \delta u^\star_{0:T-1}

Trajectory Optimization: Iteration

q_0

q_1

q_2

q_3

q_4

q_5

q_6

u_0

u_1

u_2

u_3

u_4

u_5

q_\mathrm{goal}

Roll out the new input trajectory guess and repeat til convergence

u_{0:T-1}^\star

Model Predictive Control (MPC)

q_0

q_1

q_2

q_3

q_4

q_5

q_6

u_0

u_1

u_2

u_3

u_4

u_5

Plan a trajectory towards the goal in an open-loop manner

Model Predictive Control (MPC)

q_0

q_1^\mathrm{planned}

u_0

Execute the first action.

Due to model mismatch, there will be differences in where we end up.

q_1^\mathrm{real}

Model Predictive Control (MPC)

q_0

q_1^\mathrm{planned}

u_0

Replan from the observed state,

execute the first action,

and repeat.

q_1^\mathrm{real}

q_2

q_3

q_4

q_5

q_6

u_1

u_2

u_3

u_4

u_5

MPC Rollouts

Able to stabilize to plan to the goal in a contact-rich manner

MPC Rollouts

The controller also successfully scales to the complexity of dexterous hands

Force Control

Having a model of contact impulses allows us to perform force control

\begin{aligned} \min_{\delta u_{0:T-1},\delta q_{0:T}} \quad & \|\lambda_{goal} - \lambda\|^2 + \sum^{T-1}_{t=0}\|u_{t+1} - u_t\|^2 \\ \text{s.t.} \quad & \lambda\in \mathcal{C}_\varepsilon(q_t,u_t) \\ & q_0 = q_{initial} \\ & u_t = \bar{u}_t + \delta u_t \quad\quad\forall t\in\{0,\cdots,T-1\} \end{aligned}

Applied desired forces

Minimize effort

Contact Impulse Set

Force Control

Is the friction cone constraint necessary?

\varepsilon

Hardware Setup

Small IR LEDs which allow motion capture & state estimation

iiwa bimanual bucket rotation

allegro hand in-hand reorientation

Mocap markers

[TRO 2023, TRO 2025]

Part 2. Local Planning / Control via Dynamic Smoothing

Can we do better by understanding dynamics structure?

How do we build effective local optimizers for contact?

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

Part 2. Local Planning / Control via Dynamic Smoothing

Part 1. Understanding RL with Randomized Smoothing

Introduction

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

What are fundamental limits of local control?

How can we address difficult exploration problems?

How do we achieve better global planning?

Reduction to Motion Planning over Smooth Systems

Smoothing lets us...

Abstract contact modes
Gets rid of discreteness coming from granular contact modes.

Contact-Rich Manipulation

Motion Planning over Smooth Systems

\approx

Discreteness of Motion Planning

Marcucci et al., 2022

Motion Planning over Smooth Systems is not necessarily easier

The Exploration Problem

How do we push in this direction?

How do we rotate further in presence of joint limits?

How do we push in this direction?

The Exploration Problem

The Exploration Problem

How do I know that I need to open my finger to grasp the box?

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

What are fundamental limits of local control?

How can we address difficult exploration problems?

How do we achieve better global planning?

The Exploration Problem

Because I have seen that this state is useful!

Local Control

What is a sequence of actions I must take to push the box down?

Local Search for Open-Loop Actions

The Reset Action Space

What is a sequence of actions I must take to push the box down?

Where should I place my finger if I want to make progress towards the goal?

Local Search for Open-Loop Actions

Global Search for Initial Configurations

The Reset Action Space

Utilizing the MPC Cost

Our Local Controller

\begin{aligned} {\color{red}V(q^\mathrm{a}_\mathrm{init}; q^\mathrm{u}_\mathrm{init};q^\mathrm{u}_{goal})\coloneqq} \end{aligned}

How well does the policy perform when this controller is run closed-loop?

\begin{aligned} \min_{\delta u_{0:T-1},\delta q_{0:T}} \quad & \|{\color{red}q^\mathrm{u}_{goal}} - q^\mathrm{u}_T\|^2 + \sum^{T-1}_{t=0}\|u_{t+1} - u_t\|^2 \\ \text{s.t.} \quad & q_{t+1}\in \mathcal{M}_\varepsilon(q_t,u_t) \\ & q_0^\mathrm{u} = {\color{red}q^\mathrm{u}_{init}} \\ & q_0^\mathrm{a} = {\color{red}q^\mathrm{a}_{init}} \\ & u_t = \bar{u}_t + \delta u_t \quad\quad\forall t\in\{0,\cdots,T-1\} \end{aligned}

Get to the goal

Minimize effort

Motion Set Constraint

The Actuator Relocation Problem

\begin{aligned} \min_{q^\mathrm{a}_\mathrm{init}} \quad & V^\pi(q^\mathrm{a}_\mathrm{init}; q^\mathrm{u}_\mathrm{init};q^\mathrm{u}_{goal}) \\ \text{s.t.} \quad & \phi_i(q^\mathrm{a}_\mathrm{init}, q^\mathrm{u}_\mathrm{init})\geq 0 \end{aligned}

Choose best initial actuator configuration for MPC

Non-penetration constraints

Where should I place my finger if I want to make progress towards the goal?

Global Search for Initial Configurations

Robustness Considerations

What is a better configuration to start from?

Robustness Considerations

[Ferrari & Canny, 1992]

We prefer configurations that can better reject disturbances

Regularizing with Robustness

\begin{aligned} \min_{q^\mathrm{a}_\mathrm{init}} \quad & V^\pi(q^\mathrm{a}_\mathrm{init}; q^\mathrm{u}_\mathrm{init};q^\mathrm{u}_{goal}) + {\color{red} \alpha r(q^\mathrm{a}_{init};q^\mathrm{u}_\mathrm{init})^2} \\ \text{s.t.} \quad & \phi_i(q^\mathrm{a}_\mathrm{init}, q^\mathrm{u}_\mathrm{init})\geq 0\\ & q^\mathrm{a}_\mathrm{lb} \leq q^\mathrm{a}_\mathrm{init} \leq q^\mathrm{a}_\mathrm{ub} \end{aligned}

How do we efficiently find an answer to this problem?

The Actuator Relocation Problem

Landscape of the value function is much more complex,

so we resort to sampling-based search

Grasp Synthesis Procedure

Sampling-Based Search on

Reduced-Order System

Differential Inverse Kinematics to find Hand Configuration

Verification with MPC rollouts

Grasp Synthesis Results

Pitch + 90 degrees

Yaw + 45 degrees

Pitch - 90 degrees

Konidaris & Barto,

Skill Chaining (2009)

Primitive View on Grasp Synthesis + MPC

TLP & LPK (2014)

Toussaint

Logic-Geometric Programming (2015)

We have introduced discreteness by defining a primitive

\begin{aligned} \textrm{Move}(q^\mathrm{u}_\mathrm{init}, q^\mathrm{u}_\mathrm{goal}) \end{aligned}

TM & JU & PP & RT

Graph of Convex Sets (2023)

Tedrake,

LQR-Trees (2009)

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

What are fundamental limits of local control?

How can we address difficult exploration problems?

How do we achieve better global planning?

Chaining Local Actions

q^\mathrm{a}

q^\mathrm{u}_x

q^\mathrm{u}_y

q_0

q_1

q_1'

q_2

MPC

q_2^\mathrm{u}

q_0^\mathrm{u}

q_1^\mathrm{u}

Regrasp

(Collision-Free Motion Planning)

q^\mathrm{a}

q^\mathrm{u}_x

q^\mathrm{u}_y

q_0

q_1

q_1'

q_2

MPC

q_2^\mathrm{u}

q_0^\mathrm{u}

q_1^\mathrm{u}

Regrasp

(Collision-Free Motion Planning)

Chaining Local Actions

Conditions for the Framework

Object is stable during regrasp (Acts like a predicate for the move primitive)
Local controller is able to correct for small errors while chaining actions together

q^\mathrm{a}

q^\mathrm{u}_x

Roadmap Approach to Manipulation

Offline, build a roadmap of object configuration and keep collection of nominal plans as edges

q^\mathrm{u}_y

Roadmap Approach to Manipulation

q^\mathrm{a}

q^\mathrm{u}_x

Roadmap Approach to Manipulation

When getting a new goal that is not in the roadmap, try to connect from nearest node.

q^\mathrm{u}_y

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

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

What are fundamental limits of local control?

How can we address difficult exploration problems?

How do we achieve better global planning?

Part 3. Global Planning for Contact-Rich Manipulation

Part 2. Local Planning / Control via Dynamic Smoothing

Part 1. Understanding RL with Randomized Smoothing

Introduction

Preview of Results

Our Method

Contact Scalability

Efficiency

Global Planning

60 hours, 8 NVidia A100 GPUs

5 minutes, 1 Macbook CPU