Optimization-based Motion-Planning for Legged Systems

Alexander Winkler

Jan 22, 2020 \( \cdot \) Facebook Reality Labs

Advantages of legged systems 

\( \bullet \) traverse rubble in earthquake \( \bullet \) reach trapped humans \( \bullet \) climb stairs  \( \bullet \)...

Agility ...vs rolling

Strength ...vs flying

\( \bullet \) carry heavy payload   \( \bullet \) open heavy doors \( \bullet \) rescue humans \( \bullet \) ...

vs

Source:

ANYbotics, Anymal bear, "Image: https://www.anybotics.com/anymal", 2018; Boston Dynamics, Atlas, "Image: https://www.bostondynamics.com/atlas", 2016; Italian Institute of Technology, HyQ2Max "Image: https://dls.iit.it/robots/hyq2max, 2018; Alphabet Waymo, Firefly car, "Image: https://waymo.com", 2016, DJI, Phantom 2 drone, "Image: https://www.dji.com/phantom-2", 2016

Source: https://www.youtube.com/watch?v=NX7QNWEGcNIa

Source: https://www.youtube.com/watch?v=arCOVKxGy9E

Robot Model \( \cdot \) Goal \( \cdot \) Environment

Desired Motion-Plan

Actuator Commands

force \( \cdot \) torque

Tracking

Controller

\min\limits_{\mathbf{w}} a(\mathbf{w}) \quad \text{subject to} \quad \mathbf{b}(\mathbf{w}) = \mathbf{0}, \quad \mathbf{c}(\mathbf{w})\ge \mathbf{0}

off-the-shelf

NLP Solver

Mathematical Optimization Problem (NLP)

\mathbf{x}(t), \mathbf{u}(t)

Task (continuous-time Optimal Control Problem)

Outline: Two different ways to model the physics of legged systems through mathematical equations.

?

\(\Rightarrow\) Cubic-Hermite splines

Represent continous motion \(\mathbf{u}(t), {\color{red}\mathbf{u}(t)}\) with discrete parameters

\color{black}{\mathbf{w}} = \{ \color{black}{x_0}, \color{black}{\dot{x}_0}, \color{black}{x_1}, \color{black}{\dot{x}_1}, \color{black}{x_2}, \color{black}{\dot{x}_2}, \color{black}{x_T}, \color{black}{\dot{x}_T} \}

Optimization parameters:

\min\limits_{\mathbf{w}} a(\mathbf{w}) \quad \text{subject to} \quad \mathbf{b}(\mathbf{w}) = \mathbf{0}, \quad \mathbf{c}(\mathbf{w})\ge \mathbf{0}

3rd-order polynomials defined by node values

Dynamic Model

\ddot{\mathbf{r}}(t) = \frac{g}{h}(\mathbf{r}(t)- \color{red}{\mathbf{p}_c}(t))

Linear Inverted Pendulum

\min\limits_{\mathbf{w}} a(\mathbf{w}) \quad \text{subject to} \quad \mathbf{b}(\mathbf{w}) = \mathbf{0}, \quad \mathbf{c}(\mathbf{w})\ge \mathbf{0}
\color{red}{\mathbf{p}_c}

.

Why it's difficult to plan a motion \(\{\mathbf{\ddot{q}}_{des}\}_{t=0..T}\)  for legged systems

(Base \( \in \mathbb{R}^6\))

\mathbf{M} \mathbf{\ddot{q}}_{des} + \mathbf{h} = \mathbf{J}^T \color{red}{\mathbf{f}}
\mathbf{f}
mg
  • external forces \(\mathbf{f}\) drive the system, but strong restrictions:
    • Forces can only push
    • forces only possible in contact
    • position of force fixed during stance
    • tangential forces must stay inside friction cone 
  • joint torques \(\tau\) can't directly drive the 6 DoF base  \( \Rightarrow \) underactuated

Unilateral Contact Forces \(\Leftrightarrow\) CoP inside Support-Area

