Trajectory Optimization for Legged Robots

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

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

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}
m r¨=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(t/ Ci)=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)&lt;μ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 &lt; \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

$ 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

Gait and Trajectory Optimization for Legged Systems

By Alexander W. Winkler

Gait and Trajectory Optimization for Legged Systems

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

  • 1,248
Loading comments...

More from Alexander W. Winkler