## Gait & Trajectory Optimization for Legged Robots

Alexander W. Winkler

www.awinkler.me

### Robot Motion Plan

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

NLP Solver

### Torque commands

Tracking Controller

"Gait and Trajectory Optimization for Legged Systems through Phase-based Endeffector Parameterization", RA-L, 2018

"Fast Trajectory Optimization for Legged Robots using Vertex-based ZMP Constraints", RA-L, 2017

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

Robot Model

Physical Laws

\mathbf{r}(t)
$\mathbf{r}(t)$
\mathbf{p}_c(t)
$\mathbf{p}_c(t)$
\mathbf{p}_{3}
$\mathbf{p}_{3}$
\mathbf{p}_{4}
$\mathbf{p}_{4}$
\mathbf{p}_{2}
$\mathbf{p}_{2}$
\mathbf{p}_{1}
$\mathbf{p}_{1}$

## Motion Plan

$\mathbf{r}(t) \in \mathbb{R}^2 \quad \text{(CoM)}$
\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}^2 \quad \text{(End-effector)}$
$\color{red}{\mathbf{p}_{c}}(t) \in \mathbb{R}^2 \quad \text{(CoP)}$

"Fast Trajectory Optimization for Legged Robots using Vertex-based ZMP Constraints", RA-L, 2017

## 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
• Pre-ordering of contact points
\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}$
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}$
\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}$
1. \quad \color{red}{\mathbf{p}_c} = \sum\limits_{i=1}^8 \color{red}{\lambda_i} \mathbf{v}_i(\color{blue}{\mathbf{p}},\alpha)
$1. \quad \color{red}{\mathbf{p}_c} = \sum\limits_{i=1}^8 \color{red}{\lambda_i} \mathbf{v}_i(\color{blue}{\mathbf{p}},\alpha)$
2. \quad \sum\limits_{i=1}^{8} \color{red}{\lambda_i} = 1
$2. \quad \sum\limits_{i=1}^{8} \color{red}{\lambda_i} = 1$
$3. \quad 0 \le \color{red}{\lambda_i}$

### Biped with non-point-feet

\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \end{bmatrix}
$\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \end{bmatrix}$
\le c_i
$\le c_i$
\color{red}{\mathbf{p}_c}
$\color{red}{\mathbf{p}_c}$
\color{blue}{\mathbf{p}_L}
$\color{blue}{\mathbf{p}_L}$
\color{blue}{\mathbf{p}_R}
$\color{blue}{\mathbf{p}_R}$
\color{red}{\mathbf{\lambda}_1}, \color{#7f6000}{c_1}, \mathbf{v}_1
$\color{red}{\mathbf{\lambda}_1}, \color{#7f6000}{c_1}, \mathbf{v}_1$

foothold

change

### Optimization Problem

$\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}$
• Reduce heuristics by simultaneously optimizing over body motion and foothods
• No vertical motions (jumps)
• No body orientation
• No friction force constraint
• Restricted to flat ground
• Uniformly handle point-, line- and area-contacts
• Remove need to order contact points
• Solvable in [ms]
• Contact schedule fixed

### Motion-Plan $$\mathbf{r}(t) \in \mathbb{R}^2 ,\ \color{black}{\mathbf{p}_i}(t) \in \mathbb{R}^2, \color{black}{\mathbf{p}_c}(t) \in \mathbb{R}^2$$

"Fast Trajectory Optimization for Legged Robots using Vertex-based ZMP Constraints"

### Mathematical Optimization Problem

$\mathbf{r}(t) \in \mathbb{R}^3 \quad \text{(CoM)}$
\mathbf{\theta}(t) \in \mathbb{R}^3 \quad \text{(Base orientation)}
$\mathbf{\theta}(t) \in \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}^3 \quad \text{(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)}
$\color{red}{\mathbf{f}_i}(t) \in \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$

## Motion-Plan

