Optimization-based Motion Planning  for Legged Robots

Alexander W. Winkler

May 14, 2018 \( \cdot \) PhD Defense

Why legged machines?

\( \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

Goal \( \cdot \) position \( \cdot \) velocity \( \cdot \) duration \( \cdot \)

Robot \( \cdot \) kinematic \( \cdot \) dynamic

Environment \( \cdot \) terrain \( \cdot \) friction \( \cdot \) ...

Outline

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}
minwa(w)subject tob(w)=0,c(w)0\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 

Direct Methods

e.g. Collocation

?

Paper I

"Fast Trajectory Optimization for legged Systems using Vertex-based ZMP Constraints"

Paper 2

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

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

Task

Generating dynamic motions

Dynamic Model

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

Linear Inverted Pendulum

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

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

Difficult for single point-contacts or lines

\color{blue}{\mathbf{p}_1}
p1\color{blue}{\mathbf{p}_1}
\color{blue}{\mathbf{p}_2}
p2\color{blue}{\mathbf{p}_2}
\color{blue}{\mathbf{p}_3}
p3\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}
1.pc=i=14λipi1. \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
2.i=14λi=12. \quad \sum\limits_{i=1}^{4} \color{red}{\lambda_i} = 1
3. \quad 0 \le\color{red}{\lambda_i}
3.0λi3. \quad 0 \le\color{red}{\lambda_i}
\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 0 \end{bmatrix}
c=[1110]\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 0 \end{bmatrix}
\mathbf{c} = \begin{bmatrix} 0 \\ 1 \\ 1 \\ 0 \end{bmatrix}
c=[0110]\mathbf{c} = \begin{bmatrix} 0 \\ 1 \\ 1 \\ 0 \end{bmatrix}
\le c_i
ci\le c_i
\mathbf{p}_c
pc\mathbf{p}_c
\mathbf{p}_c
pc\mathbf{p}_c

Vertex-Based Zmp-Constraint Formulation 

