### Optimization-based Motion Planning  for Legged Robots

Alexander W. Winkler

www.awinkler.me

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

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

### Dynamic Model

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

Linear Inverted Pendulum

\mathbf{p}_c
$\mathbf{p}_c$
\mathbf{p}_c
$\mathbf{p}_c$
\color{red}{\mathbf{p}_c}^T \mathbf{n}_i(\color{blue}{\mathbf{p}}) + \text{offset}(\color{blue}{\mathbf{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}
$\color{blue}{\mathbf{p}_1}$
\color{blue}{\mathbf{p}_2}
$\color{blue}{\mathbf{p}_2}$
\color{blue}{\mathbf{p}_3}
$\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. \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. \quad \sum\limits_{i=1}^{4} \color{red}{\lambda_i} = 1$
3. \quad 0 \le\color{red}{\lambda_i}
$3. \quad 0 \le\color{red}{\lambda_i}$
\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 0 \end{bmatrix}
$\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 0 \end{bmatrix}$
\mathbf{c} = \begin{bmatrix} 0 \\ 1 \\ 1 \\ 0 \end{bmatrix}
$\mathbf{c} = \begin{bmatrix} 0 \\ 1 \\ 1 \\ 0 \end{bmatrix}$
\le c_i
$\le c_i$
\mathbf{p}_c
$\mathbf{p}_c$
\mathbf{p}_c
$\mathbf{p}_c$

### Vertex-Based Zmp-Constraint Formulation

\color{blue}{\mathbf{p}_1}, \color{red}{\mathbf{\lambda}_1}, \color{#7f6000}{c_1}
$\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)}
$\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)}
$\mathbf{\theta}(t) \in \color{#07d507}{\mathbb{R}^3} \quad \text{(Base orientation)}$
\text{for every foot } i \in \{1,\ldots,n_{ee}\}:
$\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)}
$\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)}
$\color{red}{\mathbf{f}_i}(t) \in \color{#07d507}{\mathbb{R}^3} \quad \text{(Foot force)}$
\mathbf{p}_1
$\mathbf{p}_1$
\mathbf{p}_2
$\mathbf{p}_2$
\mathbf{p}_3
$\mathbf{p}_3$
\mathbf{p}_4
$\mathbf{p}_4$
\mathbf{f}_1
$\mathbf{f}_1$
\mathbf{f}_2
$\mathbf{f}_2$
\mathbf{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}$
\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

\mathbf{p}_1
$\mathbf{p}_1$
\mathbf{p}_2
$\mathbf{p}_2$
\mathbf{p}_3
$\mathbf{p}_3$
\mathbf{p}_4
$\mathbf{p}_4$
\mathbf{f}_1
$\mathbf{f}_1$
\mathbf{f}_2
$\mathbf{f}_2$
\mathbf{I}, m
$\mathbf{I}, m$
\begin{bmatrix} \mathbf{\ddot{r}} \\ \mathbf{\dot{\omega}} \end{bmatrix}
$\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)
$\color{#1c4587}{\mathbf{p}_i} \in \color{#1c4587}{\mathcal{R}_i}(\mathbf{r},\theta)$
\mathbf{p}_1
$\mathbf{p}_1$
\mathbf{p}_2
$\mathbf{p}_2$
\mathbf{r}, \mathbf{\theta}
$\mathbf{r}, \mathbf{\theta}$
\mathcal{R}_2
$\mathcal{R}_2$
\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 $$\Delta T_i$$

\Delta T_{R,1}
$\Delta T_{R,1}$
\Delta T_{R,2}
$\Delta T_{R,2}$
\Delta T_{R,3}
$\Delta T_{R,3}$
\Delta T_{L,1}
$\Delta T_{L,1}$
\Delta T_{L,2}
$\Delta T_{L,2}$
\Delta T_{L,3}
$\Delta T_{L,3}$
\Delta T_{L,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}
$\color{red}{\mathbf{f}_i} (t\notin\mathcal{C}_i) = \mathbf{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 $$\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{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
$\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})
$\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}
$t \in \mathcal{C}$

Given:

4

open-sourced software

### Summary

Computation Time                          100 ms

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

www.awinkler.me

### Publications

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

$$1^{\text{st}}$$ author

open-sourced

m \ddot{\mathbf{r}} = \sum_{i=1}^{4} \color{red}{\mathbf{f}_i} - m \mathbf{g}
$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})
$\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}
$\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
$\mathbf{q}_b, \mathbf{q}_j$
\mathbf{q}_b, \mathbf{q}_j
$\mathbf{q}_b, \mathbf{q}_j$
\mathbf{r}, \mathbf{\theta},
$\mathbf{r}, \mathbf{\theta},$
r_x, r_y
$r_x, r_y$
\tau, \mathbf{f}_i
$\tau, \mathbf{f}_i$
\mathbf{f}_i
$\mathbf{f}_i$
\mathbf{f}_i
$\mathbf{f}_i$
\mathbf{p}_c
$\mathbf{p}_c$
\mathbf{p}_i
$\mathbf{p}_i$
\mathbf{x}
$\mathbf{x}$
\mathbf{\dot{x}} = \mathbf{F}(\mathbf{x}(t), \color{red}{\mathbf{u}(t)})
$\mathbf{\dot{x}} = \mathbf{F}(\mathbf{x}(t), \color{red}{\mathbf{u}(t)})$
\mathbf{u}
$\mathbf{u}$
x(t) = a_0 + a_1t + a_2t^2 + a_3t^3
$x(t) = a_0 + a_1t + a_2t^2 + a_3t^3$
\{
$\{$
\color{black}{x_0}
$\color{black}{x_0}$
\color{black}{\dot{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}) ]
$-\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}) ]
$\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} \}
$\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)}$$