H.J. Terry Suh

 

Leveraging Structure for Efficient and Dexterous Contact-Rich Manipulation

Thesis Defense

Committee: Russ Tedrake (Advisor),

Tomas Lozano-Perez, Marc Toussaint, Justin Carpentier

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

Part 2. Local Planning for Contact-Rich Manipulation

Part 1. Contact Mode Abstraction via Smoothing

Introduction

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?

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

What is Contact-Rich 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

What is Contact-Rich Manipulation?

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

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

What is Contact-Rich Manipulation?

As possibilities become many, strategic decisions become harder.

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

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

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

Make Contact

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

Fundamental Challenges

\theta
\theta

Fundamental Challenges

\theta
\theta

Fundamental Challenges

\theta
\theta

Fundamental Challenges

\theta
\theta

Fundamental Challenges

\theta
\theta

Fundamental Challenges

\theta
\theta

Fundamental Challenges

\theta
\theta

Fundamental Challenges

\theta
\theta

Fundamental Challenges

\theta
\theta

Smooth Analysis

\theta
\theta

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

Smooth Analysis

But GIVEN a contact event, the landscape is smooth.

\theta
\theta

[Salisbury Hand, 1982]

[Murray, Li, Sastry]

What can we do given a contact mode?

Smooth Planning & Control

Combinatorial Optimization

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

\theta
\theta

Hybrid Dynamics & Combinatorial Optimization

[Hogan et al., 2019]

[Graesdal et al., 2024]

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., 2013]

