Real-time Model Predictive Control for Versatile Dynamic Motions inQuadrupedal Robots

Yanran Ding, Abhishek Pandala, and Hae-Won Park

Abstract— This paper presents a new Model PredictiveControl (MPC) framework for controlling various dynamicmovements of a quadrupedal robot. System dynamics arerepresented by linearizing single rigid body dynamics in three-dimensional (3D) space. Our formulation linearizes rotation ma-trices without resorting to parameterizations like Euler anglesand quaternions, avoiding issues of singularity and unwindingphenomenon, respectively. With a carefully chosen configura-tion error function, the MPC control law is transcribed into aQuadratic Program (QP) which can be solved efficiently in real-time. Our formulation can stabilize a wide range of periodicquadrupedal gaits and acrobatic maneuvers. We show varioussimulation as well as experimental results to validate our controlstrategy. Experiments prove the application of this frameworkwith a custom QP solver could reach execution rates of 160 Hzon embedded platforms.

I. INTRODUCTION

Legged animals have shown their versatile mobility totraverse challenging terrains via a variety of well-coordinateddynamic motions. This remarkable mobility of legged ani-mals inspired the development of many legged robots and as-sociated research works seeking for dynamic legged locomo-tion in robots. However, designing and controlling a leggedrobot to achieve similar mobility to that of legged animalsremains a difficult problem. Enabling this versatile capabilityof animals in legged robot systems requires the controldesign to make good use of its inherent dynamics whiledealing with constraints due to hardware limitations as wellas interactions with the environment. In the field of leggedrobots, Model Predictive Control (MPC) recently becamea widespread control method due to recent advancementsin computing hardware and optimization algorithms, whichenabled real-time execution of MPC controller in embeddedsystems. Based on a model prediction, the MPC frame-work easily incorporates system dynamics and constraintsby transcribing the control law as a constrained optimizationproblem. Recent applications of MPC on humanoids [1], [2]and quadrupeds [3] have shown the capability of MPC inplanning and controlling complex dynamic motions whileembracing system dynamics and constraints arising fromfriction and motor saturation.

Despite the widespread adaptation of MPC, its directimplementation on a high degree-of-freedom (DoF) systemrequires heavy computational resources, hindering the ap-plication on embedded platforms. To tackle this problem,simpler models or templates [4] that capture the dominant

Yanran Ding, Abhishek Pandala and Hae-Won Park are with the depart-ment of Mechanical Science and Engineering Department, University ofIllinois at Urbana-Champaign, IL, 61801, USA. {yding35, pandala2,haewon}@illinois.edu.

system dynamics were used to predict the behavior of thesystem. Previous MPC schemes [1], [5] worked on simplifieddynamics models like Linear Inverted Pendulum [6] (LIPM)which enabled online execution. The planar rigid bodymodel is used in [7] to plan online jumping trajectoriesfor MIT Cheetah 2 with different obstacle heights. Thespring-mass model is used in [8] to achieve jumping andlanding. Centroidal dynamics [9] model links the linear andangular momentum of the robot with the external wrench.This model is used in [10], [11] to capture the majordynamic effect of the complex full body dynamics modelof the humanoid robot Atlas. Recent work such as [12]uses centroidal dynamics for the policy regularized nonlinearMPC (PR-MPC), which takes simple heuristics as referencetrajectories for state and control to condition the optimizationformulation.

Regarding the use of a simple dynamic model for MPCframework, there has been an increasing number of workutilizing a dynamic model based on a single rigid bodyin three-dimensional (3D) space. In this model, robot’sbody and legs are lumped into a single rigid body. Groundreaction forces (GRF) are applied as inputs to control theposition and orientation of the body. Even in its simpleform, this model efficiently captures the effect of net externalwrench on the evolution of CoM motion and orientation.Utilizing this model, [13] could simultaneously optimizefor gait and trajectory through phase-based parametrization;[14] achieved various quadrupedal gaits in experiment; [15]obtained high-speed bounding with MIT Cheetah 2.

Although the single rigid body model originally representsthe orientation of the robot in 3D space with a rotationmatrix, most of the works using this model handled 3Dorientation by replacing the rotation matrix with its local co-ordinates such as Euler angles [13]. However, representationof dynamics using Euler angle is not properly invariant underthe action of rigid transformations, so a control design basedon such models will provide inconsistent behaviors acrossdifferent operating configurations. Moreover, Euler anglessuffer from singularities [16] that occur in certain config-urations. Quaternions representation could cause unwindingissue [17] arising from ambiguities due to multiple spanningof configuration space. In previous works, these problemsarising from using local coordinates were not a critical issueas they assume small deviations in roll and pitch angles [12]or only deal with planar motions [15] to represent robot’sorientation in a simpler form.

