Contact-Implicit
Model-Predictive Control
Taylor Howell and Simon Le Cleac'h
motivation: control through contact
motivation: linear MPC
linearization challenges
x(t)
reference trajectory
valid linearization domain
linearization challenges
x(t)
valid linearization domain
reference trajectory
\text{time}
\text{position}
\text{time}
\text{velocity}
strategic linearization
\underset{x_{t+1}}{\text{find}} \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \\
\text{subject to} \quad \textbf{dynamics}(x_{t+1}, \gamma) = 0\\
% \quad \:\: \textbf{sdf}(x_{t+1}) = \phi \\
% \gamma \circ \phi = 0 \\
% \gamma, \phi \geq 0 \\
nonlinear complementarity problem (NCP)
impact force
strategic linearization
\underset{x_{t+1}}{\text{find}} \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \\
\text{subject to} \quad \textbf{dynamics}(x_{t+1}, \gamma) = 0\\
\quad \:\: \textbf{sdf}(x_{t+1}) = \phi \\
% \gamma \circ \phi = 0 \\
% \gamma, \phi \geq 0 \\
nonlinear complementarity problem (NCP)
impact force
slack variable
strategic linearization
\underset{x_{t+1}}{\text{find}} \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \\
\text{subject to} \quad \textbf{dynamics}(x_{t+1}, \gamma) = 0\\
\quad \:\: \textbf{sdf}(x_{t+1}) = \phi \\
\gamma \circ \phi = 0 \\
% \gamma, \phi \geq 0 \\
nonlinear complementarity problem (NCP)
impact force
slack variable
strategic linearization
\underset{x_{t+1}}{\text{find}} \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \\
\text{subject to} \quad \textbf{dynamics}(x_{t+1}, \gamma) = 0\\
\quad \:\: \textbf{sdf}(x_{t+1}) = \phi \\
\gamma \circ \phi = 0 \\
\gamma, \phi \geq 0 \\
impact force
slack variable
nonlinear complementarity problem (NCP)
strategic linearization
\underset{x_{t+1}}{\text{find}} \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \\
\text{subject to} \quad \textbf{linear dynamics}(x_{t+1}, \gamma) = 0\\
\quad \:\: \textbf{sdf}(x_{t+1}) = \phi \quad \quad \quad \\
\gamma \circ \phi = 0 \quad \quad \quad \quad \\
\gamma, \phi \geq 0 \quad \quad \quad \quad \\
strategic linearization
\underset{x_{t+1}}{\text{find}} \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \\
\text{subject to} \quad \textbf{linear dynamics}(x_{t+1}, \gamma) = 0\\
\quad \quad \quad \quad \textbf{linear sdf}(x_{t+1}) = \phi \quad \quad \quad \\
\gamma \circ \phi = 0 \quad \quad \quad \quad \\
\gamma, \phi \geq 0 \quad \quad \quad \quad \\
linear complementarity problem (LCP)
benefits of LCP formulation
benefits of LCP formulation
preserve contact reasoning
→ adapt contact sequence online
benefits of LCP formulation
preserve contact reasoning
→ adapt contact sequence online
benefits of LCP formulation
preserve contact reasoning
→ adapt contact sequence online
benefits of LCP formulation
preserve contact reasoning
→ adapt contact sequence online
computational gains
→ real-time performance
benefits of LCP formulation
preserve contact reasoning
→ adapt contact sequence online
computational gains
→ real-time performance
contact-implicit MPC
\underset{x_{1:T}, u_{1:T-1}}{\text{minimize}} \quad \sum_{t=1}^{T} l_t(x_t, u_t) + l_T(x_T) \\
\text{subject to} \quad \:\:\: x_{t+1} = A_t x_t + B_t u_t + c_t \\
\quad (x_1 \, \text{given})
% \quad \quad \quad \: \: \: D_t x_t + E_t u_t + f_t \geq 0
contact-implicit MPC
\underset{x_{1:T}, u_{1:T-1}}{\text{minimize}} \quad \sum_{t=1}^{T} l_t(x_t, u_t) + l_T(x_T) \\
\text{subject to} \quad \:\:\: \sout{x_{t+1} = A_t x_t + B_t u_t + c_t} \\
\quad (x_1 \, \text{given})
% \quad \quad \quad \: \: \: D_t x_t + E_t u_t + f_t \geq 0
contact-implicit MPC
\underset{x_{1:T}, u_{1:T-1}}{\text{minimize}} \quad \sum_{t=1}^{T} l_t(x_t, u_t) + l_T(x_T) \\
\text{subject to} \quad \:\:\: \sout{x_{t+1} = A_t x_t + B_t u_t + c_t} \\
\quad \quad \quad \quad x_{t+1} = \textbf{LCP}_t(x_t, u_t) \\
\quad (x_1 \, \text{given})
% \quad \quad \quad \: \: \: D_t x_t + E_t u_t + f_t \geq 0
contact-implicit MPC
\underset{x_{1:T}, u_{1:T-1}}{\text{minimize}} \quad \sum_{t=1}^{T} l_t(x_t, u_t) + l_T(x_T) \\
\text{subject to} \quad \:\:\: x_{t+1} = \textbf{LCP}_t(x_t, u_t)\\
\quad (x_1 \, \text{given})
% \quad \quad \quad \quad x_{t+1} = \textbf{LCP}(x_t, u_t) \\
% \quad \quad \quad \: \: \: D_t x_t + E_t u_t + f_t \geq 0
contact-implicit MPC
\underset{x_{1:T}, u_{1:T-1}}{\text{minimize}} \quad \sum_{t=1}^{T} l_t(x_t, u_t) + l_T(x_T) \\
\text{subject to} \quad \:\:\: x_{t+1} = \textbf{LCP}_t(x_t, u_t)\\
\quad (x_1 \, \text{given})
% \quad \quad \quad \quad x_{t+1} = \textbf{LCP}(x_t, u_t) \\
% \quad \quad \quad \: \: \: D_t x_t + E_t u_t + f_t \geq 0
dynamics evaluation
dynamics gradient
solve LCP problem
differentiate LCP problem
implicit-function theorem
\frac{\partial \text{solution}}{\partial \text{data}} =
- {\left(\frac{\partial \text{residual}}{\partial \text{solution}}\right)}^{-1}
\frac{\partial \text{residual}}{\partial \text{data}}
\frac{\partial x_{t+1}}{\partial x_t}, \frac{\partial x_{t+1}}{\partial u_t}
github.com/dojo-sim/ContactImplicitMPC.jl
open-source implementation
results
quadruped: model mismatch
flamingo: unmodeled environment
disturbance rejection
offline motion generation → hardware execution
contact-implicit trajectory optimization
offline motion generation → hardware execution
contact-implicit trajectory optimization
contact-implicit MPC
offline motion generation → hardware execution
contact-implicit trajectory optimization
contact-implicit MPC
hardware transfer
offline motion generation → hardware execution
contact-implicit trajectory optimization
contact-implicit MPC
hardware transfer
offline motion generation → hardware execution
contact-implicit trajectory optimization
contact-implicit MPC
hardware transfer
team
Taylor Howell
Simon Le Cleac'h
Mac Schwager
Zachary Manchester
Chi Yen Lee
Shuo Yang
Contact-Implicit MPC (ELO-X)
By taylorhowell
Contact-Implicit MPC (ELO-X)
Presentation of Contact Implicit Model-Predictive Control for The Science of Bumping into Things workshop at RSS 2022.