[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

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?

Preview of Results

Our Method

Contact Scalability

Efficiency

Global Scope

60 hours, 8 NVidia A100 GPUs

5 minutes, 1 Macbook M3 CPU

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

Part 2. Local Planning for Contact-Rich Manipulation

Part 1. Contact Mode Abstraction via Smoothing

Introduction

[TRO 2023, TRO 2025]

Part 1. Contact Mode Abstraction via Smoothing

Introduction

Part 3. Global Planning for Contact-Rich Manipulation

Part 2. Local Planning for Contact-Rich Manipulation

[TRO 2023, TRO 2025]

Part 1. Contact Mode Abstraction via Smoothing

What allows abstraction of contact modes?

How can we achieve it efficiently?

[RA-L 2021, ICML 2022, TRO 2023]

Summary of Previous Approaches

Classical Mechanics

Hybrid Dynamics &

Combinatorial Optimization

Contact Relaxation &

Smooth Nonlinear Programming

Reinforcement Learning

Contact Scalability

Efficiency

Global Scope

How do these methods abstract contact modes?

Reinforcement Learning

Stochastic & Discrete

Deterministic & Continuous

Reinforcement Learning

Stochastic & Discrete

Deterministic & Continuous

But why does RL actually work on

continuous, seemingly deterministic problems?

Reinforcement Learning

Stochastic & Discrete

Deterministic & Continuous

RL considers a stochastic objective / policy

[RA-L 2021, ICML 2022]

How does RL abstract contact modes?

\theta

[ICML 2022]

\theta

How does RL abstract contact modes?

\theta

[ICML 2022]

How does RL abstract contact modes?

\theta

[ICML 2022]

How does RL abstract contact modes?

\theta

[ICML 2022]

How does RL abstract contact modes?

\theta

[ICML 2022]

How does RL abstract contact modes?

\theta

[ICML 2022]

How does RL abstract contact modes?

\theta

[ICML 2022]

How does RL abstract contact modes?

\theta

[ICML 2022]

How does RL abstract contact modes?

\theta

[ICML 2022]

How does RL abstract contact modes?

\theta

[ICML 2022]

Randomized Smoothing

\theta

[ICML 2022]

Noise regularizes difficult landscapes,

allevates flatness and stiffness

Randomized Smoothing

\theta

[ICML 2022]

Noise regularizes difficult landscapes,

allevates flatness and stiffness

[Duchi et al,  2012]

Contact Mode Abstraction via Sampling

Thumb Only

Thumb and index

Ring Only

Thumb, Index, Ring

[RA-L 2021, TRO 2023]

Contact Mode Abstraction via Sampling

Thumb Only

Thumb and index

Ring Only

Thumb, Index, Ring

[RA-L 2021, TRO 2023]

Contact Mode Abstraction via Sampling

Thumb Only

Thumb and index

Ring Only

Thumb, Index, Ring

[RA-L 2021, TRO 2023]

Contact Mode Abstraction via Sampling

Relu 1,5 Active

Relu 0,1,5 Active

ReLU 0,1 Active

ReLU 0 Active

[RA-L 2021, TRO 2023]

Contact Mode Abstraction via Sampling

Deep Reinforcement Learning

Randomized Smoothing in Non-smooth Optimization

[RA-L 2021]

[ICML 2022]

[Duchi et al,  2012]

[Schulman et al,  2015]

[TRO 2023]

[Le Lidec et al, 2024]

[TRO 2023, TRO 2025]

Part 1. Contact Mode Abstraction via Smoothing

What allows abstraction of contact modes?

How can we achieve it efficiently?

[RA-L 2021, ICML 2022, TRO 2023]

Towards Efficiency

How can we abstract contact modes more efficiently?

Towards Efficiency

How can we abstract contact modes more efficiently?

  1. Using Gradients
\nabla

Towards Efficiency

How can we abstract contact modes more efficiently?

  1. Using Gradients
\nabla

2. Without Monte-Carlo

Intuitive Physics of Contact

Where will the box move next?

Intuitive Physics of Contact

Where will the box move next?

Intuitive Physics of Contact

Intuitive Physics of Contact

Intuitive Physics of Contact

Simulations should be constraint-driven

Intuitive Physics of Contact

Intuitive Physics of Contact

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{o}
q^\mathrm{r}

-        Position command to an stiffness controller

u

Convex

Quasidynamic

Differentiable

Contact Model

[TRO 2023]

CQDC: A Quasi-dynamic Simulator for Manipulation

[TRO 2023]

CQDC: A Quasi-dynamic Simulator for Manipulation

[TRO 2023]

CQDC: A Quasi-dynamic Simulator for Manipulation

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

Non-Penetration

Minimum Energy Principle

[TRO 2023]

CQDC as Convex Optimization

[TRO 2023]

\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

\begin{aligned} q^\star_+ \end{aligned}

Sensitivity Analysis

[TRO 2023]

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

Second-Order Cone Program (SOCP)

\begin{aligned} \frac{\partial q^\star_+}{\partial {\color{red}b}} \end{aligned}

Allows gradient computations after solving optimization

Uses Drake as Backbone

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

                 CQDC Simulator

[TRO 2023]

Smoothing of Optimization-Based Simulators

How do we abstract contact for optimization-based simulation?

[TRO 2023]

Thumb Only

Thumb and index

Ring Only

Thumb, Index, Ring

Randomized-Smoothed Dynamics

Initial Configuration

1-Step Dynamics

Sampling

[RA-L 2021, TRO 2023]

Constrained Optimization

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

[TRO 2023]

Constrained Optimization

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

[TRO 2023]

Log-Barrier Relaxation

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

[TRO 2023]

Log-Barrier Relaxation

=
+
\infty
\infty

Constraints have an inversely proportional effect to distance.

[TRO 2023]

\begin{aligned} \min_{{\color{red}q^\mathrm{o}_+}} &\;\;\frac{1}{2}({\color{red}q^\mathrm{o}_+} - q^\mathrm{o})^\top \mathbf{M}_\mathrm{o} ({\color{red}q^\mathrm{o}_+} - q^\mathrm{o}) -\log \phi({\color{red}q^\mathrm{o}_+},{\color{blue}q^\mathrm{r}_\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

[TRO 2023]

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

[TRO 2023]

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

[TRO 2023]

No Monte Carlo + First Order

Monte Carlo + Zeroth Order

>

Contributions

Randomized Smoothing

Barrier Smoothing

[HCBKSM 2022]

 

[OLP 2019]

[PCT 2015]

[TRO 2024]

[TRO 2024]

 

[ZJW 2023]

 

Equivalence

LCP Relaxation

[RA-L 2022]

[MSLSMC, 2024]

Randomized Smoothing

[TRO 2023, TRO 2025]

Part 1. Contact Mode Abstraction via Smoothing

What allows abstraction of contact modes?

How can we achieve it efficiently?

\nabla

Can take gradients efficiently

Doesn't require Monte Carlo

[RA-L 2021, ICML 2022, TRO 2023]

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

Part 2. Local Planning for Contact-Rich Manipulation

Part 1. Contact Mode Abstraction via Smoothing

Introduction

[TRO 2023, TRO 2025]

Part 2. Local Planning for Contact-Rich Manipulation

What is a good local model of contact dynamics?

How do we enable local gradient-based planning?

[TRO 2023, TRO 2025 (WIP)]

\begin{aligned} {\color{blue}q_{+}} = f(q, u) \end{aligned}

Dynamics

Dynamics Equation

\begin{aligned} q \end{aligned}
\begin{aligned} u \end{aligned}
\begin{aligned} {\color{blue}q_{+}} \end{aligned}

Dynamics

\begin{aligned} q \end{aligned}
\begin{aligned} u \end{aligned}
\begin{aligned} {\color{blue}q_{+}^\mathrm{o}} \end{aligned}

Dynamics Equation

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

Dynamics

\begin{aligned} q \end{aligned}
\begin{aligned} u \end{aligned}
\begin{aligned} {\color{blue}q_{+}^\mathrm{o}} \end{aligned}

Dynamics Equation

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

Dynamics

\begin{aligned} q \end{aligned}
\begin{aligned} u \end{aligned}
\begin{aligned} {\color{blue}q_{+}^\mathrm{o}} \end{aligned}
\begin{aligned} {\color{magenta}q_{goal}^\mathrm{o}} \end{aligned}

How should I command the robot to move the object to some goal?

Planning & Control

\begin{aligned} \min_{\delta u} \quad & \|{\color{magenta}q^\mathrm{o}_{goal}} - {\color{blue}q^\mathrm{o}_+}\|^2 + \|\delta u\|^2 \\ \text{s.t.} \quad & {\color{blue}q^\mathrm{o}_{+}} = f^o(q,u + {\color{green}\delta u}) \end{aligned}

Get to the goal

Minimize effort

\begin{aligned} {\color{magenta}q_{goal}^\mathrm{o}} \end{aligned}

Dynamics Equation

Planning & Control

Get to the goal

Minimize effort

\begin{aligned} {\color{magenta}q_{goal}^\mathrm{o}} \end{aligned}

This problem is nonlinear....how do we tackle it with iterative optimization?

Planning & Control

\begin{aligned} \min_{\delta u} \quad & \|{\color{magenta}q^\mathrm{o}_{goal}} - {\color{blue}q^\mathrm{o}_+}\|^2 + \|\delta u\|^2 \\ \text{s.t.} \quad & {\color{blue}q^\mathrm{o}_{+}} = f^o(q,u + {\color{green}\delta u}) \end{aligned}

Dynamics Equation

Sampling-Based Strategy

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

Dynamics

\begin{aligned} q \end{aligned}
\begin{aligned} u + {\color{green} \delta u} \end{aligned}
\begin{aligned} {\color{blue}q^\mathrm{o}_{next}} \end{aligned}
\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

Sampling-Based Strategy

Sampling-Based Strategy

Get to the goal

Minimize effort

Feasible Set

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

Planning & Control

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

How do we represent this set in a computationally tractable manner?

  1. Using Gradients
\nabla

2. Without Monte-Carlo

Planning & Control

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

How do we represent this set in a computationally tractable manner?

Attempt 1.

Taylor Approximation

\begin{aligned} {\color{blue}q^\mathrm{o}_{+}} & = \frac{\partial f^\mathrm{o}}{\partial u}{\color{green}\delta u} + f^\mathrm{o}(q,u) \\ & \|{\color{green} \delta u}\|_2 \leq \varepsilon \end{aligned}

Iterative Optimization for Planning

\begin{aligned} \min_{\delta u} \quad & \|{\color{magenta}q^\mathrm{o}_{goal}} - {\color{blue}q^\mathrm{o}_+}\|^2 + \|\delta u\|^2 \\ \text{s.t.} \quad & {\color{blue}q^\mathrm{o}_{+}} = \frac{\partial f^o}{\partial u}{\color{green}\delta u} + f^o(q,u) \\ & \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

Get to the goal

Minimize effort

First-Order Taylor Approximation

Trust Region

\begin{aligned} \color{magenta} q^\mathrm{o}_{goal} \end{aligned}
\begin{aligned} \color{blue} q \end{aligned}
\begin{aligned} \delta u \end{aligned}

Now a convex optimization problem!

Linearized Dynamics

\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}
\begin{aligned} q \end{aligned}
\begin{aligned} u \end{aligned}
\begin{aligned} {\color{blue}q^\mathrm{o}_{+}} = \frac{\partial f^\mathrm{o}}{\partial u}{\color{green}\delta u} + f^\mathrm{o}(q,u) \end{aligned}
\begin{aligned} {\color{blue}q^\mathrm{o}_{+}} \end{aligned}

Local Model over the Dynamics

Trust Region

True Samples

Samples from Local Model

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

Linear Map

Dynamics

\begin{aligned} {\color{red}q^\mathrm{o}_{next}} = f^\mathrm{o}(q, u + {\color{green}\delta u}) \end{aligned}
\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

Local Model over the Dynamics

Trust Region

Existing Literature

QP Controllers

iLQR & DDP

[Kuindersma et al., 2014]

[Kong et al., 2023]

[Tassa et al., 2014]

[Farshidian et al., 2017]

MPC Controllers

iLQR & DDP

[Kim et al., 2019]

[Tassa et al., 2014]

[Sleileman et al., 2021]

Local Model with Smoothing

\begin{aligned} q_+^o = f^o(q,u) \end{aligned}

True Dynamics

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

First-Order Taylor Approximation

robot

object

robot

object

Planning & Control

How do we represent this set in a computationally tractable manner?

Attempt 1.

Taylor Approximation

\begin{aligned} {\color{blue}q^\mathrm{o}_{+}} & = \frac{\partial f^\mathrm{o}}{\partial u}{\color{green}\delta u} + f^\mathrm{o}(q,u) \\ & \|{\color{green} \delta u}\|_2 \leq \varepsilon \end{aligned}

Attempt 2.

Taylor Approximation

on Smoothed Dynamics

\begin{aligned} {\color{blue}q^\mathrm{o}_{+}} & = \frac{\partial f_{\color{red}\rho}^o}{\partial u}{\color{green}\delta u} + f_{\color{red}\rho}^o(q,u) \\ & \|{\color{green}\delta u}\|_2 \leq \varepsilon \end{aligned}
\begin{aligned} \min_{\delta u} \quad & \|{\color{magenta}q^\mathrm{o}_{goal}} - {\color{blue}q^\mathrm{o}_+}\|^2 + \|\delta u\|^2 \\ \text{s.t.} \quad & {\color{blue}q^\mathrm{o}_{+}} \in {\color{red}\mathcal{M}_\varepsilon(q,u)} \\ \end{aligned}

Local Model with Smoothing

\begin{aligned} q_+^o = f_{\color{red}\rho}^o(q,u) \end{aligned}

Smoothed Dynamics

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

First-Order Taylor Approximation

robot

object

robot

object

Iterative Optimization for Planning

Get to the goal

Minimize effort

First-Order Taylor Approximation

Trust Region

\begin{aligned} \color{magenta} q^\mathrm{o}_{goal} \end{aligned}
\begin{aligned} \color{blue} q \end{aligned}

Local Model is too myopic

\begin{aligned} \min_{\delta u} \quad & \|{\color{magenta}q^\mathrm{o}_{goal}} - {\color{blue}q^\mathrm{o}_+}\|^2 + \|\delta u\|^2 \\ \text{s.t.} \quad & {\color{blue}q^\mathrm{o}_{+}} = \frac{\partial f^o}{\partial u}{\color{green}\delta u} + f^o(q,u) \\ & \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

Iterative Optimization for Planning

\begin{aligned} \min_{\delta u} \quad & \|{\color{magenta}q^\mathrm{o}_{goal}} - {\color{blue}q^\mathrm{o}_+}\|^2 + \|\delta u\|^2 \\ \text{s.t.} \quad & {\color{blue}q^\mathrm{o}_{+}} = \frac{\partial f^o_{\color{red}\rho}}{\partial u}{\color{green}\delta u} + f^o_{\color{red}\rho}(q,u) \\ & \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

Get to the goal

Minimize effort

First-Order Taylor Approximation

Trust Region

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

Provides a slightly more global model

\begin{aligned} \color{magenta} q^\mathrm{o}_{goal} \end{aligned}

Existing Literature

[Howell, Le Cleach et al., 2022]

[Suh et al., 2022]

[Shirai et al., 2024]

[Pang et al., 2023]

 

Contact Relaxation

[Huang et al., 2021]

 

[Turpin et al., 2023]

 

True Samples

Samples from Local Model

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

Linear Map

Dynamics

\begin{aligned} {\color{red}q^\mathrm{o}_{next}} = f^\mathrm{o}_\rho(q, u + {\color{green}\delta u}) \end{aligned}
\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

Is our local model correct?

Trust Region

[TRO 2025 (WIP)]

True Samples

Samples from Local Model

Linear Map

Dynamics

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

Is our local model correct?

What are we missing here?

Trust Region

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

[TRO 2025 (WIP)]

Planning & Control

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

How do we represent this set in a computationally tractable manner?

Attempt 1.

Taylor Approximation

\begin{aligned} {\color{blue}q^\mathrm{o}_{+}} & = \frac{\partial f^\mathrm{o}}{\partial u}{\color{green}\delta u} + f^\mathrm{o}(q,u) \\ & \|{\color{green} \delta u}\|_2 \leq \varepsilon \end{aligned}

Attempt 2.

Taylor Approximation

on Smoothed Dynamics

\begin{aligned} {\color{blue}q^\mathrm{o}_{+}} & = \frac{\partial f_{\color{red}\rho}^o}{\partial u}{\color{green}\delta u} + f_{\color{red}\rho}^o(q,u) \\ & \|{\color{green}\delta u}\|_2 \leq \varepsilon \end{aligned}

Attempt 3.

Friction Cone Constraints

on Dual Linearization

Local Model with Smoothing

\begin{aligned} q_+^o = f_{\color{red}\rho}^o(q,u) \end{aligned}

Smoothed Dynamics

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

First-Order Taylor Approximation

robot

object

robot

object

Friction Cone

*image taken from Stephane Caron's blog

Unilateral

(Can't pull)

Coulomb Friction

Friction Cone

+

=

[TRO 2025 (WIP)]

Friction Cone

*image taken from Stephane Caron's blog

Unilateral

(Can't pull)

Coulomb Friction

Friction Cone

+

=

But how do we reason about constraints on contact impulses?

[TRO 2025 (WIP)]

Iterative Optimization for Planning

\begin{aligned} \min_{\delta u} \quad & \|{\color{magenta}q^\mathrm{o}_{goal}} - q^\mathrm{o}_+\|^2 + \|\delta u\|^2 \\ \text{s.t.} \quad & {\color{blue}q^\mathrm{o}_{next}} = \frac{\partial f^o_{\color{red}\rho}}{\partial u}{\color{green}\delta u} + f^o_{\color{red}\rho}(q,u) \\ & \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

Get to the goal

Minimize effort

First-Order Taylor Approximation

Trust Region

\begin{aligned} \color{magenta} q^\mathrm{u}_{goal} \end{aligned}
\begin{aligned} \color{blue} q \end{aligned}
\begin{aligned} \delta u \end{aligned}

We need an expression for contact impulses

[TRO 2025 (WIP)]

CQDC: A Quasi-dynamic Simulator for Manipulation

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

Non-Penetration

Minimum Energy Principle

[TRO 2025 (WIP)]

CQDC: A Quasi-dynamic Simulator for Manipulation

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

Stationarity Condition

\begin{aligned} \mathbf{M}_\mathrm{o}({\color{red}q^\mathrm{o}_+} - q^\mathrm{o}) = \mathbf{J}^\top {\color{magenta}\lambda} \end{aligned}

Contact Impulse

Contact Jacobian

Object Movement

The Dual Variables become Contact Impulse

[TRO 2025 (WIP)]

CQDC as Convex Optimization

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

\begin{aligned} q^\star_+ \end{aligned}
\begin{aligned} \lambda \end{aligned}

Primal Solution (Configuration)

Dual Solution (Contact Impulse)

[TRO 2025 (WIP)]

Sensitivity Analysis over Dual Variables

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

\begin{aligned} \frac{\partial q^\star_+}{\partial b} \end{aligned}
\begin{aligned} \frac{\partial \lambda}{\partial b} \end{aligned}

Primal Gradient (Configuration)

Dual Gradient (Contact Impulse)

[TRO 2025 (WIP)]

The Contact Impulse Map

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

Dynamics (Primal)

\begin{aligned} {\color{blue}q^\mathrm{o}_{next}} \end{aligned}
\begin{aligned} {\color{magenta}\lambda} = \frac{\partial \lambda}{\partial u}{\color{green}\delta u} + \lambda(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}

Trust Region

[TRO 2025 (WIP)]

Dynamics (Primal)

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

Contact Impulses (Dual)

\begin{aligned} {\color{magenta}\lambda} \end{aligned}
\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}
\begin{aligned} {\color{blue}q^\mathrm{o}_{next}} = \frac{\partial f^\mathrm{o}}{\partial u}{\color{green}\delta u} + f^\mathrm{o}(q,u) \end{aligned}
\begin{aligned} {\color{magenta}\lambda} = \frac{\partial \lambda}{\partial u}{\color{green}\delta u} + \lambda(q,u) \end{aligned}

The Contact Impulse Map

Trust Region

[TRO 2025 (WIP)]

Qualification on Contact Impulses

\begin{aligned} {\color{blue}q^\mathrm{o}_{next}} = \frac{\partial f_{\rho}}{\partial u}{\color{green}\delta u} + f_{\rho}(q,u) \end{aligned}
\begin{aligned} {\color{blue}q^\mathrm{o}_{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{magenta}\lambda}\in \{\mu\lambda_n \geq \|\lambda_t\|_2\} \end{aligned}
\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

Dynamics (Primal)

+

Trust Region

[TRO 2025 (WIP)]

Qualification on Contact Impulses

\begin{aligned} {\color{blue}q^\mathrm{o}_{next}} = \frac{\partial f_{\rho}}{\partial u}{\color{green}\delta u} + f_{\rho}(q,u) \end{aligned}
\begin{aligned} {\color{blue}q^\mathrm{o}_{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{magenta}\lambda}\in \{\mu\lambda_n \geq \|\lambda_t\|_2\} \end{aligned}
\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

Some samples violate friction cone constraints!

Dynamics (Primal)

Trust Region

[TRO 2025 (WIP)]

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 u}\|_2\leq \varepsilon \end{aligned}

Feasible Trust Region

\begin{aligned} {\color{blue}q^\mathrm{o}_{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}
\begin{aligned} \color{green} \mathcal{T}(q,u)=\{\delta u\} \end{aligned}

[TRO 2025 (WIP)]

Motion Set

Feasible Trust Region

\begin{aligned} \color{blue} \mathcal{M}(q,u)=\{q^\mathrm{o}_{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{blue}q^\mathrm{o}_{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}
\begin{aligned} \color{green} \mathcal{T}(q,u)=\{\delta u\} \end{aligned}

Feasible Trust Region

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

[TRO 2025 (WIP)]

Contact Impulse Set

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{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}
\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}
\begin{aligned} \color{green} \mathcal{T}(q,u)=\{\delta u\} \end{aligned}

[TRO 2025 (WIP)]

Now we have a close match with real samples

Motion Set

No Constraints

True Samples

[TRO 2025 (WIP)]

Motion Set

No Constraints

True Samples

Closer to ellipse when there is a full grasp

[TRO 2025 (WIP)]

The gradients take into account manipulator singularities

Motion Set

No Constraints

True Samples

[TRO 2025 (WIP)]

[OLP 2019]

[PCT 2015]

[Howell, Le Cleach et al., 2022]

[Pang et al., 2023]

 

Differentiable Simulator

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

Feasible Trust Region

x_+ = f(x,u)

Model Predictive Control

[Kuindersma et al., 2014]

[Farshidian et al., 2017]

[Kim et al., 2019]

[Sleileman et al., 2021]

M\Delta v = \mathbf{J}^\top\lambda
x_+ = f(x,u)

[Aydinoglu et al., 2021]

[TRO 2025 (WIP)]

[TRO 2023, TRO 2025]

Part 2. Local Planning for Contact-Rich Manipulation

What is a good local model of contact dynamics?

How do we enable local gradient-based planning?

Finding Optimal Action: One Step

\begin{aligned} \min_{\delta u} \quad & \|{\color{magenta}q^\mathrm{o}_{goal}} - {\color{blue}q^\mathrm{o}_+}\|^2 + \|\delta u\|^2 \\ \text{s.t.} \quad & {\color{blue}q_+^o} \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{o}_{next}\} \end{aligned}
\begin{aligned} \color{magenta} q^\mathrm{o}_{goal} \end{aligned}
\begin{aligned} \color{blue} q \end{aligned}
\begin{aligned} \delta u \end{aligned}

[TRO 2025 (WIP)]

\begin{aligned} \min_{\delta u_{0:T-1},\delta q_{0:T}} \quad & \|q^\mathrm{o}_{goal} - q^\mathrm{o}_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

[TRO 2025 (WIP)]

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}

[TRO 2025 (WIP)]

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

Create a feasible set around each point               to obtain a local dynamics model around the trajectory

(q_t,u_t)
q_\mathrm{goal}

[TRO 2025 (WIP)]

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}

[TRO 2025 (WIP)]

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}

Rollout the new initial input guess 

u^\star_{0:T-1}

[TRO 2025 (WIP)]

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

[TRO 2025 (WIP)]

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}

[TRO 2025 (WIP)]

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

[TRO 2025 (WIP)]

MPC Rollouts

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

[TRO 2025 (WIP)]

MPC Rollouts

The controller also successfully scales to the complexity of dexterous hands

[TRO 2025 (WIP)]

Experiment Results

Hardware (50 runs)

Drake (1000 runs)

1.37 deg / 1.29cm

2.81 deg / 2.2cm

14.32 deg

16.04 deg

28cm

[TRO 2025 (WIP)]

Is the friction cone constraint necessary?

\varepsilon
\varepsilon

[TRO 2025 (WIP)]

[TRO 2023, TRO 2025]

Part 2. Local Planning for Contact-Rich Manipulation

What is a good local model of contact dynamics?

How do we enable local gradient-based planning?

Contact

Scalability

Efficiency

Global Scope

[TRO 2023, TRO 2025 (WIP)]

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

Part 2. Local Planning for Contact-Rich Manipulation

Part 1. Contact Mode Abstraction via Smoothing

Introduction

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

What are fundamental limitations of local control?

How can we tackle difficult exploration problems?

How do we enable global planning capabilities?

[TRO 2023, TRO 2025 (WIP)]

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

The Exploration Problem

The Exploration Problem

The Exploration Problem

How do we push in this direction?

How do we push in this direction?

The Exploration Problem

How do we push in this direction?

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 limitations of local control?

How can we tackle difficult exploration problems?

How do we enable global planning capabilities?

[TRO 2023, TRO 2025 (WIP)]

The Exploration Problem

Because I have seen that this state is useful!

Local Control

 

Local Search for Open-Loop Actions

The Robot Relocation Problem

Global Search for Initial Configurations

Local Search for Open-Loop Actions

Where should I place my robot?

The Robot Relocation Problem

Previous Contributions

Grasp Synthesis

Goal-Oriented Grasp Synthesis

Grasp Synthesis

[Venkatesh, Bianchini et al., 2024]

[Ferrari & Canny, 1992]

[Dai et al., 2012]

[Han, Trinkle, Li, 1992]

[Li et al, 2023]

[Fang et al., 2022]

Finding the right cost

\begin{aligned} {V({\color{magenta}q^\mathrm{r}_\mathrm{init}}; {\color{blue}q^\mathrm{o}_\mathrm{init}},{\color{springgreen}q^\mathrm{o}_{goal}})\coloneqq} \end{aligned}

High Cost

Finding the right cost

Low Cost

\begin{aligned} {V({\color{magenta}q^\mathrm{r}_\mathrm{init}}; {\color{blue}q^\mathrm{o}_\mathrm{init}},{\color{springgreen}q^\mathrm{o}_{goal}})\coloneqq} \end{aligned}

Utilizing the MPC Cost

Our Local Controller

How well does the policy perform when we run local MPC?

\begin{aligned} \min_{\delta u_{0:T-1},\delta q_{0:T}} \quad & \|{\color{springgreen}q^\mathrm{o}_{goal}} - q^\mathrm{o}_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{o} = {\color{blue}q^\mathrm{o}_{init}} \\ & q_0^\mathrm{r} = {\color{magenta}q^\mathrm{r}_{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

\begin{aligned} {V({\color{magenta}q^\mathrm{r}_\mathrm{init}}; {\color{blue}q^\mathrm{o}_\mathrm{init}},{\color{springgreen}q^\mathrm{o}_{goal}})\coloneqq} \end{aligned}

[TRO 2025 (WIP)]

Utilizing the MPC Cost

Our Local Controller

How well does the policy perform when we run local MPC?

\begin{aligned} \min_{\delta u_{0:T-1},\delta q_{0:T}} \quad & \|{\color{springgreen}q^\mathrm{o}_{goal}} - q^\mathrm{o}_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{o} = {\color{blue}q^\mathrm{o}_{init}} \\ & q_0^\mathrm{r} = {\color{magenta}q^\mathrm{r}_{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

\begin{aligned} {V({\color{magenta}q^\mathrm{r}_\mathrm{init}}; {\color{blue}q^\mathrm{o}_\mathrm{init}},{\color{springgreen}q^\mathrm{o}_{goal}})\coloneqq} \end{aligned}

Efficient, Contact-Scalable MPC allows fast online computation of cost.

[TRO 2025 (WIP)]

The Actuator Relocation Problem

Where should I place my actuators?

Global Search for Initial Configurations

To a place where I can perform local control well

[TRO 2025 (WIP)]

The Robot Relocation Problem

\begin{aligned} \min_{{\color{magenta}q^\mathrm{r}_\mathrm{init}}} \quad &V( {\color{magenta}q^\mathrm{r}_\mathrm{init}}; {\color{blue}q^\mathrm{o}_\mathrm{init}}, {\color{springgreen}q^\mathrm{o}_{goal}}) \\ \text{s.t.} \quad & \phi_i({\color{magenta}q^\mathrm{r}_\mathrm{init}}, {\color{blue}q^\mathrm{o}_\mathrm{init}})\geq 0 \end{aligned}

Choose best initial robot configuration for MPC

Non-penetration constraints

[TRO 2025 (WIP)]

The Actuator Relocation Problem

Landscape is much more complex,

sampling-based global optimization is necessary.

[TRO 2025 (WIP)]

Robustness Considerations

What is a better configuration to start from?

[TRO 2025 (WIP)]

Robustness Considerations

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

[TRO 2025 (WIP)]

Robustness Considerations

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

[TRO 2025 (WIP)]

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

Robustness Considerations

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

[TRO 2025 (WIP)]

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

Robustness Considerations

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

[TRO 2025 (WIP)]

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

Robustness Considerations

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

Max Inscribed Sphere within Motion Set

{\color{blue}\min_v} \delta q({\color{blue}v})

[TRO 2025 (WIP)]

Robustness Considerations

[TRO 2025 (WIP)]

Regularizing with Robustness

\begin{aligned} \min_{q^\mathrm{r}_\mathrm{init}} \quad & {\color{magenta}V^\pi(q^\mathrm{r}_\mathrm{init}; q^\mathrm{o}_\mathrm{init};q^\mathrm{o}_{goal})} + {\color{green}\alpha} {\color{blue}r(q^\mathrm{r}_\textrm{init};q^\mathrm{o}_\mathrm{init})^2} \\ \text{s.t.} \quad & \phi_i(q^\mathrm{r}_\mathrm{init}, q^\mathrm{o}_\mathrm{init})\geq 0\\ \end{aligned}

[Dai et al., 2012]

[Li et al, 2023]

[Venkatesh, Bianchini et al., 2024]

Be Robust

Be strategic for the task

No Penetration

[TRO 2025 (WIP)]

Grasp Synthesis Procedure

Sampling-Based Search on

Reduced-Order System

Differential Inverse Kinematics to find Hand Configuration

Verification with MPC rollouts

[TRO 2025 (WIP)]

Grasp Synthesis Results

Pitch + 90 degrees

Yaw + 45 degrees

Pitch - 90 degrees

[TRO 2025 (WIP)]

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

What are fundamental limitations of local control?

How can we tackle difficult exploration problems?

How do we enable global planning capabilities?

[TRO 2023, TRO 2025 (WIP)]

Sampling-Based Motion Planning (SBMP)

[Kavraki, 1996]

[LaValle, 1998]

Probabilistic Roadmap (PRM)

Rapidly Exploring Random Tree (RRT)

Previous Contributions

[Cheng et al., 2022]

[Cheng et al., 2024]

[Wu et al., 2020]

[Chen et al., 2021]

[Toussaint, 2015]

[Zhu et al., 2022]

[Khandate et al., 2024]

Grasp Synthesis + MPC

q^\mathrm{r}
q^\mathrm{o}_x
q^\mathrm{o}_y
q_0^\mathrm{o}
q_1^\mathrm{o}

Object Configuration Space

[TRO 2025 (WIP)]

\begin{aligned} \min_{{\color{magenta}q^\mathrm{r}_\mathrm{init}}}{V({\color{magenta}q^\mathrm{r}_\mathrm{init}}; {\color{blue}q^\mathrm{o}_\mathrm{init}},{\color{springgreen}q^\mathrm{o}_{goal}})} \end{aligned}
q^\mathrm{r}
q^\mathrm{o}_x
q^\mathrm{o}_y
q_0^\mathrm{o}
q_1^\mathrm{o}
\begin{aligned} \min_{{\color{magenta}q^\mathrm{r}_\mathrm{init}}}{V({\color{magenta}q^\mathrm{r}_\mathrm{init}}; {\color{blue}q^\mathrm{o}_\mathrm{init}},{\color{springgreen}q^\mathrm{o}_{goal}})} \end{aligned}

Grasp Synthesis + MPC

Object Configuration Space

[TRO 2025 (WIP)]

q^\mathrm{r}
q^\mathrm{o}_x
q^\mathrm{o}_y
q_0^\mathrm{o}
q_1^\mathrm{o}
\begin{aligned} \min_{{\color{magenta}q^\mathrm{r}_\mathrm{init}}}{V({\color{magenta}q^\mathrm{r}_\mathrm{init}}; {\color{blue}q^\mathrm{o}_\mathrm{init}},{\color{springgreen}q^\mathrm{o}_{goal}})} \end{aligned}

Grasp Synthesis + MPC

Object Configuration Space

MPC

[TRO 2025 (WIP)]

q^\mathrm{r}
q^\mathrm{o}_x
q^\mathrm{o}_y
q_0^\mathrm{o}
q_1^\mathrm{o}

Grasp Synthesis + MPC

MPC

\begin{aligned} \min_{{\color{magenta}q^\mathrm{r}_\mathrm{init}}}{V({\color{magenta}q^\mathrm{r}_\mathrm{init}}; {\color{blue}q^\mathrm{o}_\mathrm{init}},{\color{springgreen}q^\mathrm{o}_{goal}})} \end{aligned}

[TRO 2025 (WIP)]

q^\mathrm{r}
q^\mathrm{o}_x
q^\mathrm{o}_y
q_0^\mathrm{o}
q_1^\mathrm{o}
\begin{aligned} \min_{{\color{magenta}q^\mathrm{r}_\mathrm{init}}}{V({\color{magenta}q^\mathrm{r}_\mathrm{init}}; {\color{blue}q^\mathrm{o}_\mathrm{init}},{\color{springgreen}q^\mathrm{o}_{goal}})} \end{aligned}

Grasp Synthesis + MPC

MPC

q_2^\mathrm{o}

MPC

[TRO 2025 (WIP)]

q^\mathrm{r}
q^\mathrm{o}_x
q^\mathrm{o}_y
q_0^\mathrm{o}
q_1^\mathrm{o}
\begin{aligned} \min_{{\color{magenta}q^\mathrm{r}_\mathrm{init}}}{V({\color{magenta}q^\mathrm{r}_\mathrm{init}}; {\color{blue}q^\mathrm{o}_\mathrm{init}},{\color{springgreen}q^\mathrm{o}_{goal}})} \end{aligned}

Grasp Synthesis + MPC

MPC

q_2^\mathrm{o}

MPC

How do I chain these trajectories in the full configuration space?

[TRO 2025 (WIP)]

q^\mathrm{r}
q^\mathrm{o}_x
q^\mathrm{o}_y
q_0^\mathrm{o}
q_1^\mathrm{o}
\begin{aligned} \min_{{\color{magenta}q^\mathrm{r}_\mathrm{init}}}{V({\color{magenta}q^\mathrm{r}_\mathrm{init}}; {\color{blue}q^\mathrm{o}_\mathrm{init}},{\color{springgreen}q^\mathrm{o}_{goal}})} \end{aligned}

Grasp Synthesis + MPC

MPC

q_2^\mathrm{o}

MPC

Regrasp

(Collision-Free Motion Planning)

[TRO 2025 (WIP)]

Chaining Local Actions

[TRO 2025 (WIP)]

q^\mathrm{r}
q^\mathrm{o}_x

Roadmap Approach to Manipulation

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

q^\mathrm{o}_y

[TRO 2025 (WIP)]

Roadmap Approach to Manipulation

q^\mathrm{r}
q^\mathrm{o}_x

Roadmap Approach to Manipulation

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

q^\mathrm{o}_y
q^\mathrm{o}_\mathrm{goal}

[TRO 2025 (WIP)]

[TRO 2023, TRO 2025]

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

How do we enable global planning capabilities?

[TRO 2023, TRO 2025 (WIP)]

The real discreteness comes from motion planning, not contact modes.

When we abstract contact modes with smoothing, SBMP can be highly efficient.

Difficult exploration problems can be alleviated with offline search for initial conditions.

[TRO 2023, TRO 2025]

Part 3. Global Planning for Contact-Rich Manipulation

Part 2. Local Planning for Contact-Rich Manipulation

Part 1. Contact Mode Abstraction via Smoothing

Introduction

Summary

Our Method

Contact Scalability

Efficiency

Global Planning

60 hours, 8 NVidia A100 GPUs

5 minutes, 1 Macbook CPU

Thank You

Attempt 1. Penalty Method

\phi

- No force when not in contact

- Spring-damper behavior when in contact

\phi

- No force when not in contact

- Spring-damper behavior when in contact

Attempt 1. Penalty Method

\phi

- No force when not in contact

- Spring-damper behavior when in contact

Attempt 1. Penalty Method

Attempt 1. Penalty Method

\phi
\phi
\lambda
\lambda
\lambda

Attempt 1. Penalty Method

\phi
\phi
\lambda
\lambda
\lambda
\nabla

Can take gradients efficiently

 

Abstracts contact without Monte-Carlo

Penalty Method & Force-Level Relaxation

Penalty-Based Simulation

Force-Level Relaxation

[Geilinger et al., 2020]

 

[Hunt & Crossley, 1975]

 

[Castro et al., 2020]

 

[Huang et al., 2021]

 

[Turpin et al., 2023]

 

[Drumwright, 2008]

 

[Suh & Wang, 2019]

 

Cons of the Penalty Method

Spring-damper modeling

\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}
\phi
\lambda
\lambda

Realistic behavior requires stiff springs

Gradients capture transient behavior

Cons of the Penalty Method

Good gradients require good models

Spring-damper modeling

\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}
\phi
\lambda
\lambda

Robustness Considerations

[Ferrari & Canny, 1992]

[TRO 2025 (WIP)]

Robustness Considerations

[Ferrari & Canny, 1992]

f(v_0)

[TRO 2025 (WIP)]

Robustness Considerations

[Ferrari & Canny, 1992]

f(v_1)

[TRO 2025 (WIP)]

Robustness Considerations

[Ferrari & Canny, 1992]

f(v_2)

[TRO 2025 (WIP)]

Robustness Considerations

[Ferrari & Canny, 1992]

f(v_3)

[TRO 2025 (WIP)]

Robustness Considerations

[Ferrari & Canny, 1992]

{\color{red}\min_v} f({\color{red}v})

[TRO 2025 (WIP)]

Optimal Control on a Local Model

\begin{aligned} \min_{\delta u} \quad & \|q^\mathrm{o}_{goal} - q^\mathrm{o}_+\|^2 + \|\delta u\|^2 \\ \text{s.t.} \quad & {\color{blue}q^\mathrm{o}_{next}} = \frac{\partial f^\mathrm{o}}{\partial u}{\color{green}\delta u} +f^\mathrm{o}(q,u) \\ & \|\delta u\|_2 \leq \varepsilon \end{aligned}

Get to the goal

Minimize effort

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

Dynamics

\begin{aligned} q \end{aligned}
\begin{aligned} u + {\color{green} \delta u} \end{aligned}
\begin{aligned} {\color{blue}q^\mathrm{o}_{next}} \end{aligned}
\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}

Local Model over the Dynamics

Dynamics

\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}
\begin{aligned} {\color{blue}q^\mathrm{o}_{next}} = f^\mathrm{o}(q, u + {\color{green}\delta u}) \end{aligned}
\begin{aligned} q \end{aligned}
\begin{aligned} u \end{aligned}

Local Model over the Dynamics

Dynamics

\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}
\begin{aligned} {\color{blue}q^\mathrm{o}_{next}} = f^\mathrm{o}(q, u + {\color{green}\delta u}) \end{aligned}
\begin{aligned} q \end{aligned}
\begin{aligned} u \end{aligned}

Local Model over the Dynamics

Dynamics

\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}
\begin{aligned} {\color{blue}q^\mathrm{o}_{next}} = f^\mathrm{o}(q, u + {\color{green}\delta u}) \end{aligned}
\begin{aligned} q \end{aligned}
\begin{aligned} u \end{aligned}

?

Local Model over the Dynamics

Linearized Dynamics

\begin{aligned} \|{\color{green}\delta u}\|_2\leq \varepsilon \end{aligned}
\begin{aligned} q \end{aligned}
\begin{aligned} u \end{aligned}
\begin{aligned} {\color{blue}q^\mathrm{o}_{next}} = \frac{\partial f^\mathrm{o}}{\partial u}{\color{green}\delta u} + f^\mathrm{o}(q,u) \end{aligned}

Local Model over the Dynamics

Terry Suh PhD Thesis Defense

By Terry Suh

Terry Suh PhD Thesis Defense

  • 80