This work presents a novel MPC formulation for control-ling legged robots in 3D environment without use of local

2019 International Conference on Robotics and Automation (ICRA)Palais des congres de Montreal, Montreal, Canada, May 20-24, 2019

978-1-5386-6027-0/19/$31.00 ©2019 IEEE 8484

(1) (2) (3)

(4) (5) (6)

Fig. 1: Sequential snapshots of the robot in a boundingexperiment

coordinates to represent rotation matrices. This MPC schemeis formulated into QP which can be solved efficiently inan embedded computer satisfying real-time constraints. Toderive this formulation, a linearization technique in [18],[19] which directly works on equations of motion withrotation matrix are applied in this work. We also adopted aconfiguration error function on SO(3) from [20] to obtain acost function in a quadratic form. This application of the lin-earization technique and use of a configuration error functionon SO(3) leads to the a singularity-free MPC formulationwith consistent performance even when executing motionsthat involve complex 3D rotations such as acrobatic motionsin gymnastics.

The paper is organized as follows: Section II presentsthe template dynamic model and introduces the linearizationscheme together with the modification on the cost functionthat enables the formulation in QP; Section III shows thesimulation and experimental results; Section IV provides theconcluding remark with an outlook for future work.

II. TECHNICAL APPROACH

Our goal is to formulate the quadruped dynamic locomo-tion problem as a MPC scheme that is real-time executableon a mobile embedded computer and uses a global param-eterization of orientation with rotation matrix. To meet thereal-time constraint, single rigid body model is adopted tocharacterize the main dynamics of the robot, which is a closeapproximation of the robot model since the mass of all legscombined is less than 10% of the total mass. In addition, anMPC formulation is transcribed into a real-time conceivableQP, which directly works on representation of the orientationwith rotation matrices.

A. 3D Single Rigid Body Dynamics

In this work, the dynamic model of the robot is approxi-mated as a single rigid body with fixed moment of inertia.Then, the state of the robot is,

x := [p p R Bω] (1)

where p ∈ R3 is the position of the body Center of Mass(CoM); p is the CoM velocity; R ∈ SO(3) = {R ∈R3×3|RTR = I, detR = 1} is the rotation matrix of thebody frame {B} expressed in the inertial frame {S}; Bωindicates the angular velocity vector expressed in the body

𝑥𝑆𝑦𝑆

𝑧𝑆

{𝑆}𝒖2

𝒖3

𝑥𝐵

𝑧𝐵

𝑦𝐵

{𝐵}𝒑𝐶𝑜𝑀

𝒓2 𝒓3

Fig. 2: Illustration of coordinate system and 3D rigid-bodymodel. {S} is the inertia frame while {B} is the bodyattached frame. ri is the position vector from center of massto each foot in {S}, while ui is the ground reaction forceof ith contact foot in {S}

frame {B}. Variables without superscript on the upper-leftcorner are assumed to be expressed in the inertial frame.

The input to the system is the external wrench, whichis created by the ground reaction force (GRF) ui ∈ R3 atcontact foot locations pfi ∈ R3. The foot positions pfi relativeto CoM are denoted as ri = pfi − p. Therefore, the netexternal wrench exerted on the body is:[

fτ

]=

4∑i=1

[Iri

]ui (2)

where the hat map (·) : R3 → so(3) is defined as xy =x×y, ∀x, y ∈ R3, and so(3) is the space of skew-symmetricmatrices; I is the 3-by-3 identity matrix. The full dynamicsof the rigid body can be written as

x = f(x,u) =

pp

RBω

=

p

1M f − agR · Bω

BI−1(RT τ − ωBIω)

(3)

where x ∈ Rn represents the state and Rm 3 u =[uT1 ,u

T2 ,u

T3 ,u

T4 ]T the control; M is the mass of the rigid

body; ag ∈ R3 is the gravitational acceleration; BI ∈ R3×3

is the fixed moment of inertia tensor in the body frame.

B. Nonlinear MPC Formulation

The equation of motion in (3) includes nonlinear dynamicsin R and Bω, thereby leading to a nonlinear MPC for-mulation. Utilizing a direct collocation technique [21], weformulated the nonlinear MPC as an optimization problem.The rigid-body dynamics is discretized and imposed on thesequences of predicted states {xk} and control (GRF) {uk}as equality constraints,

minimizeN−1∑k=1

`(xk,uk) + `T (xN )

