# 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

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}

off-the-shelf

NLP Solver

Mathematical Optimization Problem

Direct Method

Collocation

### ?

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

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

### Towards integrated motion-planning

keeping search-space as open as possible

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

### 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

### 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}
\Delta T_{R,2}
\Delta T_{R,3}
\Delta T_{L,1}
\Delta T_{L,2}
\Delta T_{L,3}
\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}
\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})
{\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

• 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}

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

#### 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

• 6,346