Alexander W. Winkler
Robotics researcher specialized in motion planning for legged systems.
Optimization-based Motion-Planning for Legged Systems
Alexander Winkler
Jan 22, 2020 ⋅ Facebook Reality Labs
Advantages of legged systems
∙ traverse rubble in earthquake ∙ reach trapped humans ∙ climb stairs ∙...
Agility ...vs rolling
Strength ...vs flying
∙ carry heavy payload ∙ open heavy doors ∙ rescue humans ∙ ...
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
Robot Model ⋅ Goal ⋅ Environment
Desired Motion-Plan
Actuator Commands
force ⋅ torque
Tracking
Controller
wmina(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 (NLP)
x(t),u(t)
\mathbf{x}(t), \mathbf{u}(t)
Task (continuous-time Optimal Control Problem)
Outline: Two different ways to model the physics of legged systems through mathematical equations.
?
⇒ Cubic-Hermite splines
Represent continous motion u(t),u(t) with discrete parameters
w={x0,x˙0,x1,x˙1,x2,x˙2,xT,x˙T}
\color{black}{\mathbf{w}} =
\{
\color{black}{x_0},
\color{black}{\dot{x}_0},
\color{black}{x_1},
\color{black}{\dot{x}_1},
\color{black}{x_2},
\color{black}{\dot{x}_2},
\color{black}{x_T},
\color{black}{\dot{x}_T}
\}
Optimization parameters:
wmina(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}
3rd-order polynomials defined by node values
Dynamic Model
r¨(t)=hg(r(t)−pc(t))
\ddot{\mathbf{r}}(t)
=
\frac{g}{h}(\mathbf{r}(t)- \color{red}{\mathbf{p}_c}(t))
Linear Inverted Pendulum
wmina(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}
pc
\color{red}{\mathbf{p}_c}
.
Why it's difficult to plan a motion {q¨des}t=0..T for legged systems
(Base ∈R6)
Mq¨des+h=JTf
\mathbf{M} \mathbf{\ddot{q}}_{des} + \mathbf{h} = \mathbf{J}^T \color{red}{\mathbf{f}}
f
\mathbf{f}
mg
mg
-
external forces f drive the system, but strong restrictions:
- Forces can only push
- forces only possible in contact
- position of force fixed during stance
- tangential forces must stay inside friction cone
- joint torques τ can't directly drive the 6 DoF base ⇒ underactuated
Unilateral Contact Forces ⇔ CoP inside Support-Area
wmina(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}
p4
\color{blue}{\mathbf{p}_4}
p1,λ1,c1
\color{blue}{\mathbf{p}_1},
\color{red}{\mathbf{\lambda}_1},
\color{#7f6000}{c_1}
p2
\color{blue}{\mathbf{p}_2}
p3
\color{blue}{\mathbf{p}_3}
c=1110
\mathbf{c} =
\begin{bmatrix}
1 \\ 1 \\ 1 \\ 0
\end{bmatrix}
pc
\mathbf{p}_c
1.pc=i=1∑4λipi
1. \quad{\color{red}\mathbf{p}_c} = \sum\limits_{i=1}^{{\color{#7f6000}4}}
\color{red}{\lambda_i}
\color{blue}{\mathbf{p}_i}
2.i=1∑4λi=1
2. \quad \sum\limits_{i=1}^{{\color{#7f6000}4}} {\color{red}\lambda_i} = 1
3.0≤λi
3. \quad 0 \le\color{red}{\lambda_i}
≤ci
\le c_i
foothold change
Simultaneous Foothold and CoM Optimization
wmin0subject tob(w)=0,c(w)≥0
\min\limits_{\mathbf{w}} \mathbf{0} \quad \text{subject to} \quad \mathbf{b}(\mathbf{w}) = \mathbf{0}, \quad
\mathbf{c}(\mathbf{w})\ge \mathbf{0}
Fast Trajectory Optimization for Legged Robots using Vertex-based ZMP Constraints
IEEE Robotic and Automation Letters (RA-L) ⋅ 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 {x(t),u(t)}
fullfills all contraints
wmina(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}
findr(t)∈R3(CoM)
\text{find} \quad
\mathbf{r}(t) \in \mathbb{R}^3
\quad \text{(CoM)}
θ(t)∈R3(Base orientation)
\mathbf{\theta}(t) \in \mathbb{R}^3
\quad
\text{(Base orientation)}
for every foot i∈{1,…,nee}:
\text{for every foot } i \in \{1,\ldots,n_{ee}\}:
pi(t)∈R3(Foot position)
\color{darkblue}{\mathbf{p}_i}(t) \in \mathbb{R}^3
\quad
\text{(Foot position)}
fi(t)∈R3(Foot force)
\color{red}{\mathbf{f}_i}(t) \in \mathbb{R}^3
\quad
\text{(Foot force)}
p1
\mathbf{p}_1
p2
\mathbf{p}_2
p3
\mathbf{p}_3
p4
\mathbf{p}_4
f1
\mathbf{f}_1
f2
\mathbf{f}_2
r,
\mathbf{r},
θ
\theta
Gait and Trajectory Optimization for Legged Systems through Phase-based End-Effector Parameterization
IEEE Robotic and Automation Letters (RA-L) ⋅ 2018
A. W. Winkler, D. Bellicoso, M. Hutter, J. Buchli
Towards integrated motion-planning
mr¨=∑i=14fi−mg
m \, \mathbf{\ddot{r}}
\quad \quad \quad \quad \quad =
\sum_{i=1}^{4}
{\color{red}\mathbf{f}_i}
- m \mathbf{g}
I(θ)ω˙+ω×I(θ)ω=∑i=14fi×(r−pi)
\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
p1
\mathbf{p}_1
p2
\mathbf{p}_2
p3
\mathbf{p}_3
p4
\mathbf{p}_4
f1
\mathbf{f}_1
f2
\mathbf{f}_2
I,m
\mathbf{I}, m
[r¨ω˙]
\begin{bmatrix}
\mathbf{\ddot{r}} \\
\mathbf{\dot{\omega}}
\end{bmatrix}
wmin0subject tob(w)=0,c(w)≥0
\min\limits_{\mathbf{w}} \mathbf{0} \quad \text{subject to} \quad
\mathbf{b}(\mathbf{w}) = \mathbf{0},
\quad
\mathbf{c}(\mathbf{w})\ge \mathbf{0}
Kinematic Model
pi∈Ri(r,θ)
{\color{#1c4587}\mathbf{p}_i} \in {\color{#1c4587}\mathcal{R}_i}(\mathbf{r},\theta)
p1
\mathbf{p}_1
p2
\mathbf{p}_2
r,θ
\mathbf{r}, \mathbf{\theta}
R2
\mathcal{R}_2
R1
\mathcal{R}_1
Range-of-Motion Box ≈ Joint limits
wmin0subject tob(w)=0,c(w)≥0
\min\limits_{\mathbf{w}} \mathbf{0} \quad \text{subject to} \quad
\mathbf{b}(\mathbf{w}) = \mathbf{0},
\quad
\mathbf{c}(\mathbf{w})\ge \mathbf{0}
Gait Optimization
R | 0 | R | 2 | R | 2
.... gait defined by continuous phase-durations ΔTi
without Integer Programming
swing
stance
individual foot always alternates between and
ΔTR,1
\Delta T_{R,1}
ΔTR,2
\Delta T_{R,2}
ΔTR,3
\Delta T_{R,3}
ΔTL,1
\Delta T_{L,1}
ΔTL,2
\Delta T_{L,2}
ΔTL,3
\Delta T_{L,3}
ΔTL,4
\Delta T_{L,4}
R | 2 | L | R | 2
Sequence:
Terrain constraints
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})
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
∣fi(t)⋅t(pi,sx,pi,sy)∣<μ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
< \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
t∈C
t \in \mathcal{C}
wmin0subject tob(w)=0,c(w)≥0
\min\limits_{\mathbf{w}} \mathbf{0} \quad \text{subject to} \quad
\mathbf{b}(\mathbf{w}) = \mathbf{0},
\quad
\mathbf{c}(\mathbf{w})\ge \mathbf{0}
Gait and Trajectory Optimization for Legged Systems through Phase-based End-Effector Parameterization
IEEE Robotic and Automation Letters (RA-L) ⋅ 2018
A. W. Winkler, D. Bellicoso, M. Hutter, J. Buchli
Character Simulation in Game Engines
using Unreal Engine 4, Blender, Blueprints, ...
Gait and Trajectory Optimization for Legged Systems through Phase-based End-Effector Parameterization
IEEE Robotic and Automation Letters (RA-L) ⋅ 2018
A. W. Winkler, D. Bellicoso, M. Hutter, J. Buchli
Fast Trajectory Optimization for Legged Robots using Vertex-based ZMP Constraints
IEEE Robotic and Automation Letters (RA-L) ⋅ 2017
A. W. Winkler, F. Farshidian, D. Pardo, M. Neunert, J. Buchli
F. Farshidian
D. Pardo
M. Neunert
J. Buchli
M. Hutter
D. Bellicoso
Additional Material:
4
open-sourced software
Summary
Computation Time 100 ms
1s-horizon, 4-footstep motion for a quadruped
Know if polynomial belongs to swing or stance phase
-
Foot pi(t) cannot move while
fi(t∈/Ci)=0
\color{red}{\mathbf{f}_i}
(t\notin\mathcal{C}_i)
=
\mathbf{0}
p˙i(t∈C)=0
\color{blue}{\dot{\mathbf{p}}_i}
(t\in \mathcal{C})
=
\mathbf{0}
Physical Restrictions
- Forces fi(t) cannot exist while
standing
swinging
Recreating Dust 2
using Unreal Engine 4, Blender, ...
Unilateral Contact Forces ⇔ CoP inside Support-Area
pc
\color{red}{\mathbf{p}_c}
p1
\color{blue}{\mathbf{p}_{1}}
p8,λ8,c8
\color{blue}{\mathbf{p}_{8}},
\color{red}{\mathbf{\lambda}_8},
\color{#7f6000}{c_8}
p2
\color{blue}{\mathbf{p}_{2}}
.
.
.
.
.
.
.
.
c=11111111
\mathbf{c} =
\begin{bmatrix}
1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1
\end{bmatrix}
1.pc=i=1∑∣c∣λipi
1. \quad{\color{red}\mathbf{p}_c} = \sum\limits_{i=1}^{{\color{#7f6000}|\mathbf{c}|}}
\color{red}{\lambda_i}
\color{blue}{\mathbf{p}_i}
2.i=1∑∣c∣λi=1
2. \quad \sum\limits_{i=1}^{{\color{#7f6000}|\mathbf{c}|}} {\color{red}\lambda_i} = 1
3.0≤λi
3. \quad 0 \le\color{red}{\lambda_i}
≤ci
\le c_i
pc
\mathbf{p}_c
p2
\color{blue}{\mathbf{p}_2}
p1,λ1,c1
\color{blue}{\mathbf{p}_1},
\color{red}{\mathbf{\lambda}_1},
\color{#7f6000}{c_1}
c=1110
\mathbf{c} =
\begin{bmatrix}
1 \\ 1 \\ 1 \\ 0
\end{bmatrix}
p4
\color{blue}{\mathbf{p}_4}
p3
\color{blue}{\mathbf{p}_3}
wmina(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}
mr¨=∑i=14fi−mg
m \ddot{\mathbf{r}}
=
\sum_{i=1}^{4}
\color{red}{\mathbf{f}_i}
- m \mathbf{g}
I(θ)ω˙+ω×I(θ)ω=∑i=14fi×(r−pi)
\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 ⇒ 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.
A(q)q¨+A˙(q,q˙)q˙=[∑i=14fi−mg∑i=1nifi×(r(q)−pi(q))]
\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 |
qb,qj
\mathbf{q}_b, \mathbf{q}_j
qb,qj
\mathbf{q}_b, \mathbf{q}_j
r,θ,
\mathbf{r}, \mathbf{\theta},
rx,ry
r_x, r_y
τ,fi
\tau, \mathbf{f}_i
fi
\mathbf{f}_i
fi
\mathbf{f}_i
pc
\mathbf{p}_c
pi
\mathbf{p}_i
x
\mathbf{x}
x˙=F(x(t),u(t))
\mathbf{\dot{x}} = \mathbf{F}(\mathbf{x}(t), \color{red}{\mathbf{u}(t)})
u
\mathbf{u}
x(t)=a0+a1t+a2t2+a3t3
x(t) = a_0 + a_1t + a_2t^2 + a_3t^3
{
\{
x0
\color{black}{x_0}
x˙0
\color{black}{\dot{x}_0}
{
\{
{
\{
{
\{
−ΔT1−2[3(x0−x1)+Δ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}) ]
ΔT1−3[2(x0−x1)+Δ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}) ]
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 f{x,y,z}(t),p{x,y,z}(t)
pc
\mathbf{p}_c
pc
\mathbf{p}_c
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
Constraint: Unilateral Contact Forces ⇔ CoP inside Support-Area
Difficult for single point-contacts or lines
p1
\color{blue}{\mathbf{p}_1}
p2
\color{blue}{\mathbf{p}_2}
p3
\color{blue}{\mathbf{p}_3}
Ordering of contact points
1.pc=i=1∑4λipi
1. \quad{\color{red}\mathbf{p}_c} = \sum\limits_{i=1}^4
\color{red}{\lambda_i}
\color{blue}{\mathbf{p}_i}
2.i=1∑4λi=1
2. \quad \sum\limits_{i=1}^{4} {\color{red}\lambda_i} = 1
3.0≤λi
3. \quad 0 \le\color{red}{\lambda_i}
c=1110
\mathbf{c} =
\begin{bmatrix}
1 \\ 1 \\ 1 \\ 0
\end{bmatrix}
c=0110
\mathbf{c} =
\begin{bmatrix}
0 \\ 1 \\ 1 \\ 0
\end{bmatrix}
≤ci
\le c_i
pc
\mathbf{p}_c
pc
\mathbf{p}_c
Vertex-Based Zmp-Constraint Formulation
p1,λ1,c1
\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) ⋅ 2017
A. W. Winkler, F. Farshidian, D. Pardo, M. Neunert, J. Buchli
Talk about real time c++ control, since this is also require to turn camera images into 3D meshes instantly
More emphasis on kinematic Planning at facebook
Human Gait Picture from https://www.protokinetics.com/2018/11/28/understanding-phases-of-the-gait-cycle/
Optimization-based Motion-Planning for Legged Systems Alexander Winkler Jan 22, 2020 ⋅ Facebook Reality Labs
"Facebook Presentation: Optimization-based motion planning for legged systems"
By Alexander W. Winkler
"Facebook Presentation: Optimization-based motion planning for legged systems"
Presentation for Facebook Interview
- 1,174