subject to xk+1 = g(xk,uk),x1 = xop

xk ∈ X, k = 1, 2, · · · , Nuk ∈ U, k = 1, 2, · · · , N − 1

(4)

where ` : Rn × Rm → R is the stage cost function; `T :Rn → R is the terminal cost function; N is the predictionhorizon; X,U are the feasible sets for the state and control.

8485

The stage cost function penalizes deviation of the predictedstates and control inputs from the corresponding referencetrajectories of the states and control inputs. The definition ofthe stage cost is as follows

`(xk,uk) = eTukRueuk

+ eTpkQpepk + eTpkQpepk+

eTωkQωeωk

+ eTRkQReRk

(5)

where Ru,Qp,Qp,Qω,QR are positive definite weightingmatrices; euk

, epk , epk are the error terms for deviationsfrom the corresponding desired trajectories udk,p

dk, p

dk, which

could be constructed from simple heuristics. The error termsof angular velocity and rotation matrix are given by [20] as

eωk= ωk −RT

kRd,kωd,k (6)

eRk= log(RT

d,k ·Rk)∨ (7)

where log(·) : SO(3) → so(3) is the logarithm map [22]of rotation matrices; the Vee map (·)∨ : so(3) → R3 is theinverse of the hat map. The terminal cost function is similarlydefined.

While the translational part in (3) is linear, the rotationalpart evolves nonlinearly following the discrete equation,

ωk+1 = ωk + BI−1(RTk τk − ωkBIωk)∆t

Rk+1 = Rkexp(ωk∆t)(8)

where ∆t is the time step; exp(·) : so(3) → SO(3) is thematrix exponential map that makes sure that Rk stays on theSO(3) manifold.

Although, in principle, nonlinear MPC (NMPC) can besolved to obtain control input, the nonlinearities complicatesthe solving process for the presence of local minima. More-over, the severe restrictions on computational time hinderthe application of NMPC on hardware. To obtain MPCformulation that can be solved with convex optimization,we linearize nonlinear dynamic constraints in (8) using avariation-based linearization scheme.

C. Variation-based Linearization

Although linearization around the reference trajectorygives provable local controllability [26] [18], the obtainedlinear dynamics differs from the robot’s current dynamicswhen the operating point is not close to the referencetrajectory. Hence, linearization is performed around the cur-rent state and control (operating point), which is known assuccessive linearizaiton [23] [24] [25]. To meet the real-time constraint, we employed a element-wise, forward-Eulerlinearization scheme for the rotation matrix. Linearization isperformed by taking variation δ(·) with respect to the operat-ing point, which is treated here as a linear approximation ofthe distance of two points on a manifold [27]. Assuming thatthe predicted variables are close to the operating point, whichcould be guaranteed by having small time step, the variationof the rotation matrix on so(3) will be approximated usingthe derivative of the error function on SO(3) from [20], [28],

δRk ≈1

2(RT

opRk −RTkRop) (9)

where the subscripts (·)op and (·)k indicate variables at theoperating point and the kth prediction step, respectively. Therotation matrix at the kth prediction step is also approximatedusing the first-order Taylor expansion of matrix exponentialmap,

Rk ≈ Ropexp(δRk) ≈ Rop(I + δRk). (10)

Similarly, the variation of angular velocity δωk is

δωk = ωk −RTkRopωop (11)

Using equations (10), (11), the dynamics of rotation matrixRk = Rkωk is linearized around the operating point Rop

asRk = Ropωop +RopωopδRk +Ropδωk (12)

where terms containing higher order variation are eliminated.The dynamics of angular velocity ωk is linearized around

the operating point as

BIωk =RTopτop + δRT

k τop +RTopδτk+

− ωopBIωop − ˆδωkBIωop − ωopBIδωk,

(13)

in which δτk is the variation of torque,

δτk = (4∑i=1

ui,op)δpk + (4∑i=1

ri,op · δui,k), (14)

where uop is GRF applied at the current step, δu is thevariation of GRF from uop.

D. Vectorization

Even though the dynamics of rotation matrix R andangular velocity ω are linearized, there are matrix variablesin (12) and (13) which are difficult to handle in conventionalQP solvers. To tackle the problem, this section proposesa vectorization technique which uses Kronecker product[29] to transform matrix-matrix product into matrix-vectorproduct. Each term of (12) is vectorized as

vec(RopδRkωop) = (ωTop ⊗Rop)Lvec(Rk)

vec(RopδRTk ωop) = (ωTop ⊗Rop)PLvec(Rk)

vec(RopωopδRk) = (I⊗Ropωop)Lvec(Rk)

vec(Ropδωk) = −(I⊗Rop)N(I⊗ (Ropωop)T )vec(Rk)+

(I⊗Rop)Nωk

(15)

where vec(·) is the vectorization operator for a matrix; ⊗is the Kronecker tensor operator; N is a constant matrixsuch that Nω = vec(ω) for any vector ω ∈ R3; P is aconstant permutation matrix such that P ·vec(A) = vec(AT )for an arbitrary matrix A ∈ R3×3; L is used to simplifythe expression L := 1

2 [(I ⊗RTop) − (RT

op ⊗ I)P ]. The finalexpression for vec(Rk) is

vec(Rk) = CRvec(Rk) +Cωωk + c0 (16)

where c0 = vec(Ropωop), Cω = (I ⊗Rop)N , and CR isthe sum of terms that precede vec(R) in (15).

8486

The final expression of ωk is obtained by plugging (14)in (13).

ωk = Dp(pk−pop)+DRvec(Rk)+Dωωk +Duδuk +d0(17)

where the coefficients are defined below

d0 =BI−1vec(RT

opτop − ωopBIωop)

Du =BI−1RTop(∑

rop)

Dω =BI−1(BIωop − ωopBI)

DR =BI−1[(I⊗ τTop)− BIDω(I⊗ (Ropωop)T )]

Dp =BI−1RTop(∑

uop)

(18)

Letting xk := [pTk pTk vec(Rk)T BωTk ]T ∈ Rn be thestate vector, and uk := [δuT1,k δuT2,k δuT3,k δuT4,k]T ∈Rm be the control vector, the discrete dynamics could beexpressed in the state-space form

xk+1 = Axk +Buk + d (19)

where A ∈ Rn×n,B ∈ Rn×m, and d ∈ Rn are constantmatrices which could be constructed from (16), (17).

E. Cost Function and Inequality Constraints

In the expression of the stage cost function (5), allthe terms are in a quadratic form of the state vector xkand control vector uk except the cost on orientation erroreTRk

QReRkdue to the expression of eRk

in (7) as the loga-rithm map of the rotation matrix Rk. To closely approximatethe error term, here we adopt the configuration error functionof Proposition 11.31 in [20],

Ψ(RTdRk) =

1

2tr[Gp(I−RT

dRk)] (20)

where Gp := tr(Kp)I−Kp; tr(·) gives the trace of a matrix;Kp is a symmetric positive definite matrix which will beselected to incorporate the weight matrix QR. Because thisconfiguration error function is linear with respect to Rk, thestage cost function is quadratic with the introduction of thisapproximation, penalizing orientation error.

This configuration error function Ψ(X) for X ∈ SO(3) issmooth with Ψ(I) = 0. This function is also locally positivedefinite about I within the region where the rotation angleof X from I is less than 180◦ which almost covers SO(3).

In order to obtain Kp, we use eR = log(RTdRk)∨ and

Rodirigues’ Formula [20] to rewrite the configuration errorfunction (20) as,

Ψ{exp[log(RTdRk)∨]} =Ψ{exp(eR)}

=1− cos ||eR||

2||eR||2eTR[tr(Kp)I +Kp]eR

(21)

where the coefficient term 1−cos ||eR||2||eR||2 approaches 1

4 as||eR|| → 0. Comparing (21) and the term eTRk

QReRkin

(5), the expression for Kp could be calculated from

1

4[tr(Kp) · I +Kp] = QR (22)

given weighting matrix QR.

TABLE I: System Parameters of the Robot

Parameter Value UnitM 5.5 kgIxx 0.026 kg m2

Iyy 0.112 kg m2

Izz 0.075 kg m2

Body length 0.3 mBody width 0.2 mlink length 0.14 m

Friction cone is approximated as a linearized frictionpyramid, and the following inequality constraint is imposed∥∥utop + δut

∥∥ ≤ µ(unop + δun) (23)

where µ is the coefficient of friction; superscript (·)t indi-cates tangential force, and (·)n normal force.

The following box constraints on the GRF are added toclamp the force within desired ranges.

ui,min ≤ ui,op + δui,k ≤ ui,max (24)

This clamping is used to set the value of GRF to zero for legsin swing phase by clamping ui,min and ui,max to zero.

III. RESULTS

This section evaluates the MPC controller through simu-lation and hardware experiments.

A. Simulation

The simulation experiments are setup in the following way.In each sample time, control input is obtained by solvingthe QP transcribed by MPC using quadprog in MATLAB,and then applied to the continuous model (3) through ode45to simulate the evolution of nonlinear dynamics. Table Iprovides the system parameters used in the simulation.

1) Walking Trot: The walking trot simulation serves asa baseline test for verifying the tracking capability of theproposed MPC formulation. The robot is commanded aforward walking speed of 0.3 m/s. Fig. 3 shows the positionand orientation deviation from the reference point. The gaitpattern is time-based, with the stance time set as 0.4 sec andswing time 0.2 sec.

2) Bounding: The bounding simulation is presented hereto verify that the proposed MPC formulation is capable ofleveraging full body dynamics. To achieve bounding motion,a dynamically feasible reference trajectory that preservesperiodicity is designed from the impulse-scaling analysis[15], where the gain on the pitch motion is set as αθ = 15. Inthis experiment, the robot is commanded to follow a forwardtrajectory starting from the static nominal pose and accelerateto 1.0 m/s. Fig. 4 (a) shows that the pitch motion convergesto a periodic orbit; Fig. 4 (b) demonstrates that the robot isable to closely track the forward reference trajectory. Thegait pattern is also time-based with a stance time of 0.1 secand swing time of 0.2 sec.

8487

0 0.5 1 1.5 2 2.5 3 3.5 4-0.02

-0.01

0

0.01

0.02

po

siti

on

de

via

tio

n [

m]

(a)

pxpypz

0 0.5 1 1.5 2 2.5 3 3.5 4

Time [s]

-2

0

2

4

ori

en

tati

on

de

via

tio

n [

rad

] 10 -3

(b)

roll

pitch

yaw

Fig. 3: Simulation results of walking trot (a) CoM positiondeviation (b) orientation deviation. The reference is 0 forboth position and orientation.

-0.3 -0.2 -0.1 0 0.1θ [rad]

-4

-2

0

2

4

θ[ra

d/s]

0 1 2 3 4t[s]

0

1

2

3

p x[m

]

px,dpx

Fig. 4: Simulation results of bounding are generated withthe robot start from static nominal pose and track a forwardaccelerating trajectory (a) pitch angular velocity and pitchangle converge to a periodic orbit (b) position trackingperformance of the controller

3) Aperiodic Complex Dynamic Maneuver: In additionto simulation of periodic gait motions, we demonstrate theperformance of our MPC to control a complex acrobatic 3Dmaneuver. Fig. 5 shows the reference trajectory of back flipwith twist that are tracked using the MPC. Initially, the robotstands on an inclined surface with a slope of 45◦. Then, therobot initiates jump with all the four legs in contact with thesurface until 0.1 sec. When time reaches 0.1 sec, the frontpair of legs are airborne while the hind pair of legs are still incontact with the ground applying forces to the surface. At 0.2sec, the hind pair of legs also lift off from the surface. Withthe selection of a feed-forward trajectory of ground reactionforces obtained from separate trajectory optimization, thisjumping creates a back flip with twist, which makes therobot land at 0.5 sec after an airborne phase in an uprightconfiguration that faces opposite to the inclined surface. Fig.5(c)(d) show a comparison between open-loop uncontrolledback flip with twist and closed-loop back flip with twistcontrolled by our MPC controller when the slope of surfaceis perturbed to 53.6◦ from 45◦. It can be observed that up to0.2 sec during the stance phase while the robot has controlauthority, orientation errors are better regulated in the MPCcontroller compared to the open-loop controller, placing thelanding configuration close to the desired configuration.

(a) (b)

(c) (d)𝑒𝑥𝑒𝑦𝑒𝑧

𝑒𝑥𝑒𝑦𝑒𝑧

Fig. 5: Simulation results of a complex aperiodic 3D ma-neuver where the robot performs a twist jump off a slopedsurface. The slope of the surface is deviated from the nominal45◦ to 53.6◦. (a) reference (blue) and MPC controlled (red)trajectories, (b) reference (blue) and open-loop (red) trajec-tories. (c) orientation deviation of MPC controlled trajectoryfrom the reference, (d) orientation deviation of open-looptrajectory from the reference. The orientation deviation iscalculated using log map eR = log(RT

dR)∨. (i) is whenfour legs are all in contact with the surface; (ii) is whenonly hind legs are in contact; (iii) is aerial phase

B. Experiment

1) Platform Description: The hardware platform usedfor the experiments is a fully torque controllable, electricalquadruped that weighs 5.5 kg. Each leg module of thequadruped robot is equipped with three custom made brush-less direct current (BLDC) motor units [30]. The body ofthe robot is assembled from carbon fiber tubes and plates,connected by carbon fiber reinforced 3D printed parts. Thefoot is cushioned with sorbothane because of its capability toabsorb shock. The electronic system consists of an on-boardcomputer PC104 with Intel i7-3517UE at 1.70 GHz, ElmoGold Twitter amplifiers, RLS-RMB20 magnetic encoders oneach joint, and an inertial measuring unit (IMU).

2) Computation Setup: The controller is implemented inSimulink Real-Time (SLRT), which runs on the embeddedsystem at a base rate of 4 kHz, while the IMU signals arereceived at 1 kHz. A custom QP solver based on the interior-point method with algorithmic details outlined in [31] isdeveloped to embed the QP solver into SLRT. It exploitsthe sparsity structure of the KKT matrix induced by theMPC formulation to solve QP efficiently, enabling fasterexecutions. For a prediction horizon of 7, this entails solvinga QP with 210 variables, 168 inequality and 126 equalityconstraints. Written in ANSI C, the solver is library-freewhile and it interfaces with SLRT through a gateway s-function. Even with reduced computational resources onembedded system, the proposed MPC achieves executionrates of 160 Hz for prediction horizon of 7. Further details

8488

Amplifiers Computer Encoders

IMU

ABAD motor

KNEE motorHIP motor

Linkage

Carbon fiber tube

Knee jointThigh

Shank

(a) (b)

Foot

Fig. 6: An illustration of the hardware Platform (a) CADmodel of the mechanical components of the leg module,which includes ABAD, HIP and KNEE modules and linkagesof the leg (b) A picture of the assembled quadruped platform,which integrates a computer, sensors including an IMU and12 encoders

0 0.5 1 1.5 2 2.5 3 3.5-0.01

-0.005

0

0.005

0.01

posi

tion

devi

atio

n [m

]

(a) pxpypz

0 0.5 1 1.5 2 2.5 3 3.5

Time [s]

-0.05

0

0.05

orie

ntat

ion

devi

atio

n [ra

d]

(b) rollpitchyaw

Fig. 7: Results of a trotting experiment on the hardware. (a)CoM position deviation from the reference (b) orientationdeviation from the reference. The shaded area indicates whenall four legs are in stance phase. The reference is 0 for bothposition and orientation.

about the custom QP solver are out of the scope of this paper.3) Walking Trot: The walking trot gait is tested to evalu-

ate the performance of the proposed MPC formulation. Fig. 7shows that the deviations in both position and orientation arekept small during the trotting experiment. Stance and swingphases are switched in a fixed time schedule. The swingfoot follows a smooth spline using the PD position control.Specifically, the foot retracts and re-initiate contact at thedesired foothold, which is designed based on the capture-point [32]. The gain values in Table II are first tuned insimulation and then adjusted for experiment as the modelonly approximates real physical systems.

4) Bounding: Bounding experiments are conducted toshow that the proposed method could leverage the full systemdynamics. Fig. 1 shows a sequence of snapshots of thebounding in place experiment. Fig. 8 shows the pitch angleand pitch angular velocity plots of a bounding experimentwhere the robot starts from a static pose. Transition tostance phase occurs upon a touchdown event detected usingmomentum observer [33]. This experiment is preliminaryresult where the robot could bound up to 4 steps. Furtherexperimental test will be conducted.

TABLE II: Cost function weights for simulation and ex-periments. The values in parenthesis represent weights onterminal cost.

Sim. Sim. Acro. Exp. Exp.Bounding Trotting Maneuver Bouding Trotting

Qpx 4e5 (5e5) 5e4 5e6 5e4 2e5 (12e4)Qpy 4e5 (5e5) 5e4 5e6 5e4 4e5 (4e5)Qpz 4e5 (5e5) 5e4 5e6 7e4 1.5e5 (2e5)Qpx 1e3 (4e3) 2e3 (4e3) 5e3 5e2 50Qpy 1e3 (4e3) 2e3 (4e3) 5e3 5e2 200 (150)Qpz 1e3 (4e3) 2e3 (4e3) 5e3 5e2 30QRx 3e4 (5e4) 1e4 (5e4) 1e6 1e3 3e3 (1e3)QRy 3e4 (5e4) 1e4 (5e4) 1e6 0 4e3 (8e3)QRz 3e4 (5e4) 1e4 (5e4) 1e6 0 1e3 (3e3)Qωx 5e2 5e2 5e3 1e2 3 (2)Qωy 5e2 5e2 5e3 0 6 (2)Qωz 5e2 5e2 5e3 2e2 5 (8)Rux 0.1 0.1 0.1 0.1 0.1Ruy 0.1 0.1 0.1 0.1 0.18Ruz 0.1 0.1 0.1 0.1 0.2

0 0.2 0.4 0.6 0.8 1-0.4

-0.2

0

0.2

0.4

Pit

ch A

ng

le [

rad

] (a)

0 0.2 0.4 0.6 0.8 1

Time [s]

-5

0

5

Pit

ch A

ng

ula

r V

elo

city

[ra

d/s

]

(b)

Desired

Measured

Fig. 8: Experiment results of (a) pitch angle and (b) pitchangular velocity in a bounding experiment

IV. CONCLUSION AND FUTURE WORK

In this work, we present a Model Predictive Controlframework to control various kinds of dynamic maneuvers in3D space. By directly using linearized models with rotationmatrices, our method could avoid issues arising from use oflocal coordinates including singularities (Euler angles) andunwinding issues (quaternion). Along with this linearization,the choice of error function on body orientation enables aQP formulation, which could be efficiently solved to achievereal-time execution. With the proposed control framework,a wide range of dynamic gaits both in simulation andexperiments are demonstrated. In the future, we plan toapply this MPC controller to control more complex dynamicmaneuvers including fast galloping and more acrobatic gym-nastic motions.

V. ACKNOWLEDGMENT

This project is supported by NAVER LABS Corp. undergrant 087387, Air Force Office of Scientific Research undergrant FA2386-17-1-4665, and National Science Foundationunder grant 1752262.

8489

REFERENCES

[1] A. Herdt, H. Diedam, P.-B. Wieber, D. Dimitrov, K. Mombaur, andM. Diehl, “Online walking motion generation with automatic footstepplacement,” Advanced Robotics, vol. 24, no. 5-6, pp. 719–737, 2010.

[2] B. Henze, C. Ott, and M. A. Roa, “Posture and balance control forhumanoid robots in multi-contact scenarios based on model predic-tive control,” in Intelligent Robots and Systems (IROS 2014), 2014IEEE/RSJ International Conference on. IEEE, 2014, pp. 3253–3258.

[3] M. Neunert, M. Stauble, M. Giftthaler, C. D. Bellicoso, J. Carius,C. Gehring, M. Hutter, and J. Buchli, “Whole-body nonlinear modelpredictive control through contacts for quadrupeds,” IEEE Roboticsand Automation Letters, vol. 3, no. 3, pp. 1458–1465, 2018.

[4] R. J. Full and D. E. Koditschek, “Templates and anchors: neu-romechanical hypotheses of legged locomotion on land,” Journal ofexperimental biology, vol. 202, no. 23, pp. 3325–3332, 1999.

[5] A. Herdt, N. Perrin, and P.-B. Wieber, “Walking without thinking aboutit,” in IROS 2010-IEEE-RSJ International Conference on IntelligentRobots & Systems. IEEE, 2010, pp. 190–195.

[6] S. KAJITA, “Study of dynamic biped locomotion on rugged terrain-derivation and application of the linear inverted pendulum mode,” inProc. IEEE Int. Conf. on Robotics and Automation, Sacramento, CA,1991, 1991, pp. 1405–1411.

[7] H.-W. Park, P. M. Wensing, S. Kim et al., “Online planning forautonomous running jumps over obstacles in high-speed quadrupeds,”in Robotics: Science and Systems, Rome, Italy, July 13-17, 2015.

[8] X. Xiong and A. D. Ames, “Bipedal hopping: Reduced-order modelembedding via optimization-based control,” in 2018 IEEE/RSJ Inter-national Conference on Intelligent Robots and Systems (IROS). IEEE,2018, pp. 3821–3828.

[9] D. E. Orin, A. Goswami, and S. H. Lee, “Centroidal dynamics of ahumanoid robot,” Autonomous Robots, vol. 35, no. 2-3, pp. 161–176,2013.

[10] S. Kuindersma, R. Deits, M. Fallon, A. Valenzuela, H. Dai, F. Per-menter, T. Koolen, P. Marion, and R. Tedrake, “Optimization-basedlocomotion planning, estimation, and control design for the atlashumanoid robot,” Autonomous Robots, vol. 40, no. 3, pp. 429–455,2016.

[11] H. Dai, A. Valenzuela, and R. Tedrake, “Whole-body motion planningwith simple dynamics and full kinematics,” in Proceedings of theIEEE-RAS international conference on humanoid robots, 2014.

[12] G. Bledt, P. M. Wensing, and S. Kim, “Policy-regularized modelpredictive control to stabilize diverse quadrupedal gaits for the MITcheetah,” in Intelligent Robots and Systems (IROS), 2017 IEEE/RSJInternational Conference on. IEEE, 2017, pp. 4102–4109.

[13] A. W. Winkler, C. D. Bellicoso, M. Hutter, and J. Buchli, “Gait andtrajectory optimization for legged systems through phase-based end-effector parameterization,” IEEE Robotics and Automation Letters,vol. 3, no. 3, pp. 1560–1567, 2018.

[14] J. Di Carlo, P. M. Wensing, B. Katz, G. Bledt, and S. Kim, “Dynamiclocomotion in the mit cheetah 3 through convex model-predictivecontrol,” in 2018 IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS). IEEE, 2018, pp. 1–9.

[15] H.-W. Park, P. M. Wensing, and S. Kim, “High-speed bounding withthe MIT cheetah 2: Control design and experiments,” The InternationalJournal of Robotics Research, vol. 36, no. 2, pp. 167–192, 2017.

[16] M. D. Shuster, “A survey of attitude representations,” Navigation,vol. 8, no. 9, pp. 439–517, 1993.

[17] S. P. Bhat and D. S. Bernstein, “A topological obstruction to globalasymptotic stabilization of rotational motion and the unwinding phe-nomenon,” in American Control Conference, 1998. Proceedings of the1998, vol. 5. IEEE, 1998, pp. 2785–2789.

[18] G. Wu and K. Sreenath, “Variation-based linearization of nonlinearsystems evolving on SO(3)and S2,,” IEEE Access, vol. 3, pp. 1592–1604, 2015.

[19] T. Lee, M. Leok, and N. H. McClamroch, “Stable manifolds of saddleequilibria for pendulum dynamics on S2 and SO(3),” in Decision andControl and European Control Conference (CDC-ECC), 2011 50thIEEE Conference on Decision and Control and European ControlConference (CDC-ECC). IEEE, 2011, pp. 3915–3921.

[20] F. Bullo and A. D. Lewis, Geometric control of mechanical systems:modeling, analysis, and design for simple mechanical control systems.Springer Science & Business Media, 2004, vol. 49.

[21] O. von Stryk, Numerical Solution of Optimal Control Problems byDirect Collocation. Basel: Birkhauser Basel, 1993, pp. 129–143.[Online]. Available: https://doi.org/10.1007/978-3-0348-7539-4 10

[22] F. C. Park, “Distance metrics on the rigid-body motions with applica-tions to mechanism design,” Journal of Mechanical Design, vol. 117,no. 1, pp. 48–54, 1995.

[23] M. A. Henson, “Nonlinear model predictive control: current status andfuture directions,” Computers and chemical engineering, vol. 23, pp.187–202, 1998.

[24] F. Kuhne, W. F. Lages, and J. G. da Silva Jr, “Model predictive controlof a mobile robot using linearization,” in Proceedings of mechatronicsand robotics, 2004, pp. 525–530.

[25] M. Cannon, D. Ng, and B. Kouvaritakis, “Successive linearizationnmpc for a class of stochastic nonlinear systems,” in Nonlinear ModelPredictive Control. Springer, 2009, pp. 249–262.

[26] J. P. Hespanha, Linear systems theory. Princeton university press,2018.

[27] J. E. Marsden and T. S. Ratiu, “Introduction to mechanics andsymmetry,” Physics Today, vol. 48, no. 12, p. 65, 1995.

[28] T. Lee, M. Leoky, and N. H. McClamroch, “Geometric tracking controlof a quadrotor uav on SE(3),” in Decision and Control (CDC), 201049th IEEE Conference on. IEEE, 2010, pp. 5420–5425.

[29] A. Graham, Kronecker products and matrix calculus with applications.Courier Dover Publications, 2018.

[30] Y. Ding and H.-W. Park, “Design and experimental implementation ofa quasi-direct-drive leg for optimized jumping,” in Intelligent Robotsand Systems (IROS), 2017 IEEE/RSJ International Conference. IEEE,2017, pp. 300–305.

[31] L. Vandenberghe, “The cvxopt linear and quadratic cone programsolvers,” Online: http://cvxopt.org/documentation/coneprog. pdf, 2010.

[32] J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture point: Astep toward humanoid push recovery,” in Humanoid Robots, 2006 6thIEEE-RAS International Conference on. IEEE, 2006, pp. 200–207.

[33] A. De Luca and R. Mattone, “Actuator failure detection and isolationusing generalized momenta,” in Robotics and Automation, 2003.Proceedings. ICRA’03. IEEE International Conference on, vol. 1.IEEE, 2003, pp. 634–639.

8490