Gait & Trajectory Optimization for Legged Robots

Alexander W. Winkler

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

NLP Solver

Mathematical Optimization Problem 

Problem

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

High-level Task

Robot Model

Physical Laws

\mathbf{r}(t)
r(t)\mathbf{r}(t)
\mathbf{p}_c(t)
pc(t)\mathbf{p}_c(t)
\mathbf{p}_{3}
p3\mathbf{p}_{3}
\mathbf{p}_{4}
p4\mathbf{p}_{4}
\mathbf{p}_{2}
p2\mathbf{p}_{2}
\mathbf{p}_{1}
p1\mathbf{p}_{1}

Motion Plan

\mathbf{r}(t) \in \mathbb{R}^2 \quad \text{(CoM)}
r(t)R2(CoM)\mathbf{r}(t) \in \mathbb{R}^2 \quad \text{(CoM)}
\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}^2 \quad \text{(End-effector)}
pi(t)R2(End-effector)\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)}
pc(t)R2(CoP)\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))
r¨(t)=gh(r(t)pc(t)) \ddot{\mathbf{r}}(t) = \frac{g}{h}(\mathbf{r}(t)- \color{red}{\mathbf{p}_c}(t))
\color{red}{\mathbf{p}_c}
pc\color{red}{\mathbf{p}_c}
\color{red}{\mathbf{p}_c}
pc\color{red}{\mathbf{p}_c}

Linear Inverted Pendulum

\mathbf{p}_c
pc\mathbf{p}_c
\mathbf{p}_c
pc\mathbf{p}_c
\color{red}{\mathbf{p}_c}^T \mathbf{n}_i(\color{blue}{\mathbf{p}}) + \text{offset}(\color{blue}{\mathbf{p}}) > 0
pcTni(p)+offset(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}
p1\color{blue}{\mathbf{p}_1}
\color{blue}{\mathbf{p}_2}
p2\color{blue}{\mathbf{p}_2}
\color{blue}{\mathbf{p}_3}
p3\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.pc=i=14λipi1. \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.i=14λi=12. \quad \sum\limits_{i=1}^{4} \color{red}{\lambda_i} = 1
3. \quad 0 \le\color{red}{\lambda_i}
3.0λi3. \quad 0 \le\color{red}{\lambda_i}
\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 0 \end{bmatrix}
c=[1110]\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 0 \end{bmatrix}
\mathbf{c} = \begin{bmatrix} 0 \\ 1 \\ 1 \\ 0 \end{bmatrix}
c=[0110]\mathbf{c} = \begin{bmatrix} 0 \\ 1 \\ 1 \\ 0 \end{bmatrix}
\le c_i
ci\le c_i
\mathbf{p}_c
pc\mathbf{p}_c
\mathbf{p}_c
pc\mathbf{p}_c

Vertex-Based Zmp-Constraint Formulation 

\color{blue}{\mathbf{p}_1}, \color{red}{\mathbf{\lambda}_1}, \color{#7f6000}{c_1}
p1,λ1,c1\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.pc=i=18λivi(p,α)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.i=18λi=12. \quad \sum\limits_{i=1}^{8} \color{red}{\lambda_i} = 1
3. \quad 0 \le \color{red}{\lambda_i}
3.0λi3. \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}
c=[11111111]\mathbf{c} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \end{bmatrix}
\le c_i
ci\le c_i
\color{red}{\mathbf{p}_c}
pc\color{red}{\mathbf{p}_c}
\color{blue}{\mathbf{p}_L}
pL\color{blue}{\mathbf{p}_L}
\color{blue}{\mathbf{p}_R}
pR\color{blue}{\mathbf{p}_R}
\color{red}{\mathbf{\lambda}_1}, \color{#7f6000}{c_1}, \mathbf{v}_1
λ1,c1,v1\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}
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}
  • 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)}
r(t)R3(CoM)\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

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}
mr¨=i=14fimg 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})
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 (Newton-Euler Equations)

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 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}}
ΔTR,1\color{#7f6000}{\Delta T_{R,1}}
\color{#7f6000}{\Delta T_{R,2}}
ΔTR,2\color{#7f6000}{\Delta T_{R,2}}
\color{#7f6000}{\Delta T_{R,3}}
ΔTR,3\color{#7f6000}{\Delta T_{R,3}}
\color{#7f6000}{\Delta T_{L,1}}
ΔTL,1\color{#7f6000}{\Delta T_{L,1}}
\color{#7f6000}{\Delta T_{L,2}}
ΔTL,2\color{#7f6000}{\Delta T_{L,2}}
\color{#7f6000}{\Delta T_{L,3}}
ΔTL,3\color{#7f6000}{\Delta T_{L,3}}
\color{#7f6000}{\Delta T_{L,4}}
ΔTL,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

Contact Model: Physical laws influenced by contact state

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}
fi(tCi)=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}

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

RVIZ visualization
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

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]
findx(t),u(t),for t[0,T]\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
subject tox(0)x0=0\text{subject to} \quad \mathbf{x}(0) - \mathbf{x}_0 = 0
\dot{x} - f(x(t), u(t)) = 0
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))0h(x(t), u(t)) \leq 0
x(T) - x_T = 0
x(T)xT=0x(T) - x_T = 0
x(t), u(t) = \text{arg min } J(x,u)
x(t),u(t)=arg min J(x,u)x(t), u(t) = \text{arg min } J(x,u)

given initial state

dynamic Model

Path Constraints

Desired Final State

Objective

\text{find} \quad \color{red}{x(t)}, \color{red}{u(t)} , \quad \text{for } t \in [0,T]
findx(t),u(t),for t[0,T]\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})
piz(tCi,s)=pi,sz=hterrain(pi,sx,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
fi(t)nterrain(pi,sxy)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})
fi(t)tterrain(pi,sxy)<μfi(t)nterrain(pi,sxy)\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)=a0+a1t+a2t2+a3t3x(t) = a_0 + a_1t + a_2t^2 + a_3t^3
\{
{\{
\color{black}{x_0}
x0\color{black}{x_0}
\color{black}{\dot{x}_0}
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}) ]
ΔT12[3(x0x1)+ΔT1(2x˙0+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}) ]
ΔT13[2(x0x1)+ΔT1(x˙0+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} \}
wj={x0,x˙0,ΔT1,x1,x˙1,ΔT2,x2,x˙2,ΔT3,xT,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)}\)

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}} )
q¨j,ref=Jj+(p¨J˙q˙Jbq¨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}}
p¨=Jq¨+J˙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
P=IJc+Jc\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} )
τ=(PST)+P(Mq¨ref+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]}\):