Mixed-integer convex optimization for planning aggressive motions of legged robots over rough terrain

October 26, 2015

Andrés Valenzuela avalenzu@mit.edu Thesis advisor: Russ Tedrake

My story

2009 - Joined Biomimetic Robotics Lab under Sangbae Kim
2011 - Master's Thesis:
"Stride-level control of quadrupedal runners through optimal scaling of hip-force profiles"
2013 - Switched to Robot Locomotion Group to work on planning and control methods

The DRC Era

2013 - Virtual Robotics Challenge - crawling planning
2013 - DARPA Robotics Challenge (DRC) trials
2015 - DRC Finals

The DRC Era

2013 - Virtual Robotics Challenge - crawling planning
2013 - DARPA Robotics Challenge (DRC) trials
2015 - DRC Finals

The DRC Era

2013 - Virtual Robotics Challenge - crawling planning
2013 - DARPA Robotics Challenge (DRC) trials
2015 - DRC Finals

Big question

How can we plan aggressive motions (like running and jumping) for legged robots in situations where precise footstep placement is critical?

Disaster response

Plan over obstacles not known in advance
Use dynamic motions to reach critical area

BigDog as robotic Balto?

Photo from Boston Dynamics

Cross rough terrain
At high speed
Choosing where to step

Learning Locomotion Program

Careful footstep placement

Aggressive dynamic motions

... but not at the same time

[1]K. Byl, A. Shkolnik, S. Prentice, N. Roy, and R. Tedrake, “Reliable Dynamic Motions for a Stiff Quadruped,” in Experimental Robotics, O. Khatib, V. Kumar, and G. J. Pappas, Eds. Springer Berlin Heidelberg, 2009, pp. 319–328.

Approaches to planning for legged robots

My approach

Push some dynamics into footstep planning problem
Formulate footstep planning + dynamics as a mixed-integer convex program
Use results of that program to inform whole body planner

Outline

Background and related work

Formulation of footsteps and dynamics as an MICP

Using MICP results in whole body motion planning

Approaches to planning for legged robots

Three classes of motion planning for legged robots

No dynamics, no footstep selection Planning with dynamics Footstep planning

Three classes of motion planning for legged robots

No dynamics, no footstep selection Planning with dynamics

Optimization based
Simple model based

Footstep planning

Trajectory optimization

Definition:

A method for solving an optimal control problem (OCP) in which

the OCP is converted into a finite optimization problem that optimization problem is then solved numerically.

Trajectory optimization based planning with dynamics

K. Mombaur [2]

Enforce full dynamics of robot
Optimize with respect to some cost function
E.g. Self-stable bipedal running, Mombaur [2]

[2] K. Mombaur, “Using optimization to create self-stable human-like running,” Robotica, vol. 27, no. 03, pp. 321–330, May 2009.

Trajectory optimization based planning with dynamics

D. Remy et. al [3]

E.g. Optimizing gait efficiency, D. Remy et. al [3]

[3] C. D. Remy, K. Buffinton, and R. Siegwart, “A MATLAB framework for efficient gait creation,” in 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2011, pp. 190–196.

Simple model

A simple model of a robot is a dynamical system, usually with fewer states than the original robot, whose dynamics capture key aspects of the original dynamics.

Example: Point Mass Model

Suppose that all of the robot's mass is concentrated at its center of mass (COM)

Simple-model based planning

Kajita et al. [4]

Zero Moment Point (ZMP) \[ x_{\rm zmp} = x - \frac{z}{g}\ddot{x} \]
Robot is dynamically balanced if ZMP stays in interior of support polygon

[4] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi, and H. Hirukawa, “Biped walking pattern generation by using preview control of zero-moment point,” in IEEE International Conference on Robotics and Automation, 2003. Proceedings. ICRA ’03, 2003, vol. 2, pp. 1620–1626 vol.2.

Another robot that uses ZMP-based planning

