Trajectory Optimization for Legged Robots

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

awinkler.me

Paper published in IEEE Robotic and Automation Letters (RA-L 2018) ⋅ \cdot DOI: 10.1109/LRA.2018.2798285

github.com/awinkler

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⁡wa(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}
\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

?

x(t),u(t)\mathbf{x}(t), \mathbf{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⁡w0subject 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}
\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 {x(t),u(t)} \{ \mathbf{x}(t), \mathbf{u}(t) \}

fullfills all contraints

findr(t)∈R3(CoM)\text{find} \quad \mathbf{r}(t) \in \mathbb{R}^3 \quad \text{(CoM)}
\text{find} \quad \mathbf{r}(t) \in \mathbb{R}^3 \quad \text{(CoM)}
θ(t)∈R3(Base orientation)\mathbf{\theta}(t) \in \mathbb{R}^3 \quad \text{(Base orientation)}
\mathbf{\theta}(t) \in \mathbb{R}^3 \quad \text{(Base orientation)}
for every foot i∈{1,…,nee}:\text{for every foot } i \in \{1,\ldots,n_{ee}\}:
\text{for every foot } i \in \{1,\ldots,n_{ee}\}:
pi(t)∈R3(Foot position)\color{darkblue}{\mathbf{p}_i}(t) \in \mathbb{R}^3 \quad \text{(Foot position)}
\color{darkblue}{\mathbf{p}_i}(t) \in \mathbb{R}^3 \quad \text{(Foot position)}
fi(t)∈R3(Foot force)\color{red}{\mathbf{f}_i}(t) \in \mathbb{R}^3 \quad \text{(Foot force)}
\color{red}{\mathbf{f}_i}(t) \in \mathbb{R}^3 \quad \text{(Foot force)}
p1\mathbf{p}_1
\mathbf{p}_1
p2\mathbf{p}_2
\mathbf{p}_2
p3\mathbf{p}_3
\mathbf{p}_3
p4\mathbf{p}_4
\mathbf{p}_4
f1\mathbf{f}_1
\mathbf{f}_1
f2\mathbf{f}_2
\mathbf{f}_2
r,\mathbf{r},
\mathbf{r},
θ\theta
\theta

Towards integrated motion-planning

keeping search-space as open as possible 

m r¨=∑i=14fi−mgm \, \mathbf{\ddot{r}} \quad \quad \quad \quad \quad = \sum_{i=1}^{4} {\color{red}\mathbf{f}_i} - m \mathbf{g}
m \, \mathbf{\ddot{r}} \quad \quad \quad \quad \quad = \sum_{i=1}^{4} {\color{red}\mathbf{f}_i} - m \mathbf{g}
I(θ) ω˙+ω ⁣× ⁣I(θ)ω=∑i=14fi ⁣× ⁣(r−pi)\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})
\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

p1\mathbf{p}_1
\mathbf{p}_1
p2\mathbf{p}_2
\mathbf{p}_2
p3\mathbf{p}_3
\mathbf{p}_3
p4\mathbf{p}_4
\mathbf{p}_4
f1\mathbf{f}_1
\mathbf{f}_1
f2\mathbf{f}_2
\mathbf{f}_2
I,m\mathbf{I}, m
\mathbf{I}, m
[r¨ω˙]\begin{bmatrix} \mathbf{\ddot{r}} \\ \mathbf{\dot{\omega}} \end{bmatrix}
\begin{bmatrix} \mathbf{\ddot{r}} \\ \mathbf{\dot{\omega}} \end{bmatrix}

Kinematic Model

pi∈Ri(r,θ){\color{#1c4587}\mathbf{p}_i} \in {\color{#1c4587}\mathcal{R}_i}(\mathbf{r},\theta)
{\color{#1c4587}\mathbf{p}_i} \in {\color{#1c4587}\mathcal{R}_i}(\mathbf{r},\theta)
p1\mathbf{p}_1
\mathbf{p}_1
p2\mathbf{p}_2
\mathbf{p}_2
r,θ\mathbf{r}, \mathbf{\theta}
\mathbf{r}, \mathbf{\theta}
R2\mathcal{R}_2
\mathcal{R}_2
R1\mathcal{R}_1
\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 ΔTi\Delta T_i

ΔTR,1\Delta T_{R,1}
\Delta T_{R,1}
ΔTR,2\Delta T_{R,2}
\Delta T_{R,2}
ΔTR,3\Delta T_{R,3}
\Delta T_{R,3}
ΔTL,1\Delta T_{L,1}
\Delta T_{L,1}
ΔTL,2\Delta T_{L,2}
\Delta T_{L,2}
ΔTL,3\Delta T_{L,3}
\Delta T_{L,3}
ΔTL,4\Delta T_{L,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 pi(t) \mathbf{p}_i(t) cannot move while

fi(t∉Ci)=0\color{red}{\mathbf{f}_i} (t\notin\mathcal{C}_i) = \mathbf{0}
\color{red}{\mathbf{f}_i} (t\notin\mathcal{C}_i) = \mathbf{0}
p˙i(t∈C)=0\color{blue}{\dot{\mathbf{p}}_i} (t\in \mathcal{C}) = \mathbf{0}
\color{blue}{\dot{\mathbf{p}}_i} (t\in \mathcal{C}) = \mathbf{0}

Physical Restrictions 

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

standing

swinging

Terrain constraints 

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{blue}p_{i,s}^z} = h({\color{blue}p_{i,s}^x}, {\color{blue}p_{i,s}^y})
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
{\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)⋅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})
\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)∈R h(x,y) \in \mathbb{R} 
  • normals n(x,y)∈R3 \mathbf{n}(x,y) \in \mathbb{R}^3
  • tangents t(x,y)∈R3 \mathbf{t}(x,y) \in \mathbb{R}^3
t∈Ct \in \mathcal{C}
t \in \mathcal{C}

Given:

 open-sourced software

Summary 

Computation Time                          100 ms

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

www.awinkler.me
$ sudo apt-get install ros-kinetic-xpp

These slides, papers and more at

J. Buchli

M. Hutter

D. Bellicoso

$ sudo apt-get install ros-kinetic-towr_ros
$ sudo apt-get install ros-kinetic-ifopt

A. Winkler

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

Gait and Trajectory Optimization for Legged Systems

By Alexander W. Winkler

Made with Slides.com

Gait and Trajectory Optimization for Legged Systems

Paper: https://ieeexplore.ieee.org/document/8283570/ Recorded talk: https://youtu.be/KhWuLvb934g

  • 7,533

More from Alexander W. Winkler