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

  • 1,612
Loading comments...

More from Alexander W. Winkler