Three classes of motion planning for legged robots

No dynamics, no footstep selection Planning with dynamics

Optimization based
Simple model based

Footstep planning

Search based
Optimization based

Search-based footstep placement

Search over action graph of either
- Footsteps
- Body positions
Discrete set of reachable footsteps
No timing or dynamics
e.g. Chestnutt et al. [5, 6], Veranza et al. [7], Winkler et al. [8], Zucker et. al. [8]

[5] J. Chestnutt, K. Nishiwaki, J. Kuffner, and S. Kagami, “An adaptive action model for legged navigation planning,” in 2007 7th IEEE-RAS International Conference on Humanoid Robots, 2007, pp. 196–202.

[6] J. Chestnutt, J. Kuffner, K. Nishiwaki, and S. Kagami, “Planning Biped Navigation Strategies in Complex Environments,” in in IEEE Int. Conf. Humanoid Robots, 2003.

[7] P. Vernaza, M. Likhachev, S. Bhattacharya, S. Chitta, A. Kushleyev, and D. D. Lee, “Search-based planning for a legged robot over rough terrain,” in IEEE International Conference on Robotics and Automation, 2009. ICRA ’09, 2009, pp. 2380–2387.

[8] A. W. Winkler, C. Mastalli, I. Havoutis, M. Focchi, D. G. Caldwell, and C. Semini, “Planning and Execution of Dynamic Whole-Body Locomotion for a Hydraulic Quadruped on Challenging Terrain,” in IEEE International Conference on Robotics and Automation (ICRA), 2015.

[9] M. Zucker, N. Ratliff, M. Stolle, J. Chestnutt, J. A. Bagnell, C. G. Atkeson, and J. Kuffner, “Optimization and learning for rough terrain legged locomotion,” The International Journal of Robotics Research, vol. 30, no. 2, pp. 175–191, Feb. 2011.

Optimization based footstep planning

Example: MIT DRC footstep planning

Generate polygonal "safe regions"
Constrain each footstep to belong to at least one region
Solve as a mixed-integer convex program

[10] R. Deits and R. Tedrake, “Computing Large Convex Regions of Obstacle-Free Space Through Semidefinite Programming,” in Algorithmic Foundations of Robotics XI, H. L. Akin, N. M. Amato, V. Isler, and A. F. van der Stappen, Eds. Springer International Publishing, 2015, pp. 109–124.

[11] R. Deits and R. Tedrake, “Footstep planning on uneven terrain with mixed-integer convex optimization,” in 2014 14th IEEE-RAS International Conference on Humanoid Robots (Humanoids), 2014, pp. 279–286.

All-at-once approaches

Contact implicit trajectory optimization

Posa et al. [12]
Represent contact with complementarity constraints, e.g. \[ \phi \lambda_{z} = 0 \] where $\phi$ is distance to surface, $\lambda_{z}$ is normal force
Solve constrained nonlinear program

[12] M. Posa, C. Cantu, and R. Tedrake, “A direct method for trajectory optimization of rigid bodies through contact,” The International Journal of Robotics Research, vol. 33, no. 1, pp. 69–81, Jan. 2014.

Contact invariant trajectory optimization

Mordatch, Todorov, and Popović [13]
Allow (but penalize) force at a distance
Solve un-constrained nonlinear program

[13] I. Mordatch, E. Todorov, and Z. Popović, “Discovery of Complex Behaviors Through Contact-invariant Optimization,” ACM Trans. Graph., vol. 31, no. 4, pp. 43:1–43:8, Jul. 2012.

Downsides of all-in-one approaches

Very large optimization problems
Non-convex optimization problems
- Ensuring convergence is difficult

Why?

Flavors of mathematical programs

Given a mathematical program (optimization problem): \[ \min_{x \in X} f(x) \]

Flavors

Convexity and mathematical programs

Convex sets

Non-convex set

