Contact-Implicit Model-Predictive Control
Simon Le Cleac'h and Taylor Howell
motivation: control through contact
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 (RSS 2022)
By simonlc
Contact Implicit MPC (RSS 2022)
Presentation of Contact Implicit Model-Predictive Control for The Science of Bumping into Things workshop at RSS 2022.
