Alexander W. Winkler
Robotics researcher specialized in motion planning for legged systems.
Optimization-based Motion Planning for Legged Robots
Alexander W. Winkler
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
Source: https://www.youtube.com/watch?v=NX7QNWEGcNIa
Source: https://www.youtube.com/watch?v=arCOVKxGy9E
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}
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)
Task
Generating dynamic motions
Dynamic Model
\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}
Linear Inverted Pendulum
\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
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
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)}
\mathbf{\theta}(t) \in \color{#07d507}{\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}^\color{#07d507}{3}
\quad
\text{(Foot position)}
\color{red}{\mathbf{f}_i}(t) \in \color{#07d507}{\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}
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
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,2}
\Delta T_{R,3}
\Delta T_{L,1}
\Delta T_{L,2}
\Delta T_{L,3}
\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{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{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
- 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}
Given:
4
open-sourced software
Summary
Computation Time 100 ms
1s-horizon, 4-footstep motion for a quadruped
Software
Publications
+ co-authored various others with F. Farshidian, D. Pardo, M. Neunert, ...
\( 1^{\text{st}} \) author
open-sourced
Additional Material:
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)}\)
PhD Defense 2018: "Optimization-based motion planning for legged robots"
By Alexander W. Winkler
PhD Defense 2018: "Optimization-based motion planning for legged robots"
Pdf: https://www.research-collection.ethz.ch/handle/20.500.11850/272432