A mathematical program is convex iff
- the feasible set $X$ is a convex set
- the epigraph of the cost function $f$ is a convex set
Any optimum of a convex program is a global optimum
Non-convex programs may have many local optima

Mixed integer programs

Both discrete (integer or binary) and continuous variables
Non-convex
Non-differentiable
General case is awful
Why would we ever use one of these?

Mixed-integer Convex Programs

Still non-convex, but so much better

Convex continuous relaxations make finding lower bounds easier
Branch and bound effective
Good off-the-shelf solvers available

Outline

Background and related work

Formulation of footsteps and dynamics as an MICP

Using MICP results in whole body motion planning

Bounding quadruped

Free-space regions

Three contact regions (bold lines)

Free-space regions

Three contact regions (bold lines)
Three (overlapping) free-space regions (shaded)

Free-space regions

Three contact regions (bold lines)
Three (overlapping) free-space regions (shaded)

Free-space regions

Three contact regions (bold lines)
Three (overlapping) free-space regions (shaded)

Free-space regions

Three contact regions (bold lines)
Three (overlapping) free-space regions (shaded)
Regions defined by $\mathcal{R}_{j} = \{ \mathbf{x} \; | \; A_{j}x \le b_{j} \}$
Constraint on the $i$-th foot position, $r_{i}$: \[ r_{i} \in \mathop{\bigcup}_{j=1}^{6} \mathcal{R}_{j} \]

Position-force regions

For hind foot, $\mathbf{f}_{1}$ in friction cone, $\mathcal{K}_{1}$
For front foot, $\mathbf{f}_{2} \in \{\mathbf{0}\}$, $\mathcal{K}_{6}$
Let $\mathcal{S}_{j} = \mathcal{R}_{j} \times \mathcal{K}_{j}$
Constraint on position and force: \[ (r_{i}, f_{i}) \in \mathop{\bigcup}_{j=1}^{6} \mathcal{S}_{j} \]

Centroidal dynamics

The centroidal dynamics of a robot define the evolution of its linear momentum, $\mathbf{p}$, and angular momentum, $\mathbf{k}$, about its COM over time \begin{align} \dot{\mathbf{k}} &= \sum \tau_{\mathrm{ext}}\\ \dot{\mathbf{p}} &= \sum \mathbf{f}_{\mathrm{ext}} \end{align}
Depend only on external forces and moments
Low dimensional

Going to the planar case

Evaluate method on planar case first
Preserves the relevant characteristics of the problem
- Planning with dynamics
- Planning footstep placement
Avoids entangling those characteristics with 3D rotation

Planar dynamics

\begin{align} \dot{\mathbf{r}} &= \mathbf{v} & T &= \mathbf{p} \times \mathbf{f} \\ \dot{\theta} &= \omega && = p_{z}f_{x} - p_{x}f_{z}\\ \dot{\mathbf{v}} &= m^{-1}\mathbf{f} + \mathbf{g}\\ \dot{\omega} &= I^{-1}T\\ \end{align}

Mass, moment of inertia: $m$, $I$
Center of mass (COM) position: $\mathbf{r}$
COM velocity: $\mathbf{v}$
Orientation: $\theta$
Angular velocity: $\omega$
Foot position relative to COM: $\mathbf{v}$
Contact force: $\mathbf{f}$
Moment about COM: $T$

MI-Convex relaxations of bilinear terms

$T = pf$

Original non-convex surface

MI-Convex relaxations of bilinear terms

$T = pf$

Linear programming relaxation (McCormick Envelope)
Four linear constraints

MI-Convex relaxations of bilinear terms

$T = pf$

Piecewise McCormick Envelope (PCM)
Tighter relaxation
Adds integer variables

MICP Results

Outline

Background and related work

Formulation of footsteps and dynamics as an MICP

Using MICP results in whole body motion planning

Centroidal-dynamic full-kinematics planning

