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

Made with Slides.com