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
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?
- Using Gradients
\nabla
Towards Efficiency
How can we abstract contact modes more efficiently?
- 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
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]
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?
- 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}
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]
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