Developed in collaboration with Hongkai Dai
Wanted a dynamic planner for DRC
Wanted a problem that was smaller than trajectory optimization with Atlas' full dynamics
- 30 joints
- 73 states (joint positions/velocities + floating-base)

Full kinematic model

Use existing kinematic constraints such as ...

Collision avoidance

Grasp specifications

Centroidal-dynamic full-kinematics planning

Main ideas:
- Replace joint-space dynamics with centroidal dynamics
- Keep full kinematic model
- Enforce consistency between momentum trajectories and kinematic trajectories

Centroidal dynamics for Atlas

[16] S. Kuindersma, R. Deits, M. Fallon, A. Valenzuela, H. Dai, F. Permenter, T. Koolen, P. Marion, and R. Tedrake, “Optimization-based locomotion planning, estimation, and control design for the atlas humanoid robot,” Auton Robot, pp. 1–27, Jul. 2015.

Formulation

Decision variables

COM Position, $\mathbf{r}$
COM Velocity, $\dot{\mathbf{r}}$
COM Acceleration, $\ddot{\mathbf{r}}$
Angular momentum, $\mathbf{k}$
Angular momentum rate, $\dot{\mathbf{k}}$
Configuration, $\mathbf{q}$
Velocity, $\mathbf{v}$
Contact forces, $\mathbf{\lambda}$
Time-step durations, $h$

Constraints

Time integration (Backwards Euler)
Centroidal dynamics
Friction cones
Kinematic constraints (task-specific)
Consistency between momenta and joint trajectories

Cost terms

Magnitude of COM acceleration
Magnitude of contact forces
Magnitude of joint velocities
Deviation from nominal joint trajectory

Enforcing consistency of dynamics

Two expressions for angular momentum and COM position

Centroidal dynamics

\begin{align} \dot{\mathbf{k}} &= \sum \tau_{\mathrm{ext}}\\ m\ddot{\mathbf{r}} &= \sum \mathbf{f}_{\mathrm{ext}} \end{align}

Configuration trajectory

\begin{align} \mathbf{k} &= \mathbf{A}_{G}^{\mathbf{k}}(\mathbf{q}) \mathbf{v} \\ \mathbf{r} &= COM(\mathbf{q}) \end{align} where $\mathbf{A}_{G}^{\mathbf{k}}(\mathbf{q})$ is the first three rows of the centroidal momentum matrix [14]

Requiring these to match makes results dynamically feasible for some set of joint torques.

[14] D. E. Orin, A. Goswami, and S.-H. Lee, “Centroidal dynamics of a humanoid robot,” Auton Robot, vol. 35, no. 2–3, pp. 161–176, Oct. 2013.

Results for Atlas

[15] H. Dai, A. Valenzuela, and R. Tedrake, “Whole-body motion planning with centroidal dynamics and full kinematics,” in 2014 14th IEEE-RAS International Conference on Humanoid Robots (Humanoids), 2014, pp. 295–302.

From bipedal running to quadrupedal running

What needs to change?

Robot model
Timing of footfalls
That's it!

Quadrupedal gaits

Why isn't that good enough?

Could we just use that method for this problem?

In previous slide we:

Had to specify footstep timing
Didn't care about footstep placement

From MICP results to whole-body planning

How do we use the results of our footstep + centroidal dynamics planner to formulate the whole body planning problem?

Region assignments become kinematic constraints

\[ \mathbf{r}_{i} \in \mathcal{R}_{j} \ \] becomes \[ A_{j}\,\mathrm{FK}_{i}(\mathbf{q}) \le b_{j} \] where $\mathrm{FK}_{i}(\mathbf{q})$ gives the position of the $i$-th foot for the configuration $\mathbf{q}$.

From MICP results to whole-body planning

Seeding the optimization

Some variables map directly

COM position and velocity
Angular momentum
Contact forces

Configuration seeds from inverse kinematics

At each time find configuration that matches

foot positions
COM position

from MICP result

Results