\min\limits_{\mathbf{w}} a(\mathbf{w}) \quad \text{subject to} \quad \mathbf{b}(\mathbf{w}) = \mathbf{0}, \quad \mathbf{c}(\mathbf{w})\ge \mathbf{0}
\color{blue}{\mathbf{p}_4}
\color{blue}{\mathbf{p}_1}, \color{red}{\mathbf{\lambda}_1}, \color{#7f6000}{c_1}
\color{blue}{\mathbf{p}_2}
\color{blue}{\mathbf{p}_3}
\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 0 \end{bmatrix}
\mathbf{p}_c
1. \quad{\color{red}\mathbf{p}_c} = \sum\limits_{i=1}^{{\color{#7f6000}4}} \color{red}{\lambda_i} \color{blue}{\mathbf{p}_i}
2. \quad \sum\limits_{i=1}^{{\color{#7f6000}4}} {\color{red}\lambda_i} = 1
3. \quad 0 \le\color{red}{\lambda_i}
\le c_i

foothold change

Simultaneous Foothold and CoM Optimization

\min\limits_{\mathbf{w}} \mathbf{0} \quad \text{subject to} \quad \mathbf{b}(\mathbf{w}) = \mathbf{0}, \quad \mathbf{c}(\mathbf{w})\ge \mathbf{0}

Fast Trajectory Optimization for Legged Robots using Vertex-based ZMP Constraints

IEEE Robotic and Automation Letters (RA-L) \( \cdot \) 2017

A. W. Winkler, F. Farshidian, D. Pardo, M. Neunert, J. Buchli

  • Contact schedule
  • CoM height (no jumps)
  • Body orientation (horizontal)
  • Foothold height (flat ground)

Mathematical Optimization Problem

predefined:

Motion-Plan Search Space

restrict search space

all motion-plans \( \{ \mathbf{x}(t), \mathbf{u}(t) \} \)

fullfills all contraints

\min\limits_{\mathbf{w}} a(\mathbf{w}) \quad \text{subject to} \quad \mathbf{b}(\mathbf{w}) = \mathbf{0}, \quad \mathbf{c}(\mathbf{w})\ge \mathbf{0}
\text{find} \quad \mathbf{r}(t) \in \mathbb{R}^3 \quad \text{(CoM)}
\mathbf{\theta}(t) \in \mathbb{R}^3 \quad \text{(Base orientation)}
\text{for every foot } i \in \{1,\ldots,n_{ee}\}:
\color{darkblue}{\mathbf{p}_i}(t) \in \mathbb{R}^3 \quad \text{(Foot position)}
\color{red}{\mathbf{f}_i}(t) \in \mathbb{R}^3 \quad \text{(Foot force)}
\mathbf{p}_1
\mathbf{p}_2
\mathbf{p}_3
\mathbf{p}_4
\mathbf{f}_1
\mathbf{f}_2
\mathbf{r},
\theta

Gait and Trajectory Optimization for Legged Systems through Phase-based End-Effector Parameterization

IEEE Robotic and Automation Letters (RA-L) \( \cdot \) 2018

A. W. Winkler, D. Bellicoso, M. Hutter, J. Buchli

Towards integrated motion-planning

m \, \mathbf{\ddot{r}} \quad \quad \quad \quad \quad = \sum_{i=1}^{4} {\color{red}\mathbf{f}_i} - m \mathbf{g}
\mathbf{I}(\theta) \, \dot{\omega} + \omega\!\times\!\mathbf{I}(\theta) \omega = \sum_{i=1}^{4} {\color{red}\mathbf{f}_i}\!\times\!(\mathbf{r}-{\color{#1c4587}\mathbf{p}_i})

Dynamic Model

Single Rigid Body \( \cdot \) Newton-Euler Equations

\mathbf{p}_1
\mathbf{p}_2
\mathbf{p}_3
\mathbf{p}_4
\mathbf{f}_1
\mathbf{f}_2
\mathbf{I}, m
\begin{bmatrix} \mathbf{\ddot{r}} \\ \mathbf{\dot{\omega}} \end{bmatrix}
\min\limits_{\mathbf{w}} \mathbf{0} \quad \text{subject to} \quad \mathbf{b}(\mathbf{w}) = \mathbf{0}, \quad \mathbf{c}(\mathbf{w})\ge \mathbf{0}

Kinematic Model

{\color{#1c4587}\mathbf{p}_i} \in {\color{#1c4587}\mathcal{R}_i}(\mathbf{r},\theta)
\mathbf{p}_1
\mathbf{p}_2
\mathbf{r}, \mathbf{\theta}
\mathcal{R}_2
\mathcal{R}_1

Range-of-Motion Box \(\approx\) Joint limits

\min\limits_{\mathbf{w}} \mathbf{0} \quad \text{subject to} \quad \mathbf{b}(\mathbf{w}) = \mathbf{0}, \quad \mathbf{c}(\mathbf{w})\ge \mathbf{0}

Gait Optimization 

   ​    R     |              0            |  R |              2                |       R        |      2   

.... gait defined by continuous phase-durations \(\Delta T_i\)

without Integer Programming

swing

stance

individual foot always alternates between                       and

\Delta T_{R,1}
\Delta T_{R,2}
\Delta T_{R,3}
\Delta T_{L,1}
\Delta T_{L,2}
\Delta T_{L,3}
\Delta T_{L,4}

   R                         |   2  |           L           |       R        |      2      

Sequence:

Terrain constraints 

{\color{blue}p_{i,s}^z} = h({\color{blue}p_{i,s}^x}, {\color{blue}p_{i,s}^y})
{\color{red}\mathbf{f}_i(t)} \cdot \mathbf{n}({\color{blue}p_{i,s}^x}, {\color{blue}p_{i,s}^y}) \ge 0
\lvert {\color{red}\mathbf{f}_i(t)}\cdot \mathbf{t}({\color{blue}p_{i,s}^x}, {\color{blue}p_{i,s}^y}) \rvert < \mu {\color{red}\mathbf{f}_i(t)} \cdot \mathbf{n}({\color{blue}p_{i,s}^x}, {\color{blue}p_{i,s}^y})

Foot can only stand on terrain

Forces can only push 

Forces inside friction pyramid

t \in \mathcal{C}
\min\limits_{\mathbf{w}} \mathbf{0} \quad \text{subject to} \quad \mathbf{b}(\mathbf{w}) = \mathbf{0}, \quad \mathbf{c}(\mathbf{w})\ge \mathbf{0}

Gait and Trajectory Optimization for Legged Systems through Phase-based End-Effector Parameterization

IEEE Robotic and Automation Letters (RA-L) \( \cdot \) 2018

A. W. Winkler, D. Bellicoso, M. Hutter, J. Buchli

Character Simulation in Game Engines

using Unreal Engine 4, Blender, Blueprints, ...

Gait and Trajectory Optimization for Legged Systems through Phase-based End-Effector Parameterization

IEEE Robotic and Automation Letters (RA-L) \( \cdot \) 2018

A. W. Winkler, D. Bellicoso, M. Hutter, J. Buchli

Fast Trajectory Optimization for Legged Robots using Vertex-based ZMP Constraints

IEEE Robotic and Automation Letters (RA-L) \( \cdot \) 2017

A. W. Winkler, F. Farshidian, D. Pardo, M. Neunert, J. Buchli

www.alex-winkler.com

F. Farshidian

D. Pardo

M. Neunert

J. Buchli

M. Hutter

D. Bellicoso

Additional Material:

4

 open-sourced software

Summary 

Computation Time                          100 ms

1s-horizon, 4-footstep motion for a quadruped

Know if polynomial belongs to swing or stance phase

  • Foot \( \mathbf{p}_i(t)\) cannot move while

\color{red}{\mathbf{f}_i} (t\notin\mathcal{C}_i) = \mathbf{0}
\color{blue}{\dot{\mathbf{p}}_i} (t\in \mathcal{C}) = \mathbf{0}

Physical Restrictions 

  • Forces \(\mathbf{f}_i(t)\) cannot exist while

standing

swinging

Recreating Dust 2

using Unreal Engine 4, Blender, ...

Unilateral Contact Forces \(\Leftrightarrow\) CoP inside Support-Area

\color{red}{\mathbf{p}_c}
\color{blue}{\mathbf{p}_{1}}
\color{blue}{\mathbf{p}_{8}}, \color{red}{\mathbf{\lambda}_8}, \color{#7f6000}{c_8}
\color{blue}{\mathbf{p}_{2}}

.

.

.

.

.

.

.

.

\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \end{bmatrix}
1. \quad{\color{red}\mathbf{p}_c} = \sum\limits_{i=1}^{{\color{#7f6000}|\mathbf{c}|}} \color{red}{\lambda_i} \color{blue}{\mathbf{p}_i}
2. \quad \sum\limits_{i=1}^{{\color{#7f6000}|\mathbf{c}|}} {\color{red}\lambda_i} = 1
3. \quad 0 \le\color{red}{\lambda_i}
\le c_i
\mathbf{p}_c
\color{blue}{\mathbf{p}_2}
\color{blue}{\mathbf{p}_1}, \color{red}{\mathbf{\lambda}_1}, \color{#7f6000}{c_1}
\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 0 \end{bmatrix}
\color{blue}{\mathbf{p}_4}
\color{blue}{\mathbf{p}_3}
\min\limits_{\mathbf{w}} a(\mathbf{w}) \quad \text{subject to} \quad \mathbf{b}(\mathbf{w}) = \mathbf{0}, \quad \mathbf{c}(\mathbf{w})\ge \mathbf{0}
m \ddot{\mathbf{r}} = \sum_{i=1}^{4} \color{red}{\mathbf{f}_i} - m \mathbf{g}
\mathbf{I}(\theta)\dot{\omega} + \omega\!\times\!\mathbf{I}(\theta) \omega = \sum_{i=1}^{4} \color{red}{\mathbf{f}_i}\!\times\!(\mathbf{r}-\color{#1c4587}{\mathbf{p}_i})

Centroidal Dynamics \(\Rightarrow \) Single Rigid Body Dynamics

Newton-Euler Equations

+ Assumption A2: Momentum produced by the joint velocities is negligible.

+ Assumption A3: Full-body inertia remains similar to the one in nominal configuration.

\mathbf{A}(\mathbf{q}) \mathbf{\ddot{q}} + \mathbf{\dot{A}}(\mathbf{q},\mathbf{\dot{q}}) \mathbf{\dot{q}} = \begin{bmatrix} \sum_{i=1}^{4} \color{red}{\mathbf{f}_i} - m \mathbf{g} \\ \sum_{i=1}^{n_i} \color{red}{\mathbf{f}_i}\!\times\!(\mathbf{r}(\mathbf{q})-\color{#1c4587}{\mathbf{p}_i}(\mathbf{q})) \end{bmatrix}
      (pos) Assumptions
Rigid Body Dynamics (RBD) A1
Centroidal Dynamics (CD) A1
Single Rigid Body Dynamics (SRBD) A1, A2, A3
Linear Inverted Pendulum (LIP) A1, A2, A3, A4, A5, A6
\mathbf{q}_b, \mathbf{q}_j
\mathbf{q}_b, \mathbf{q}_j
\mathbf{r}, \mathbf{\theta},
r_x, r_y
\tau, \mathbf{f}_i
\mathbf{f}_i
\mathbf{f}_i
\mathbf{p}_c
\mathbf{p}_i
\mathbf{x}
\mathbf{\dot{x}} = \mathbf{F}(\mathbf{x}(t), \color{red}{\mathbf{u}(t)})
\mathbf{u}
x(t) = a_0 + a_1t + a_2t^2 + a_3t^3
\{
\color{black}{x_0}
\color{black}{\dot{x}_0}
\{
\{
\{
-\color{#7f6000}{\Delta T_1}^{-2} [ 3(\color{black}{x_0} - \color{black}{x_1}) + \color{#7f6000}{\Delta T_1}(2\color{black}{\dot{x}_0} + \color{black}{\dot{x}_1}) ]
\color{#7f6000}{\Delta T_1}^{-3} [ 2(\color{black}{x_0} - \color{black}{x_1}) + \color{#7f6000}{\Delta T_1}( \color{black}{\dot{x}_0} + \color{black}{\dot{x}_1}) ]
\color{black}{\mathbf{w}_j} = \{ \color{black}{x_0}, \color{black}{\dot{x}_0}, \color{#7f6000}{\Delta T_1}, \color{black}{x_1}, \color{black}{\dot{x}_1}, \color{#7f6000}{\Delta T_2}, \color{black}{x_2}, \color{black}{\dot{x}_2}, \color{#7f6000}{\Delta T_3}, \color{black}{x_T}, \color{black}{\dot{x}_T} \}

 Cubic-Hermite Spline for \(\color{red}{f_{\{x,y,z\}}(t)}, \color{blue}{p_{\{x,y,z\}}(t)}\)

\mathbf{p}_c
\mathbf{p}_c
{\color{red}\mathbf{p}_c}^T \mathbf{n}_i({\color{blue}\mathbf{p}}) + \text{offset}({\color{blue}\mathbf{p}}) > 0

Constraint: Unilateral Contact Forces \(\Leftrightarrow\) CoP inside Support-Area 

Difficult for single point-contacts or lines

\color{blue}{\mathbf{p}_1}
\color{blue}{\mathbf{p}_2}
\color{blue}{\mathbf{p}_3}

Ordering of contact points

1. \quad{\color{red}\mathbf{p}_c} = \sum\limits_{i=1}^4 \color{red}{\lambda_i} \color{blue}{\mathbf{p}_i}
2. \quad \sum\limits_{i=1}^{4} {\color{red}\lambda_i} = 1
3. \quad 0 \le\color{red}{\lambda_i}
\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 0 \end{bmatrix}
\mathbf{c} = \begin{bmatrix} 0 \\ 1 \\ 1 \\ 0 \end{bmatrix}
\le c_i
\mathbf{p}_c
\mathbf{p}_c

Vertex-Based Zmp-Constraint Formulation 

\color{blue}{\mathbf{p}_1}, \color{red}{\mathbf{\lambda}_1}, \color{#7f6000}{c_1}

Fast Trajectory Optimization for Legged Robots using Vertex-based ZMP Constraints

IEEE Robotic and Automation Letters (RA-L) \( \cdot \) 2017

A. W. Winkler, F. Farshidian, D. Pardo, M. Neunert, J. Buchli

Talk about real time c++ control, since this is also require to turn camera images into 3D meshes instantly

More emphasis on kinematic Planning at facebook

Icons from: https://www.behance.net/gallery/22478593/ICONS-Pack-2

Human Gait Picture from https://www.protokinetics.com/2018/11/28/understanding-phases-of-the-gait-cycle/

Made with Slides.com