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

Alexander W. Winkler, Dario Bellicoso, Marco Hutter, Jonas Buchli

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

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 \) ...

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 Method

Collocation

?

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

Task

Optimization-based

Motion Planning

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

\min\limits_{\color{blue}{\mathbf{w}}} 0 \quad \text{subject to} \quad \color{blue}{\mathbf{b}(\mathbf{w})} = \mathbf{0}, \quad \color{blue}{\mathbf{c}(\mathbf{w})} \ge \mathbf{0}
minw0subject tob(w)=0,c(w)0\min\limits_{\color{blue}{\mathbf{w}}} 0 \quad \text{subject to} \quad \color{blue}{\mathbf{b}(\mathbf{w})} = \mathbf{0}, \quad \color{blue}{\mathbf{c}(\mathbf{w})} \ge \mathbf{0}
  • Contact schedule
  • CoM height (no jumps)
  • Body orientation (horizontal)
  • Foothold height (flat ground)

Mathematical Optimization Problem

predefined / "factorized":

Why integrated motion-planning?

restrict search space

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

fullfills all contraints

\text{find} \quad \mathbf{r}(t) \in \mathbb{R}^3 \quad \text{(CoM)}
findr(t)R3(CoM)\text{find} \quad \mathbf{r}(t) \in \mathbb{R}^3 \quad \text{(CoM)}
\mathbf{\theta}(t) \in \mathbb{R}^3 \quad \text{(Base orientation)}
θ(t)R3(Base orientation)\mathbf{\theta}(t) \in \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}^3 \quad \text{(Foot position)}
pi(t)R3(Foot position)\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)}
fi(t)R3(Foot force)\color{red}{\mathbf{f}_i}(t) \in \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

Towards integrated motion-planning

keeping search-space as open as possible 

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

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:

 open-sourced software

Summary 

Computation Time                          100 ms

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

Software

sudo apt-get install ros-kinetic-xpp

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

These slides and more at

F. Farshidian

D. Pardo

M. Neunert

J. Buchli

M. Hutter

D. Bellicoso

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)}\)

RA-L 2018 Paper

By Alexander W Winkler

RA-L 2018 Paper

"Gait and Trajectory Optimization for Legged Systems through Phase-based End-Effector Parameterization" Recorded talk: https://youtu.be/KhWuLvb934g

  • 34
Loading comments...

More from Alexander W Winkler