\color{blue}{\mathbf{p}_1}, \color{red}{\mathbf{\lambda}_1}, \color{#7f6000}{c_1}
p1,λ1,c1\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

foothold

change

Simultaneous Foothold and CoM Optimization

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

\text{find} \quad \mathbf{r}(t) \in \mathbb{R}^\color{#07d507}{3} \quad \text{(CoM)}
findr(t)R3(CoM)\text{find} \quad \mathbf{r}(t) \in \mathbb{R}^\color{#07d507}{3} \quad \text{(CoM)}
\mathbf{\theta}(t) \in \color{#07d507}{\mathbb{R}^3} \quad \text{(Base orientation)}
θ(t)R3(Base orientation)\mathbf{\theta}(t) \in \color{#07d507}{\mathbb{R}^3} \quad \text{(Base orientation)}
\text{for every foot } i \in \{1,\ldots,n_{ee}\}:
for every foot i{1,,nee}:\text{for every foot } i \in \{1,\ldots,n_{ee}\}:
\color{darkblue}{\mathbf{p}_i}(t) \in \mathbb{R}^\color{#07d507}{3} \quad \text{(Foot position)}
pi(t)R3(Foot position)\color{darkblue}{\mathbf{p}_i}(t) \in \mathbb{R}^\color{#07d507}{3} \quad \text{(Foot position)}
\color{red}{\mathbf{f}_i}(t) \in \color{#07d507}{\mathbb{R}^3} \quad \text{(Foot force)}
fi(t)R3(Foot force)\color{red}{\mathbf{f}_i}(t) \in \color{#07d507}{\mathbb{R}^3} \quad \text{(Foot force)}
\mathbf{p}_1
p1\mathbf{p}_1
\mathbf{p}_2
p2\mathbf{p}_2
\mathbf{p}_3
p3\mathbf{p}_3
\mathbf{p}_4
p4\mathbf{p}_4
\mathbf{f}_1
f1\mathbf{f}_1
\mathbf{f}_2
f2\mathbf{f}_2
\mathbf{r},
r,\mathbf{r},
\theta
θ\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}
mr¨=i=14fimg 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})
I(θ)ω˙+ω×I(θ)ω=i=14fi×(rpi)\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
p1\mathbf{p}_1
\mathbf{p}_2
p2\mathbf{p}_2
\mathbf{p}_3
p3\mathbf{p}_3
\mathbf{p}_4
p4\mathbf{p}_4
\mathbf{f}_1
f1\mathbf{f}_1
\mathbf{f}_2
f2\mathbf{f}_2
\mathbf{I}, m
I,m\mathbf{I}, m
\begin{bmatrix} \mathbf{\ddot{r}} \\ \mathbf{\dot{\omega}} \end{bmatrix}
[r¨ω˙]\begin{bmatrix} \mathbf{\ddot{r}} \\ \mathbf{\dot{\omega}} \end{bmatrix}

Kinematic Model

\color{#1c4587}{\mathbf{p}_i} \in \color{#1c4587}{\mathcal{R}_i}(\mathbf{r},\theta)
piRi(r,θ)\color{#1c4587}{\mathbf{p}_i} \in \color{#1c4587}{\mathcal{R}_i}(\mathbf{r},\theta)
\mathbf{p}_1
p1\mathbf{p}_1
\mathbf{p}_2
p2\mathbf{p}_2
\mathbf{r}, \mathbf{\theta}
r,θ\mathbf{r}, \mathbf{\theta}
\mathcal{R}_2
R2\mathcal{R}_2
\mathcal{R}_1
R1\mathcal{R}_1

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

Gait Optimization 

   R                         |   2  |           L           |       R        |      2      

       R     |              0            |  R |              2                |       R        |      2   

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

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

without Integer Programming

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

Sequence:

swing

stance

individual foot always alternates between                       and

Phase-Based End-Effector Parameterization 

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}
fi(tCi)=0\color{red}{\mathbf{f}_i} (t\notin\mathcal{C}_i) = \mathbf{0}
\color{blue}{\dot{\mathbf{p}}_i} (t\in \mathcal{C}) = \mathbf{0}
p˙i(tC)=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

Terrain constraints 

\color{blue}{p_{i,s}^z} = h(\color{blue}{p_{i,s}^x}, \color{blue}{p_{i,s}^y})
pi,sz=h(pi,sx,pi,sy)\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
fi(t)n(pi,sx,pi,sy)0\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})
fi(t)t(pi,sx,pi,sy)<μfi(t)n(pi,sx,pi,sy)\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

  • height map \( h(x,y) \in \mathbb{R}\) 
  • normals \( \mathbf{n}(x,y) \in \mathbb{R}^3 \)
  • tangents \( \mathbf{t}(x,y) \in \mathbb{R}^3 \)
t \in \mathcal{C}
tCt \in \mathcal{C}

Given:

4

 open-sourced software

Summary 

Computation Time                          100 ms

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

Software

Publications

 + co-authored various others with F. Farshidian, D. Pardo, M. Neunert, ...

  \( 1^{\text{st}} \) author

 open-sourced

Additional Material:

m \ddot{\mathbf{r}} = \sum_{i=1}^{4} \color{red}{\mathbf{f}_i} - m \mathbf{g}
mr¨=i=14fimg 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})
I(θ)ω˙+ω×I(θ)ω=i=14fi×(rpi)\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}
A(q)q¨+A˙(q,q˙)q˙=[i=14fimgi=1nifi×(r(q)pi(q))]\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
qb,qj\mathbf{q}_b, \mathbf{q}_j
\mathbf{q}_b, \mathbf{q}_j
qb,qj\mathbf{q}_b, \mathbf{q}_j
\mathbf{r}, \mathbf{\theta},
r,θ,\mathbf{r}, \mathbf{\theta},
r_x, r_y
rx,ryr_x, r_y
\tau, \mathbf{f}_i
τ,fi\tau, \mathbf{f}_i
\mathbf{f}_i
fi\mathbf{f}_i
\mathbf{f}_i
fi\mathbf{f}_i
\mathbf{p}_c
pc\mathbf{p}_c
\mathbf{p}_i
pi\mathbf{p}_i
\mathbf{x}
x\mathbf{x}
\mathbf{\dot{x}} = \mathbf{F}(\mathbf{x}(t), \color{red}{\mathbf{u}(t)})
x˙=F(x(t),u(t))\mathbf{\dot{x}} = \mathbf{F}(\mathbf{x}(t), \color{red}{\mathbf{u}(t)})
\mathbf{u}
u\mathbf{u}
x(t) = a_0 + a_1t + a_2t^2 + a_3t^3
x(t)=a0+a1t+a2t2+a3t3x(t) = a_0 + a_1t + a_2t^2 + a_3t^3
\{
{\{
\color{black}{x_0}
x0\color{black}{x_0}
\color{black}{\dot{x}_0}
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}) ]
ΔT12[3(x0x1)+ΔT1(2x˙0+x˙1)]-\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}) ]
ΔT13[2(x0x1)+ΔT1(x˙0+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} \}
wj={x0,x˙0,ΔT1,x1,x˙1,ΔT2,x2,x˙2,ΔT3,xT,x˙T}\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)}\)