Alexander W. Winkler
Robotics researcher specialized in motion planning for legged systems.
Optimization-based Motion-Planning for Legged Systems
Alexander Winkler
Jan 22, 2020 \( \cdot \) Facebook Reality Labs
Advantages of legged systems
\( \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
Source: https://www.youtube.com/watch?v=NX7QNWEGcNIa
Source: https://www.youtube.com/watch?v=arCOVKxGy9E
Robot Model \( \cdot \) Goal \( \cdot \) Environment
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 (NLP)
\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.
?
\(\Rightarrow\) Cubic-Hermite splines
Represent continous motion \(\mathbf{u}(t), {\color{red}\mathbf{u}(t)}\) with discrete parameters
\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:
\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
\ddot{\mathbf{r}}(t)
=
\frac{g}{h}(\mathbf{r}(t)- \color{red}{\mathbf{p}_c}(t))
Linear Inverted Pendulum
\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}
\color{red}{\mathbf{p}_c}
.
Why it's difficult to plan a motion \(\{\mathbf{\ddot{q}}_{des}\}_{t=0..T}\) for legged systems
(Base \( \in \mathbb{R}^6\))
\mathbf{M} \mathbf{\ddot{q}}_{des} + \mathbf{h} = \mathbf{J}^T \color{red}{\mathbf{f}}
\mathbf{f}
mg
-
external forces \(\mathbf{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 \(\tau\) can't directly drive the 6 DoF base \( \Rightarrow \) underactuated
Unilateral Contact Forces \(\Leftrightarrow\) CoP inside Support-Area
\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}
\color{blue}{\mathbf{p}_4}
\color{blue}{\mathbf{p}_1},
\color{red}{\mathbf{\lambda}_1},
\color{#7f6000}{c_1}
\color{blue}{\mathbf{p}_2}
\color{blue}{\mathbf{p}_3}
\mathbf{c} =
\begin{bmatrix}
1 \\ 1 \\ 1 \\ 0
\end{bmatrix}
\mathbf{p}_c
1. \quad{\color{red}\mathbf{p}_c} = \sum\limits_{i=1}^{{\color{#7f6000}4}}
\color{red}{\lambda_i}
\color{blue}{\mathbf{p}_i}
2. \quad \sum\limits_{i=1}^{{\color{#7f6000}4}} {\color{red}\lambda_i} = 1
3. \quad 0 \le\color{red}{\lambda_i}
\le c_i
foothold change
Simultaneous Foothold and CoM Optimization
\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) \( \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
\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}
\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
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})
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}
\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
{\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
\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 \(\Delta T_i\)
without Integer Programming
swing
stance
individual foot always alternates between and
\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}
R | 2 | L | R | 2
Sequence:
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
t \in \mathcal{C}
\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) \( \cdot \) 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) \( \cdot \) 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) \( \cdot \) 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 \( \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
Recreating Dust 2
using Unreal Engine 4, Blender, ...
Unilateral Contact Forces \(\Leftrightarrow\) CoP inside Support-Area
\color{red}{\mathbf{p}_c}
\color{blue}{\mathbf{p}_{1}}
\color{blue}{\mathbf{p}_{8}},
\color{red}{\mathbf{\lambda}_8},
\color{#7f6000}{c_8}
\color{blue}{\mathbf{p}_{2}}
.
.
.
.
.
.
.
.
\mathbf{c} =
\begin{bmatrix}
1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ 1
\end{bmatrix}
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. \quad \sum\limits_{i=1}^{{\color{#7f6000}|\mathbf{c}|}} {\color{red}\lambda_i} = 1
3. \quad 0 \le\color{red}{\lambda_i}
\le c_i
\mathbf{p}_c
\color{blue}{\mathbf{p}_2}
\color{blue}{\mathbf{p}_1},
\color{red}{\mathbf{\lambda}_1},
\color{#7f6000}{c_1}
\mathbf{c} =
\begin{bmatrix}
1 \\ 1 \\ 1 \\ 0
\end{bmatrix}
\color{blue}{\mathbf{p}_4}
\color{blue}{\mathbf{p}_3}
\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}
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})
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}
| (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{r}, \mathbf{\theta},
r_x, r_y
\tau, \mathbf{f}_i
\mathbf{f}_i
\mathbf{f}_i
\mathbf{p}_c
\mathbf{p}_i
\mathbf{x}
\mathbf{\dot{x}} = \mathbf{F}(\mathbf{x}(t), \color{red}{\mathbf{u}(t)})
\mathbf{u}
x(t) = a_0 + a_1t + a_2t^2 + a_3t^3
\{
\color{black}{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}^{-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}
\}
Cubic-Hermite Spline for \(\color{red}{f_{\{x,y,z\}}(t)}, \color{blue}{p_{\{x,y,z\}}(t)}\)
\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
Constraint: Unilateral Contact Forces \(\Leftrightarrow\) CoP inside Support-Area
Difficult for single point-contacts or lines
\color{blue}{\mathbf{p}_1}
\color{blue}{\mathbf{p}_2}
\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}
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}
0 \\ 1 \\ 1 \\ 0
\end{bmatrix}
\le c_i
\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}
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
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/
"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