Extension to full 3D case

Centroidal dynamics full-kinematics planning is already 3D
MICP can choose the position of the plane along its normal
Future work: Add 3D rotational dynamics to MICP stage
- Adds more bilinear terms
- Makes the choice of approximation for bilinear terms more important

Conclusion

How can we plan aggressive motions (like running and jumping) for legged robots in situations where precise footstep placement is critical?

My contributions described today

Mixed-integer convex formulation of footstep planning with centroidal dynamics
Method for generating full configuration trajectories from the results of that MICP

Many thanks to:

Prof. Sangbae Kim and the members of the MIT Biomimetic Robotics Lab
The members of the Robot Locomotion Group
- Special thanks to those who gave feedback on this presentation
Prof. Seth Teller
The MIT DRC Team
Research collaborators (and friends) Robin Deits and Hongkai Dai
My thesis committee
- Prof. Neville Hogan
- Prof. Tomás Lozano-Pérez
- Prof. Alberto Rodriguez
My advisor, Prof. Russ Tedrake
My parents
My children, Sofía, Emilia, and Isaac
My wife, Christina

+AMDG+

References

K. Byl, A. Shkolnik, S. Prentice, N. Roy, and R. Tedrake, “Reliable Dynamic Motions for a Stiff Quadruped,” in Experimental Robotics, O. Khatib, V. Kumar, and G. J. Pappas, Eds. Springer Berlin Heidelberg, 2009, pp. 319–328.

K. Mombaur, “Using optimization to create self-stable human-like running,” Robotica, vol. 27, no. 03, pp. 321–330, May 2009.

C. D. Remy, K. Buffinton, and R. Siegwart, “A MATLAB framework for efficient gait creation,” in 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2011, pp. 190–196.

S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi, and H. Hirukawa, “Biped walking pattern generation by using preview control of zero-moment point,” in IEEE International Conference on Robotics and Automation, 2003. Proceedings. ICRA ’03, 2003, vol. 2, pp. 1620–1626 vol.2.

J. Chestnutt, K. Nishiwaki, J. Kuffner, and S. Kagami, “An adaptive action model for legged navigation planning,” in 2007 7th IEEE-RAS International Conference on Humanoid Robots, 2007, pp. 196–202.

J. Chestnutt, J. Kuffner, K. Nishiwaki, and S. Kagami, “Planning Biped Navigation Strategies in Complex Environments,” in in IEEE Int. Conf. Humanoid Robots, 2003.

P. Vernaza, M. Likhachev, S. Bhattacharya, S. Chitta, A. Kushleyev, and D. D. Lee, “Search-based planning for a legged robot over rough terrain,” in IEEE International Conference on Robotics and Automation, 2009. ICRA ’09, 2009, pp. 2380–2387.

A. W. Winkler, C. Mastalli, I. Havoutis, M. Focchi, D. G. Caldwell, and C. Semini, “Planning and Execution of Dynamic Whole-Body Locomotion for a Hydraulic Quadruped on Challenging Terrain,” in IEEE International Conference on Robotics and Automation (ICRA), 2015.

M. Zucker, N. Ratliff, M. Stolle, J. Chestnutt, J. A. Bagnell, C. G. Atkeson, and J. Kuffner, “Optimization and learning for rough terrain legged locomotion,” The International Journal of Robotics Research, vol. 30, no. 2, pp. 175–191, Feb. 2011.

R. Deits and R. Tedrake, “Computing Large Convex Regions of Obstacle-Free Space Through Semidefinite Programming,” in Algorithmic Foundations of Robotics XI, H. L. Akin, N. M. Amato, V. Isler, and A. F. van der Stappen, Eds. Springer International Publishing, 2015, pp. 109–124.

R. Deits and R. Tedrake, “Footstep planning on uneven terrain with mixed-integer convex optimization,” in 2014 14th IEEE-RAS International Conference on Humanoid Robots (Humanoids), 2014, pp. 279–286.