"Gait and Trajectory Optimization for Legged Systems through Phase-based Endeffector Parameterization", RA-L, 2018

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

## Dynamic Model

Single Rigid Body (Newton-Euler Equations)

## 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 without Integer Programming

R                         |   2  |           L           |       R        |      2

R     |              0            |  R |              2                |       R        |      2

$$\Rightarrow$$ Gait can be defined and changed through continuous variables $$\color{#7f6000}{\Delta T_i}$$

$$\Rightarrow$$ Pattern: Individual foot always alternates between swing and contact.

\color{#7f6000}{\Delta T_{R,1}}
$\color{#7f6000}{\Delta T_{R,1}}$
\color{#7f6000}{\Delta T_{R,2}}
$\color{#7f6000}{\Delta T_{R,2}}$
\color{#7f6000}{\Delta T_{R,3}}
$\color{#7f6000}{\Delta T_{R,3}}$
\color{#7f6000}{\Delta T_{L,1}}
$\color{#7f6000}{\Delta T_{L,1}}$
\color{#7f6000}{\Delta T_{L,2}}
$\color{#7f6000}{\Delta T_{L,2}}$
\color{#7f6000}{\Delta T_{L,3}}
$\color{#7f6000}{\Delta T_{L,3}}$
\color{#7f6000}{\Delta T_{L,4}}
$\color{#7f6000}{\Delta T_{L,4}}$
• Forces $$\mathbf{f}_i(t)$$ can only be generated while in contact
• Foot $$\mathbf{p}_i(t)$$ cannot move while in contact

## Phase-Based End-Effector Parameterization

$$\Rightarrow$$ Predefine which phase each polynomial belongs to

• Forces $$\mathbf{f}_i(t)$$ cannot exist while swinging
• Foot $$\mathbf{p}_i(t)$$ cannot move while standing
\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}$

### How to make use of swing-stance pattern?

$${}^2$$Percent of all visualized motions shown on real robot (R) and in simulation (S).

1

$${}^1$$Planning a flat ground, 1s-horizon, 4-foothold motion for a quadruped.

2

http://wiki.ros.org/towr
http://wiki.ros.org/towr
http://wiki.ros.org/xpp
RVIZ visualization
http://wiki.ros.org/ifopt

Eigen -> IPOPT/SNOPT
Core algorithm

Open-source software

A. W. Winkler, C. Bellicoso, M. Hutter, J. Buchli, "Gait and Trajectory Optimization for Legged Systems through Phase-based Endeffector Parameterization", IEEE Robotic and Automation Letters (RA-L), 2018

A. W. Winkler, F. Farshidian, D. Pardo, M. Neunert, J. Buchli, "Fast Trajectory Optimization for Legged Robots using Vertex-based ZMP Constraints", IEEE Robotic and Automation Letters (RA-L), 2017

www.awinkler.me

F. Farshidian

D. Pardo

M. Neunert

J. Buchli

M. Hutter

D. Bellicoso

# Trajectory Optimization

$\text{find} \quad \color{black}{x(t)}, \color{black}{u(t)} , \quad \text{for } t \in [0,T]$
\text{subject to} \quad \mathbf{x}(0) - \mathbf{x}_0 = 0
$\text{subject to} \quad \mathbf{x}(0) - \mathbf{x}_0 = 0$
\dot{x} - f(x(t), u(t)) = 0
$\dot{x} - f(x(t), u(t)) = 0$
h(x(t), u(t)) \leq 0
$h(x(t), u(t)) \leq 0$
x(T) - x_T = 0
$x(T) - x_T = 0$
x(t), u(t) = \text{arg min } J(x,u)
$x(t), u(t) = \text{arg min } J(x,u)$

## Objective

$\text{find} \quad \color{red}{x(t)}, \color{red}{u(t)} , \quad \text{for } t \in [0,T]$

## Legged Locomotion Constraints

• Forces cannot exist while swinging
• Foot cannot move while standing
• Foot can only stand on terrain, not in the air
• Forces can only push
• Forces inside friction cone
\color{green}{\checkmark}
$\color{green}{\checkmark}$
\color{green}{\checkmark}
$\color{green}{\checkmark}$

## Terrain constraints

p_i^z(t \in \mathcal{C}_{i,s}) = p_{i,s}^z = h_{terrain}(\mathbf{p}_{i,s}^{x,y})
$p_i^z(t \in \mathcal{C}_{i,s}) = p_{i,s}^z = h_{terrain}(\mathbf{p}_{i,s}^{x,y})$
\mathbf{f}_i(t) \cdot \mathbf{n}_{terrain}(\mathbf{p}_{i,s}^{xy}) \ge 0
$\mathbf{f}_i(t) \cdot \mathbf{n}_{terrain}(\mathbf{p}_{i,s}^{xy}) \ge 0$
\lvert \mathbf{f}_i(t)\cdot \mathbf{t}_{terrain}(\mathbf{p}_{i,s}^{xy}) \rvert < \mu \mathbf{f}_i(t) \cdot \mathbf{n}_{terrain}(\mathbf{p}_{i,s}^{xy})
$\lvert \mathbf{f}_i(t)\cdot \mathbf{t}_{terrain}(\mathbf{p}_{i,s}^{xy}) \rvert < \mu \mathbf{f}_i(t) \cdot \mathbf{n}_{terrain}(\mathbf{p}_{i,s}^{xy})$

Foot can only stand on terrain, not in the air:

Forces can only push:

Forces inside friction cone:

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

## Generating Torques to Track the Optimized Motion

\Leftrightarrow \ddot{\mathbf{q}}_{j,ref} = \mathbf{J}_j^+ ( \color{red}{\ddot{\mathbf{p}}} - \dot{\mathbf{J}}\dot{\mathbf{q}} - \mathbf{J}_b \color{red}{\ddot{\mathbf{q}}_{b,ref}} )
$\Leftrightarrow \ddot{\mathbf{q}}_{j,ref} = \mathbf{J}_j^+ ( \color{red}{\ddot{\mathbf{p}}} - \dot{\mathbf{J}}\dot{\mathbf{q}} - \mathbf{J}_b \color{red}{\ddot{\mathbf{q}}_{b,ref}} )$
\ddot{\mathbf{p}} = \mathbf{J} \ddot{\mathbf{q}} + \dot{\mathbf{J}} \dot{\mathbf{q}}
$\ddot{\mathbf{p}} = \mathbf{J} \ddot{\mathbf{q}} + \dot{\mathbf{J}} \dot{\mathbf{q}}$
\mathbf{P} = \mathbf{I} - \mathbf{J}_c^+ \mathbf{J}_c
$\mathbf{P} = \mathbf{I} - \mathbf{J}_c^+ \mathbf{J}_c$
\mathbf{\tau} = (\mathbf{P} \mathbf{S}^T)^+ \mathbf{P} ( \mathbf{M}\ddot{\mathbf{q}}_{ref} + \mathbf{C} )
$\mathbf{\tau} = (\mathbf{P} \mathbf{S}^T)^+ \mathbf{P} ( \mathbf{M}\ddot{\mathbf{q}}_{ref} + \mathbf{C} )$

[1] F. Aghili, “A unified approach for inverse and direct dynamics of constrained multibody systems based on linear projection operator: Applications to control and simulation,” IEEE T-RO, 2005.

[2] M. Mistry, J. Buchli, and S. Schaal, “Inverse dynamics control of floating base systems using orthogonal decomposition,” IEEE ICRA, 2010

Cartesian $$\to$$ Joint:

Joint+Contacts $$\to$$ Torque $$^{[1,2]}$$:

#### Gait & Trajectory Optimization for Legged Robots

By Alexander W Winkler

### Gait & Trajectory Optimization for Legged Robots

Talk at LAAS-CNRS

• 163