Contact-rich Planning for Robotic Manipulation with Quasi-static Contact Models
Tao Pang
Word cloud of my PhD thesis :D
Why contact-rich manipulation?
Collision-free Motion Planning
Interact with the world
Smaller
Why is contact-rich planning hard?
Contact dynamics is non-smooth!
qa
q^\mathrm{a}
u
u
qu
q^\mathrm{u}
u: un-actuated
control input: commanded ball position
a: actuated
What does the plot of this dynamics look like? (demo)
Contact dynamics is non-smooth
qa
q^\mathrm{a}
u
u
qu
q^\mathrm{u}
Moving the box to the right
q+u
q^\mathrm{u}_+
u
u
No contact
Contact
uminimize21(q+u−qgoalu)2
\underset{u}{\mathrm{minimize}} \; \frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2
−∂u∂(21(q+u−qgoalu)2)=+(qgoalu−q+u)0∂u∂q+u
-\frac{\partial }{\partial u} \left(\frac{1}{2} (q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2 \right)
= \underbrace{(q^\mathrm{u}_\text{goal} - q^\mathrm{u}_+)}_+ \underbrace {\frac{\partial q_+^\mathrm{u}}{\partial u}}_0
Solve with gradient descent
∂u∂q+u
\frac{\partial q^\mathrm{u}_+}{\partial u}
u
u
No contact
Contact
qa
q^\mathrm{a}
u
u
qu
q^\mathrm{u}
qgoalu
q^\mathrm{u}_\text{goal}
Smoothing comes to the rescue!
Smoothing comes to the rescue!
q+u
q^\mathrm{u}_+
u
u
No contact
Contact
uminimize21(q+u−qgoalu)2
\underset{u}{\mathrm{minimize}} \; \frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2
−∂u∂(21(q+u−qgoalu)2)=+(qgoalu−q+u)+∂u∂q+u
-\frac{\partial }{\partial u} \left(\frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2\right)
= \underbrace{(q^\mathrm{u}_\text{goal} - q^\mathrm{u}_+)}_+ \underbrace {\frac{\partial q_+^\mathrm{u}}{\partial u}}_+
Solve with gradient descent
∂u∂q+u
\frac{\partial q^\mathrm{u}_+}{\partial u}
u
u
No contact
Contact
qa
q^\mathrm{a}
u
u
qu
q^\mathrm{u}
qgoalu
q^\mathrm{u}_\text{goal}
Moving the box to the left?
q+u
q^\mathrm{u}_+
u
u
No contact
Contact
uminimize21(q+u−qgoalu)2
\underset{u}{\mathrm{minimize}} \; \frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2
−∂u∂(21(q+u−qgoalu)2)=−(qgoalu−q+u)+∂u∂q+u
-\frac{\partial }{\partial u} \left(\frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2\right)
= \underbrace{(q^\mathrm{u}_\text{goal} - q^\mathrm{u}_+)}_- \underbrace {\frac{\partial q_+^\mathrm{u}}{\partial u}}_+
Solve with gradient descent
∂u∂q+u
\frac{\partial q^\mathrm{u}_+}{\partial u}
u
u
No contact
Contact
Stuck in local minima!
qa
q^\mathrm{a}
u
u
qu
q^\mathrm{u}
qgoalu
q^\mathrm{u}_\text{goal}
Global Search with Contact Modes
System
Number of modes
- Left not touching
- Left touching
- Right touching
- Right not touching
4
4
x
Friction cone
4×4
4 \times 4
- Not touching
- Sticking
- Sliding left
- Sliding right
y
Number of faces
x
x
y
z
6(220)=6190
6^\binom{20}{2} = 6^{190}
- Not touching
- Sticking
- 4 directions of sliding
Number of contact pairs.
Model-based Planning through Contact: a Quick Review
- Trajectory Optimization through Contact
- Mordatch, Igor, Emanuel Todorov, and Zoran Popović. "Discovery of complex behaviors through contact-invariant optimization." ACM Transactions on Graphics (TOG), 2012.
- Hogan, François Robert, and Alberto Rodriguez. "Feedback control of the pusher-slider system: A story of hybrid and underactuated contact dynamics." arXiv preprint arXiv:1611.08268, 2016.
- Posa, Michael, Cecilia Cantu, and Russ Tedrake. "A direct method for trajectory optimization of rigid bodies through contact." The International Journal of Robotics Research, 2014.
- Aydinoglu, Alp, and Michael Posa. "Real-time multi-contact model predictive control via admm." 2022 International Conference on Robotics and Automation (ICRA), 2022.
- Cheng, Xianyi, et al. "Contact mode guided motion planning for quasidynamic dexterous manipulation in 3d." 2022 International Conference on Robotics and Automation (ICRA), 2022.
- Marcucci, Tobia, et al. "Approximate hybrid model predictive control for multi-contact push recovery in complex environments." 2017 IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids), 2017.
-
Tao Pang*, H.J. Terry Suh*, Lujie Yang, Russ Tedrake, "Global Planning for Contact-Rich Manipulation via Local Smoothing of Quasi-dynamic Contact Models", under review, 2022.
- Locally enumerate mode switches (fast)
- Globally enumerate Mode Switches (slow)
Good Scalability (Smoothed dynamics)
Limited Scalability (Mode Transitions)
Local Planning
Global Planning
- Our method: Global planning with RRT + Smoothed dynamics [7]
[1] Mordatch et al.
[3] Posa et al.
[6] Marcucci et al.
[4] Aydinoglu and Posa.
[2] Hogan and Rodriguez.
- Sampling mode switches.
[5] Cheng et al.
What can we learn from RL (reinforcement learning)?
- RL generates impressive policies for specific tasks, but...
- RL requires heavy offline computation,
- The Nvidia policy is trained with "only 8 NVIDIA A40 GPUs" for 60 hours.
- The learned policy does not generalize (e.g. to opening a door).
- RL requires heavy offline computation,
- (OpenAI) Andrychowicz et al. "Learning dexterous in-hand manipulation." The International Journal of Robotics Research, 2020.
- (Nvidia) Handa et al. "DeXtreme: Transfer of Agile In-hand Manipulation from Simulation to Reality", arXiv preprint, 2022.
-
H.J. Terry Suh*, Tao Pang*, Russ Tedrake, “Bundled Gradients through Contact via Randomized Smoothing”, RA-L, 2022.
-
Tao Pang*, H.J. Terry Suh*, Lujie Yang, Russ Tedrake, "Global Planning for Contact-Rich Manipulation via Local Smoothing of Quasi-dynamic Contact Models", under review, 2022.
[1] OpenAI, 2018
[2] Nvidia, 2022
How does RL power through non-smooth contact dynamics? [3][4]
- RL maximizes a stochastic reward using gradients estimated from sampling.
- Sampling has a smoothing effect on the gradients.
In model-based planning, we can smooth contact dynamics, but without using samples (faster!).
Terry Suh
What our planner can do
What our planner can do
Contributions
- A novel contact dynamics model which is
- quasi-static
- convex
- differentiable
- amenable to smoothing
- A sampling-based global search algorithm guided by the proposed contact model.
A contact-rich planner that can generate plans for complex systems involving dexterous hands / hardware with about 1 minute of online computation on a laptop.
In contrast, RL-based methods needs tens of hours of offline computation on a beefy workstation.
Our planner is enabled by
A Quasi-static, Convex, Differentiable Contact Dynamics Model
What is quasi-static dynamics?
qa
q^\mathrm{a}
u
u
Ka
K_\mathrm{a}
qu
q^\mathrm{u}
- Quasi-static: velocity is small so that inertial and Coriolis forces are negligible.
- q:= [qu,qa]: system configuration.
- qa: actuated, imepdance-controlled robots
- qu: un-actuated objects.
- u: position command to the stiffness-controlled robots [1].
- δq:=q+−q
- h: step size in seconds.
q+=f(q,u)
λ1
\lambda_1
λ3
\lambda_3
λ1
\lambda_1
λ2
\lambda_2
- λi: contact forces/impulses
- ϕi: signed distances
ϕ2(q)
\phi_2({q})
ϕ1(q)
\phi_1({q})
ϕ3(q)
\phi_3({q})
δq⋆=δqa,δquargmin21hK(qa+δqa−u)2s.t.ϕi(q+δq)≥0,∀i.
\begin{aligned}
\delta q^\star = \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\mathrm{argmin}} \; &\frac{1}{2} hK (q^\mathrm{a} + \delta q^\mathrm{a} - u)^2 \; \text{s.t.} \\
&\phi_i (q + \delta q)\geq 0, \forall i.
\end{aligned}
⇔
\Leftrightarrow
(KKT)
- A convex quadratic program (QP), after linearizing ϕi.
- "Minimizing potential energy, subject to non-penetration constraints."
-
Tao Pang, Russ Tedrake, "A Convex Quasistatic Time-stepping Scheme for Rigid Multibody Systems with Contact and Friction", ICRA, 2021.
q+=q+δq⋆
q_+ = q + \delta q^\star
hK(qa+δqa−u)=−λ1+λ2
hK(q^\mathrm{a} + \delta q^\mathrm{a} - u) = -\lambda_1 + \lambda_2
Force balance of the robot.
0=λ1−λ3
0 = \lambda_1 - \lambda_3
Force balance of the object.
ϕi(q+δq)≥0,∀i
\phi_i(q + \delta q) \geq 0, \; \forall i
Non-penetration.
λi≥0∀i
\lambda_i \geq 0 \; \forall i
"Contact forces cannot pull."
ϕ(q+δq)λ=0,∀i
\phi (q + \delta q) \lambda = 0, \forall i
"Contact force needs contact."
Quasi-static dynamics is good for manipulation planning!
- Spatial benefit: Smaller state space ([q,v] vs. only q).
-
Temporal benefit: Ignoring transients => looking into the future with fewer steps.
-
Many manipulation tasks are quasi-static.
qa
q^\mathrm{a}
ma
m^\mathrm{a}
u
u
K
K
D
D
Second-order Dynamics
Quasi-static Dynamics
Quasi-static dynamics needs regularization
δqa,δqumin21hK(q+δqa−u)2,s.t.ϕi(q+δq)≥0,∀i.
\begin{aligned}
\underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2, \; \text{s.t.} \\
&\phi_i (q + \delta q)\geq 0, \forall i.
\end{aligned}
Quasi-static Dynamics
- The quadratic cost is positive semi-definite.
- The object can be anywhere between the robot and the wall.
xmin∥Ax−b∥2
\underset{x}{\min} \|Ax - b\|^2
xmin∥Ax−b∥2+ϵ∥x∥2
\underset{x}{\min} \|Ax - b\|^2 + \epsilon \|x\|^2
Regularized least squares
Regularized Quasi-static Dynamics
δqa,δqumin21hK(qa+δqa−u)2+21ϵ(δqu)2,s.t.ϕi(q+δq)≥0,∀i.
\begin{aligned}
\underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q^\mathrm{a} + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \epsilon (\delta q^\mathrm{u})^2, \; \text{s.t.} \\
&\phi_i (q + \delta q)\geq 0, \forall i.
\end{aligned}
- The quadratic cost is now positive definite.
- "Among all possible object motions, give me the one that moves the least."
- Picking ϵ=mu/h gives Matt Mason's definition of quasi-dynamic dynamics.
qa
q^\mathrm{a}
u
u
Ka
K_\mathrm{a}
qu
q^\mathrm{u}
ϕ2(q)
\phi_2({q})
ϕ1(q)
\phi_1({q})
ϕ3(q)
\phi_3({q})
What about general systems with friction?
δqmin21δq[δquδqa]⊺Q[Mu/h00hKa]δq[δquδqa]+bh[−τu−Ka(u−qa)−τa]⊺δq[δquδqa]s.t.Linearizedϕi(q+δq)ϕi+Jniδq≥0,∀i∈{1⋯nc}.
\begin{aligned}
\underset{\delta q}{\min} \;
&\frac{1}{2}
{\underbrace{
\begin{bmatrix}
\delta q^\mathrm{u} \\
\delta q^\mathrm{a}
\end{bmatrix}}_{\delta q}}^\intercal
\underbrace{
\begin{bmatrix}
\mathbf{M}_\mathrm{u} / h & 0 \\
0 & h \mathbf{K}_\mathrm{a}
\end{bmatrix}}_{\mathbf{Q}}
\underbrace{
\begin{bmatrix}
\delta q^\mathrm{u} \\
\delta q^\mathrm{a}
\end{bmatrix}}_{\delta q}
+
{\underbrace{
h
\begin{bmatrix}
- \tau^\mathrm{u} \\
- \mathbf{K}_\mathrm{a} (u - q^\mathrm{a}) - \tau^\mathrm{a}
\end{bmatrix}}_{b}}^\intercal
\underbrace{
\begin{bmatrix}
\delta q^\mathrm{u} \\
\delta q^\mathrm{a}
\end{bmatrix}}_{\delta q}
\;\text{s.t.} \\
&\underbrace{\phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q}_{\text{Linearized} \; \phi_i(q + \delta q)} \geq 0, \; \forall i \in \{1 \cdots n_{\mathrm{c}}\}.
\end{aligned}
- For a frictionless multi-body system with nc contacts, the dynamics can be solved as a QP:
- Ki⋆ is the dual of the i-th friction cone.
- Still convex!
δqmin21δq⊺Qδq+b⊺δqs.t.[ϕi+JniδqJtiδq]∈Ki⋆,∀i∈{1⋯nc}.
\begin{aligned}
\underset{\delta q}{\min} \;
&\frac{1}{2} {\delta q}^\intercal \mathbf{Q} \delta q + b^\intercal \delta q \;\text{s.t.} \\
&
\begin{bmatrix}
\phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q \\
\mathbf{J}_{\mathrm{t}_i}\delta q
\end{bmatrix}
\in \mathcal{K}^\star_i, \; \forall i \in \{1 \cdots n_{\mathrm{c}}\}.
\end{aligned}
- Anitescu [1] has a nice convex approximation of Coulomb friction constraints, allowing us to write down the dynamics as a Second-Order Cone Program (SOCP):
Tangential displacements
δqa,δqumin21hK(q+δqa−u)2+21hmu(δqu)2,s.t.ϕi(q+δq)≥0,∀i.
\begin{aligned}
\underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \frac{m^\mathrm{u}}{h} (\delta q^\mathrm{u})^2, \; \text{s.t.} \\
&\phi_i (q + \delta q)\geq 0, \; \forall i.
\end{aligned}
Dynamics of the Toy Problem
- Anitescu, Mihai. "Optimization-based simulation of nonsmooth rigid multibody dynamics." Mathematical Programming 105.1 (2006): 113-143.
Differentiability
δq⋆:=δqargmin.21δq⊺Qδq+b⊺δqs.t.[ϕi+JniδqJtiδq]∈Ki⋆,∀i∈{1⋯nc}.
\begin{aligned}
\delta q^\star \coloneqq \underset{\delta q}{\text{argmin}.} \;
&\frac{1}{2} {\delta q}^\intercal \mathbf{Q} \delta q + b^\intercal \delta q \;\text{s.t.} \\
&
\begin{bmatrix}
\phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q \\
\mathbf{J}_{\mathrm{t}_i}\delta q
\end{bmatrix}
\in \mathcal{K}^\star_i, \; \forall i \in \{1 \cdots n_{\mathrm{c}}\}.
\end{aligned}
Convex, Quasi-Static, Differentiable Dynamics (an SOCP)
q+=f(q,u)=q+δq⋆(q,u)
q_+ = f(q, u) = q + \delta q^\star(q, u)
A:=∂q∂f=I+∂q∂δq⋆,B:=∂u∂f=∂u∂δq⋆,
\newcommand{\DfDx}[2]{\frac{\partial {#1}}{\partial {#2}}}
\mathbf{A} \coloneqq \DfDx{f}{q} = \mathbf{I} + \DfDx{\delta q^\star}{q}, \;
\mathbf{B} \coloneqq \DfDx{f}{u} = \DfDx{\delta q^\star}{u},
- A,B can then be computed by applying the Implicit Function Theorem to the active constraints at optimality.
- Standard practice for many differentiable simulators.
- Allows first-order randomized smoothing.
Analytic Smoothing
Optimality Condition
δqa,δqumin21hK(q+δqa−u)2+21hmu(δqu)2−i∑κ1logϕi(q+δq)
\begin{aligned}
\underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \frac{m^\mathrm{u}}{h} (\delta q^\mathrm{u})^2
- \sum_i\frac{1}{\kappa}\log{\phi_i(q + \delta q)}
\end{aligned}
⇔
\Leftrightarrow
⎩⎨⎧hK(qa+δqa−u)=−λ1+λ2,(mu/h)δqu=λ1−λ3,ϕi(q+δq)≥0,∀i,λi≥0,∀i,ϕ(q+δq)λ=0,∀i.
\begin{cases}
hK(q^\mathrm{a} + \delta q^\mathrm{a} - u) = -\lambda_1 + \lambda_2, \\
(m^\mathrm{u} / h) \delta q^\mathrm{u} = \lambda_1 - \lambda_3, \\
\phi_i(q + \delta q) \geq 0, \; \forall i, \\
\lambda_i \geq 0, \; \forall i, \\
\phi (q + \delta q) \lambda = 0, \forall i.
\end{cases}
qa
q^\mathrm{a}
u
u
Ka
K_\mathrm{a}
qu
q^\mathrm{u}
λ1
\lambda_1
λ3
\lambda_3
λ1
\lambda_1
λ2
\lambda_2
ϕ2(q)
\phi_2({q})
ϕ1(q)
\phi_1({q})
ϕ3(q)
\phi_3({q})
δqa,δqumin21hK(q+δqa−u)2+21hmu(δqu)2,s.t.ϕi(q+δq)≥0,∀i.
\begin{aligned}
\underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \frac{m^\mathrm{u}}{h} (\delta q^\mathrm{u})^2, \; \text{s.t.} \\
&\phi_i (q + \delta q)\geq 0, \forall i.
\end{aligned}
⇔
\Leftrightarrow
KKT
Original Dynamics
Smoothed Dynamics
⇔
\Leftrightarrow
δqa,δqumin21hK(q+δqa−u)2+21hmu(δqu)2+i∑I−(ϕi(q+δq))
\begin{aligned}
\underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \frac{m^\mathrm{u}}{h} (\delta q^\mathrm{u})^2
+ \sum_i I_-\left(\phi_i (q + \delta q) \right)
\end{aligned}
I−(a)={0,a≥0∞,a<0
I_-(a) =
\begin{cases}
0, \; a \geq 0 \\
\infty, \; a < 0
\end{cases}
⎩⎨⎧hK(qa+δqa−u)=−λ1+λ2,(mu/h)δqu=λ1−λ3,ϕi(q+δq)≥0,∀i,λi≥0,∀i,ϕi(q+δq)λ=1/κ,∀i.
\begin{cases}
hK(q^\mathrm{a} + \delta q^\mathrm{a} - u) = -\lambda_1 + \lambda_2, \\
(m^\mathrm{u} / h) \delta q^\mathrm{u} = \lambda_1 - \lambda_3, \\
\phi_i(q + \delta q) \geq 0, \; \forall i, \\
\lambda_i \geq 0, \; \forall i, \\
\phi_i (q + \delta q) \lambda = 1 / \kappa, \forall i.
\end{cases}
Analytic Smoothing with Friction
δqmin.21δq⊺Qδq+b⊺δqs.t.[ϕi+JniδqJtiδq]∈Ki⋆,∀i∈{1⋯nc}.
\begin{aligned}
\underset{\delta q}{\text{min}.} \;
&\frac{1}{2} {\delta q}^\intercal \mathbf{Q} \delta q + b^\intercal \delta q \;\text{s.t.} \\
&
\begin{bmatrix}
\phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q \\
\mathbf{J}_{\mathrm{t}_i}\delta q
\end{bmatrix}
\in \mathcal{K}^\star_i, \; \forall i \in \{1 \cdots n_{\mathrm{c}}\}.
\end{aligned}
δqmin.21δq⊺Qδq+b⊺δq−κ1∑i=1nclog[μi2(ϕi+Jniδq)2−(Jtiδq)⊺(Jtiδq)]
\underset{\delta q}{\text{min}.} \;
\frac{1}{2} {\delta q}^\intercal \mathbf{Q} \delta q + b^\intercal \delta q
-
\frac{1}{\kappa}
\sum_{i=1}^{n_\mathrm{c}}
\log
\left[
\frac{(\phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q)^2}{\mu_i^2} - (\mathbf{J}_{\mathrm{t}_i}\delta q)^\intercal (\mathbf{J}_{\mathrm{t}_i}\delta q)
\right]
Put constraints in a generalized log for κi⋆
⇒
\Rightarrow
- An unconstrained convex program.
- Can be solved with Newton's method.
- Also differentiable.
Convex, Quasi-Dynamic, Differentiable Dynamics (an SOCP)
Notation:
q+=f(q,u)
q_+ = f(q, u)
Original dynamics
q+=fρ(q,u)
q_+ = f_\rho(q, u)
Smoothed dynamics
Global Sampling-based Contact-rich Planning with Quasi-static Contact Models
Smoothed gradients enable trajectory optimization for dexterous hands, but...
Task: Turning the ball by 30 degrees.
What about 180 degrees?
Finding this trajectory requires powering through local minima!
Sampling-based Motion Planing is Great at Global Exploration
- Rapidly-Exploring Random Tree (RRT): iteratively builds a tree that fills the state space. [1]
- Steven M. LaValle, "Planning Algorithms", Cambridge University Press , 2006.
One iteration of RRT (simplified)
C
\mathcal{C}
(1) Sample subgoal
C
\mathcal{C}
(2) Find nearest node
C
\mathcal{C}
(3) Grow towards
"Nearest" is tricky to define under dynamics constraint.
- Dynamic reachability is essential for efficient exploration. [1]
- H.J.T. Suh, J. Deacon, Q. Wang, "A Fast PRM Planner for Car-like Vehicles", self-hosted, 2018.
How do we measure reachability under the contact-dynamics constraint?
A distance "metric" based on smoothed linearization
q+=fρ(q,u)
q_+ = f_\rho(q, u)
Rρ,εu(qˉ):={Bρu(qˉ,qˉa)δu+μρu∣∥δu∥≤ε}
\mathcal{R}_{\rho, \varepsilon}^\mathrm{u} (\bar{q})\coloneqq \left\{\mathbf{B}_\rho^\mathrm{u}(\bar{q},\bar{q}^{\mathrm{a}}) \delta u + \mu_\rho^\mathrm{u}\; | \; \|\delta u\|\leq \varepsilon\right\}
Smoothed Contact Dynamics
A node in the RRT tree: qˉ=[qˉa,qˉu]
q+=∂u∂fρ(qˉ,qˉa)Bρ(qˉ,qˉa)(u−qˉa)δu+fρ(qˉ,qˉa)μρ
\newcommand{\DfDx}[2]{\frac{\partial {#1}}{\partial {#2}}}
q_+ = \underbrace{\mathbf{B}_\rho(\bar{q}, \bar{q}^\mathrm{a})}_{\DfDx{f_\rho}{u}(\bar{q},\bar{q}^\mathrm{a})}
\underbrace{\delta u}_{(u - \bar{q}^\mathrm{a})}
+ \underbrace{\mu_\rho}_{f_\rho(\bar{q}, \bar{q}^\mathrm{a})}
Take the rows corresponding to the object:
dρu(q;qˉ):=21(qu−μρu)⊺[BρuBρu⊺]−1(qu−μρu)
\begin{aligned}
d^\mathrm{u}_{\rho}(q; \bar{q}) &\coloneqq \frac{1}{2} (q^\mathrm{u} - \mu^\mathrm{u}_\rho)^\intercal {\left[\mathbf{B}_{\rho}^{\mathrm{u}} {\mathbf{B}_{\rho}^{\mathrm{u}}}^\intercal\right]}^{-1} (q^\mathrm{u} - \mu^\mathrm{u}_\rho) \\
\end{aligned}
Smoothed
Smoothed Input Linearization:
ε sub-level set
Cu
\mathcal{C}^\mathrm{u}
configuration space of the object.
q+u=Bρu(qˉ,qˉa)δu+μρu
\newcommand{\DfDx}[2]{\frac{\partial {#1}}{\partial {#2}}}
q_+^\mathrm{u} = \mathbf{B}^\mathrm{u}_\rho(\bar{q}, \bar{q}^\mathrm{a}) \delta u
+ \mu_\rho^\mathrm{u}
un-actauted(objects)
dρu(⋅;qˉ) locally reflects dynamic reachability
Rρ,εu(qˉ):={Bρu(qˉ,qˉa)δu+μρu∣∥δu∥≤ε}
\mathcal{R}_{\rho, \varepsilon}^\mathrm{u} (\bar{q})\coloneqq \left\{\mathbf{B}_\rho^\mathrm{u}(\bar{q},\bar{q}^{\mathrm{a}}) \delta u + \mu^\mathrm{u}_\rho\; | \; \|\delta u\|\leq \varepsilon\right\}
dρu(q;qˉ):=21(qu−μρu)⊺[BρuBρu⊺]−1(qu−μρu)
\begin{aligned}
d^\mathrm{u}_{\rho}(q; \bar{q}) &\coloneqq \frac{1}{2} (q^\mathrm{u} - \mu^\mathrm{u}_\rho)^\intercal {\left[\mathbf{B}_{\rho}^{\mathrm{u}} {\mathbf{B}_{\rho}^{\mathrm{u}}}^\intercal\right]}^{-1} (q^\mathrm{u} - \mu^\mathrm{u}_\rho) \\
\end{aligned}
ε sub-level set
(Distance Metric Demo)
RRT through contact (so far)
One iteration of RRT through Contact
(1) Sample subgoal ▲
Cu
\mathcal{C}^\mathrm{u}
Cu
\mathcal{C}^\mathrm{u}
(2) Find nearest node
qa is only changed locally, this is against the RRT spirit! (demo)
Dynamically-consistent Extension
q+u=Bρuδu+μρu
\newcommand{\DfDx}[2]{\frac{\partial {#1}}{\partial {#2}}}
q_+^\mathrm{u} = \mathbf{B}^\mathrm{u}_\rho \delta u
+ \mu_\rho^\mathrm{u}
is only valid locally.
δu⋆=δuargmin∥Bρuδu+μρu−q▲u∥2,s.t.∥δu∥≤ε
\begin{aligned}
\delta u^\star = \underset{\delta u}{\mathrm{argmin}} &\|\mathbf{B}^\mathrm{u}_\rho \delta u + \mu_\rho^\mathrm{u} - q^\mathrm{u}_\blacktriangle \|^2, \; \text{s.t.} \\
& \|\delta u\| \leq \varepsilon
\end{aligned}
f(qˉ,qˉa+δu⋆)
f(\bar{q}, \bar{q}^\mathrm{a} + \delta u^\star)
qˉ
\bar{q}
Cu
\mathcal{C}^\mathrm{u}
(3) Grow towards ▲
Introducing contact sampling
One iteration of RRT through contact, with contact sampling
(1) Sample a different grasp (qa) for one of the nodes, giving a new distance metric
Cu
\mathcal{C}^\mathrm{u}
Contact sampling allows global exploration of qa on the contact manifold!
Cu
\mathcal{C}^\mathrm{u}
(2) Find nearest node
Cu
\mathcal{C}^\mathrm{u}
(3) Grow towards
Contact-sampler is system-specific
- Treat each finger as a robot arm and solve IK.
- With the hand open, pick a random direction to close the fingers until contact.
RRT through contact
Three modifications to the vanilla RRT are made:
- Nearest node on tree is found using dρu(q;qˉ).
- Contact sampling: global exploration of robot configuration near the contact manifold.
- Dynamic-consistent extension: edges respect non-smooth quasi-static dynamics.
Distance Metric
RRT tree for a simple system with contacts
Ablation study: what trees look like after growing a fixed number of nodes.
dρu(q;qˉ):=21(qu−μρu)⊺[BρuBρu⊺]−1(qu−μρu)
\begin{aligned}
d^\mathrm{u}_{\rho}(q; \bar{q}) &\coloneqq \frac{1}{2} (q^\mathrm{u} - \mu^\mathrm{u}_\rho)^\intercal {\left[\mathbf{B}_{\rho}^{\mathrm{u}} {\mathbf{B}_{\rho}^{\mathrm{u}}}^\intercal\right]}^{-1} (q^\mathrm{u} - \mu^\mathrm{u}_\rho) \\
\end{aligned}
Sim2Real/Hardware Transfer
Sim2Real/Hardware Transfer
Sim2Real/Hardware Transfer
What's next?
Low-resolution models
High-resolution models
Locomotion
[Boston Dynamics]
PV=nRT
PV = nRT
Heat Cycle of an Internal Combustion Engine
Contact-rich Manipulation
Quasi-static contact dynamics?
- Full second-order dynamics?
- Second-order centroidal dynamics?
End of Presentation
Quasi-static dynamics is good for manipulation planning!
- Spatial benefit: Smaller state space ([q,v] vs. only q).
-
Temporal benefit: Ignoring transients => looking into the future with fewer steps.
-
Many manipulation tasks are quasi-static.
qa
q^\mathrm{a}
ma
m^\mathrm{a}
u
u
K
K
D
D
Second-order Dynamics
Quasi-static Dynamics
q+a
q^\mathrm{a}_+
u
u
RRT through contact (so far)
One iteration of RRT through Contact
Cu
\mathcal{C}^\mathrm{u}
(3) Grow towards
But only take small actions.
:qgoalu
: q^\mathrm{u}_\mathrm{goal}
:qstartu
: q^\mathrm{u}_\mathrm{start}
Normalized ellipsoid volume
qu:=[xu],qa:=[xa,ya].
q^\mathrm{u} \coloneqq [x^\mathrm{u}],\; q^\mathrm{a} \coloneqq [x^\mathrm{a}, y^\mathrm{a}].
(1) Sample subgoal
Cu
\mathcal{C}^\mathrm{u}
Cu
\mathcal{C}^\mathrm{u}
(2) Find nearest node
qa is only changed locally!
Introducing contact sampling
One iteration of RRT through contact, with contact sampling
(1) Sample a different grasp (qa) for one of the nodes, giving a new distance metric
Cu
\mathcal{C}^\mathrm{u}
Contact sampling allows global exploration of qa on the contact manifold.
qu:=[xu],qa:=[xa,ya].
q^\mathrm{u} \coloneqq [x^\mathrm{u}],\; q^\mathrm{a} \coloneqq [x^\mathrm{a}, y^\mathrm{a}].
:qgoalu
: q^\mathrm{u}_\mathrm{goal}
:qstartu
: q^\mathrm{u}_\mathrm{start}
Normalized ellipsoid volume
Cu
\mathcal{C}^\mathrm{u}
(2) Find nearest node
Cu
\mathcal{C}^\mathrm{u}
(3) Grow towards
But only take small actions.
Sim2Real/Hardware Transfer
Rotating the Ball by 45 degrees.
The modified RRT works well for contact-rich tasks!
Planning wall-clock time (seconds)
Why is contact-rich planning hard?
Contact dynamics is non-smooth!
(a)
(b)
q+u−qu
q^\mathrm{u}_+ - q^\mathrm{u}
u−qu
u - q^\mathrm{u}
No contact
Contact
q+u−qu
q^\mathrm{u}_+ - q^\mathrm{u}
u−qu
u - q^\mathrm{u}
No contact
Contact
Global Search with Contact Modes
q+u
q^\mathrm{u}_+
u
u
No contact
Contact
uminimize21(q+u−qgoalu)2
\underset{u}{\mathrm{minimize}} \; \frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2
−∂u∂(21(q+u−qgoalu)2)=(qgoalu−q+u)∂u∂q+u
-\frac{\partial }{\partial u} \left(\frac{1}{2}(q^\mathrm{u}_+ - q^\mathrm{u}_\text{goal})^2\right)
= (q^\mathrm{u}_\text{goal} - q^\mathrm{u}_+) \frac{\partial q_+^\mathrm{u}}{\partial u}
Solve with gradient descent
u
u
No contact
Contact
q+u
q^\mathrm{u}_+
∂u∂q+u=0
\frac{\partial q_+^\mathrm{u}}{\partial u} = 0
∂u∂q+u=1
\frac{\partial q_+^\mathrm{u}}{\partial u} = 1
∂u∂q+u=1
\frac{\partial q_+^\mathrm{u}}{\partial u} = 1
∂u∂q+u=0
\frac{\partial q_+^\mathrm{u}}{\partial u} = 0
qa
q^\mathrm{a}
u
u
qu
q^\mathrm{u}
qa
q^\mathrm{a}
u
u
qu
q^\mathrm{u}
(a)
(b)
q+u
q^\mathrm{u}_+
u
u
No contact
Contact
u
u
No contact
Contact
q+u
q^\mathrm{u}_+
∂u∂q+u=0
\frac{\partial q_+^\mathrm{u}}{\partial u} = 0
∂u∂q+u=1
\frac{\partial q_+^\mathrm{u}}{\partial u} = 1
∂u∂q+u=1
\frac{\partial q_+^\mathrm{u}}{\partial u} = 1
∂u∂q+u=0
\frac{\partial q_+^\mathrm{u}}{\partial u} = 0
(c)
(d)
Why is contact-rich planning hard?
Contact dynamics is non-smooth!
Two solutions
Descend with gradients of smoothed dynamics
Reason explicitly about contact mode transitions
q+a
q^\mathrm{a}_+
u
u
Contact
No Contact
- Non-linear optimization can scale to complex systems.
- But descent methods get stuck in local minima.
- Good at escaping local minima,
- But mode enumeration scales poorly (exponentially) with the number of contacts.
q+u
q^\mathrm{u}_+
u
u
No contact
Contact
Push Left
Do not Push Left
Push Right
Do not Push Right
x
Contact-rich Planning for Robotic Manipulation with Quasi-static Contact Models Tao Pang Word cloud of my PhD thesis :D