M. Posa, C. Cantu, and R. Tedrake, “A direct method for trajectory optimization of rigid bodies through contact,” The International Journal of Robotics Research, vol. 33, no. 1, pp. 69–81, Jan. 2014.

I. Mordatch, E. Todorov, and Z. Popović, “Discovery of Complex Behaviors Through Contact-invariant Optimization,” ACM Trans. Graph., vol. 31, no. 4, pp. 43:1–43:8, Jul. 2012.

D. E. Orin, A. Goswami, and S.-H. Lee, “Centroidal dynamics of a humanoid robot,” Auton Robot, vol. 35, no. 2–3, pp. 161–176, Oct. 2013.

H. Dai, A. Valenzuela, and R. Tedrake, “Whole-body motion planning with centroidal dynamics and full kinematics,” in 2014 14th IEEE-RAS International Conference on Humanoid Robots (Humanoids), 2014, pp. 295–302.

S. Kuindersma, R. Deits, M. Fallon, A. Valenzuela, H. Dai, F. Permenter, T. Koolen, P. Marion, and R. Tedrake, “Optimization-based locomotion planning, estimation, and control design for the atlas humanoid robot,” Auton Robot, pp. 1–27, Jul. 2015.

Mixed-integer convex optimization for planning aggressive motions of legged robots over rough terrain October 26, 2015 Andrés Valenzuela avalenzu@mit.edu Thesis advisor: Russ Tedrake

thesis-defense

avalenzu

thesis-defense

1 0 (function() { var po = document.createElement('script'); po.type = 'text/javascript'; po.async = true; po.src = 'https://apis.google.com/js/platform.js'; var s = document.getElementsByTagName('script')[0]; s.parentNode.insertBefore(po, s); })();

thesis-defense

Mixed-integer convex optimization for planning aggressive motions of legged robots over rough terrain

October 26, 2015

Andrés Valenzuela avalenzu@mit.edu Thesis advisor: Russ Tedrake

My story

The DRC Era

The DRC Era

The DRC Era

Big question

Disaster response

BigDog as robotic Balto?

Learning Locomotion Program

Careful footstep placement

Aggressive dynamic motions

Approaches to planning for legged robots

My approach

Outline

Background and related work

Formulation of footsteps and dynamics as an MICP

Using MICP results in whole body motion planning

Approaches to planning for legged robots

Three classes of motion planning for legged robots

Three classes of motion planning for legged robots

Trajectory optimization

Definition:

Trajectory optimization based planning with dynamics

Trajectory optimization based planning with dynamics

Simple model

Example: Point Mass Model

Simple-model based planning

Another robot that uses ZMP-based planning

Three classes of motion planning for legged robots

Search-based footstep placement

Optimization based footstep planning

Example: MIT DRC footstep planning

All-at-once approaches

Contact implicit trajectory optimization

Contact invariant trajectory optimization

Downsides of all-in-one approaches

Flavors of mathematical programs

Flavors

Convexity and mathematical programs

Convex sets

Non-convex set

Mixed integer programs

Mixed-integer Convex Programs

Outline

Background and related work

Formulation of footsteps and dynamics as an MICP

Using MICP results in whole body motion planning

Bounding quadruped

Free-space regions

Free-space regions

Free-space regions

Free-space regions

Free-space regions

Position-force regions

Centroidal dynamics

Going to the planar case

Planar dynamics

MI-Convex relaxations of bilinear terms

MI-Convex relaxations of bilinear terms

MI-Convex relaxations of bilinear terms

MICP Results

Outline

Background and related work

Formulation of footsteps and dynamics as an MICP

Using MICP results in whole body motion planning

Centroidal-dynamic full-kinematics planning

Full kinematic model

Collision avoidance

Grasp specifications

Centroidal-dynamic full-kinematics planning

Centroidal dynamics for Atlas

Formulation

Decision variables

1 0