Approximate Simulation for Template-Based Whole-Body Control

Vince Kurtz, Patrick M. Wensing, Michael D. Lemmon, and Hai Lin

Abstract— Reduced-order template models are widely usedto control high degree-of-freedom legged robots, but existingmethods for template-based whole-body control rely heavilyon heuristics and often suffer from robustness issues. In thisletter, we propose a template-based whole-body control methodgrounded in the formal framework of approximate simulation.Our central contribution is to demonstrate how the Hamiltonianstructure of rigid-body dynamics can be exploited to estab-lish approximate simulation for a high-dimensional nonlinearsystem. The resulting controller is passive, more robust topush disturbances, uneven terrain, and modeling errors thanstandard QP-based methods, and naturally enables high centerof mass walking. Our theoretical results are supported bysimulation experiments with a 30 degree-of-freedom Valkyriehumanoid model.

I. INTRODUCTION

Reduced-order “template” models are foundational to con-trolling high degree-of-freedom legged robots [1], [2]. Suchmodels, like the Linear Inverted Pendulum (LIP), abstractaway complicating details of a robot’s dynamics, enablingfocus on key aspects of balancing and locomotion, such asregulating the center of mass (CoM) and center of pressure(CoP) [3]. Template models allow for longer planning hori-zons and reduce computational costs, and in many ways haveenabled the expanded use of legged robots in recent years.

Despite the widespread usage of template models, how-ever, surprisingly little is known about formal connectionsbetween templates and full-order rigid-body robot models.This means that whole-body controllers used to track tem-plate models include many heuristic design elements. Whilesuch methods can achieve good results in practice, theyrequire extensive parameter tuning and are not always robust.

Addressing the theoretical gap between template andwhole-body models, as well as the performance issues thisgives rise to in practice, is the focus of a growing bodyof literature. A strict approach was taken in [4], where thedynamics of a Spring-Loaded Inverted Pendulum (SLIP)were embedded as the the Hybrid Zero Dynamics [5] ofan asymmetric hopper. Reachability analysis was used in [6]to constrain the template to enforce constraints in the full-order model. Our proposed approach is related most closelyto [7], which used the framework of approximate simulationto establish a formal relationship between the centroidaldynamics of a multi-link balancer and the LIP model.

Approximate simulation, an extension of simulation/bi-simulation from formal methods to continuous systems, en-sures that the outputs of two systems can remain ε-close [8].In [7], it was shown that establishing such a relation enablesthe projection of contact friction constraints to the template

(a) Recovering from pushes. (b) Walking over uneven terrain.

(c) Robustness to modeling errors. (d) Walking with a high CoM.

Fig. 1. Simulation test scenarios considered in this letter. Our approachenforces approximate simulation and passivity, and consistently outperformsstandard optimization-based whole-body control algorithms.

model, and the resulting controller enabled better recoveryfrom large push disturbances than a standard approach.

These prior results have several important limitations,however. First, no formal connection was made between theCoM dynamics and the full rigid-body dynamics. Instead,it was assumed that desired CoM accelerations could beenforced via task-space feedback linearization: this enabledthe use of existing results on approximate simulation forlinear systems. Second, since full rigid-body dynamics werenot considered explicitly, the approach of [7] does not ac-count for subtasks essential to locomotion such as controllingswing foot trajectories. Finally, external disturbances arisingfrom contact events and modeling errors were not considered.

In this letter, we address each of these limitations. Weestablish approximate simulation between a simple linearCoM model and the (nonlinear) whole-body dynamics bydrawing on the Hamiltonian structure of rigid-body dy-namics. This enables a heirarchical approximate simulationrelationship between the LIP model, CoM model, and whole-body model. We propose an optimization-based whole-bodycontroller that enforces approximate simulation, managescomplex whole-body constraints, and enables lower prioritytasks when possible (e.g., swing foot tracking). We explicitlyaccount for disturbances from impact events and modelingerrors using the recently proposed framework of robustapproximate simulation [9]. In addition to enforcing approx-

arX

iv:2

006.

0992

1v2

[cs

.RO

] 2

3 Ju

l 202

0

imate simulation, our proposed controller ensures passivity,suggesting superior safety and robustness properties [10].

These theoretical results are validated in simulation ex-periments with a 30 degree-of-freedom (DoF) Valkyrie hu-manoid. We find that our proposed whole-body controlleris more robust to push disturbances, modeling errors, anduneven terrain than a standard whole-body controller. Fur-thermore, our approach continues to function effectively evenin singular configurations, such as with straight legs, enablingmore efficient walking with a high CoM.

The remainder of this letter is organized as follows: back-ground on approximate simulation is introduced in Section II.Our main theoretical results are presented in Section III,and simulation experiments are discussed in Section IV. Weconclude with Section V.

II. BACKGROUND

Approximate simulation is a relationship between twodynamical systems, Σ1 and Σ2:

Σ1 :

{x1 = f1(x1,u1,d)

y1 = g1(x1), Σ2 :

{x2 = f2(x2,u2)

y2 = g2(x2),

(1)where xi ∈ Xi ⊆ Rni are the system states, ui ∈ Ui ⊆Rpi are the control inputs, d ∈ Rd is a disturbance signalrepresenting external disturbances or modeling errors, andyi ∈ Rm are the system outputs. Note that the states maybe of different sizes but the outputs must be of the samesize. We typically consider Σ1 to be a more complex systemmodel (e.g., full rigid-body dynamics) and Σ2 to be a simplermodel (e.g., template dynamics). Σ1 approximately simulatesΣ2 if Σ1 can always track the output of Σ2 with ε precision:

Definition 1 (Approximate Simulation): Let ui(t) be in-put signals to Σi with corresponding state and output tra-jectories xi(t) and yi(t). We say that Σ1 approximatelysimulates Σ2 if for every u2(t) there exists an ε > 0 suchthat if ‖y1(0) − y2(0)‖ ≤ ε, there exists u1(t) such that‖y1(t)− y2(t)‖ ≤ ε ∀ t ≥ 0.

We can certify approximate simulation by finding aLyapunov-like simulation function V(x1,x2) which boundsthe output error, and an interface u1 = uV(x1,x2,u2)which specifies inputs to Σ1 such that y1 tracks y2. Thenotion of robust approximate simulation [9] extends theseresults to account for external disturbances d. Once a (robust)simulation function and an interface are found, they can beused to compute an output error bound ε [8], [9].

Typically, a controller is designed for the simplified modelΣ2: the full-order model Σ1 can also be guaranteed tocomplete the given task via the interface uV .

The main challenge is finding a Lyapunov-like simulationfunction and an associated interface. For linear systems, thereare well-established conditions for the existence of quadraticsimulation functions [9], [11]. Systematic design proceduresfor nonlinear systems remain an area of ongoing research,though state-of-the-art techniques based on Sum-of-Squaresprogramming [12] do not currently scale to high dimensionalsystems like legged robots. To address this challenge, we

Template (LIP) Model, Σ3

CoM Model, Σ2

Whole-Body Model, Σ1

linear interface (4)

energy shaping interface (8)

Fig. 2. Model hierarchy considered in this approach. Our primarycontribution is in establishing a robust approximate simulation relationbetween the intermediate CoM model and the whole-body model.

draw inspiration from energy shaping control [10] to find asimulation function.

III. MAIN RESULTS

A. Model Hierarchy

In our proposed control approach, we use the modelhierarchy shown in Figure 2. The whole-body model Σ1

approximately simulates an intermediate CoM model Σ2,which in turn approximately simulates the LIP model Σ3.

At the highest level of abstraction, the LIP model Σ3 isused to generate a constant-height CoM trajectory consistentwith a set of pre-planned footsteps. The LIP dynamics aregiven by [3]

x3 = Alipx3 + Blipu3, y3 = Clipx3, (2)

where x3 =[(plipcom)T (vlipcom)T

]T ∈ R6 is the position andvelocity of the CoM, u3 ∈ R2 is the CoP, and y3 = plipcom.

At the intermediate level of abstraction, we consider asimple single-integrator model of the CoM,

x2 = u2, y2 = x2, (3)

where x2 ∈ R3 represents the CoM position. This modeladds flexibility over the LIP (2) in the sense that the CoMis not constrained to a constant height. Noting that (2) and(3) are linear systems, the interface

u2 = vlipcom + K(x2 − plipcom) (4)

enforces a robust approximate simulation relation betweensystems (2) and (3), where K is any Hurwitz matrix [9].

At the lowest level of abstraction, we consider the fullmulti-contact rigid-body dynamics of an n-link robot

M(q)q + C(q, q)q + τg = ST τ +∑

JTcj fcj + τext, (5)

where q ∈ Rn+6 are generalized coordinates and τ arecontrolled joint torques. M(q) is the positive definite massmatrix, C(q, q)q collects Coriolis and centripetal terms, τgis the torque due to gravity, fcj ∈ R3 are forces at contactpoints cj , Jcj are the associated contact Jacobians, and τextare external (disturbance) torques.

These whole-body dynamics form a nonlinear system with

x1 =[qT qT

]T, y1 = xcom(q), (6)

where xcom is the position of the CoM. We denote the CoMJacobian as J (i.e., xcom = Jq).

B. Energy Shaping for Approximate Simulation

As discussed in Section II, establishing approximate sim-ulation for nonlinear systems is a difficult and open problem.In this section, we show that the Hamiltonian structure of Σ1

(6) can be used to certify that Σ1 approximately simulatesthe intermediate CoM model Σ2 (3).

We first introduce the control transformation

u1 = ST τ +∑

JTcj fcj . (7)

The resolution of particular joint torques τ and contact forcesfcj will be discussed in Section III-C. With this controltransformation, energy shaping principles can be used todesign a simulation function and interface, as follows:

Theorem 1: Assume that x1 and x2 are constrained tocompact sets X1, X2, and that ‖u2‖ ≤ u2. Let κ > 0 be ascalar constant, and KD � 0 be a symmetric positive definitematrix. Then the interface

u1 = τg − 2κJT (y1 − y2)−KD(q− JTu2) (8)

certifies that Σ1 (6) robustly approximately simulates Σ2 (3)with the associated simulation function given by

V(x1,x2) =

[q

y1 − y2

]T [ 12κM α

2 JTΛα2 ΛJ I

] [q

y1 − y2

],

(9)where, Λ(q) = (JM−1JT )−1 is the task-space inertiamatrix, 0 < α < 1√

2κλ∗ , λ∗ is the maximum eigenvalueof Λ(q) over X1, and 1

kKD � αJTΛJ.This simulation function essentially consists of the scaled

closed-loop Hamiltonian under (8) plus a cross term thatensures that V < 0 for large output errors ‖y1 − y2‖.

To prove Theorem 1, we first need the following resultregarding the structure of the rigid-body dynamics (5):

Lemma 1 ( [13]): There exists a matrix C(q, q) such that(5) holds and M(q, q)− 2C(q, q) is skew-symmetric.

Proof: [Theorem 1] We will use V to establish uniformultimate boundedness [14] of ‖y1 − y2‖ under (8).

First, we show that V ≥ 0. The Schur complement of

P =

[12κM α

2 JTΛα2 ΛJ I

]is given by I − 1

2κα2Λ. From this,

the condition α < 1√1/2κλ∗

ensures V ≥ 0. Furthermore,

we can see that P(q) � βI, where β > 0 is the minimumeigenvalue of P. This ensures β‖y1−y2‖2 ≤ V , and so thesimulation function is positive and bounds the output error.

Next, we will establish an output error bound ε ≥ ‖y1 −y2‖ by considering V . Note that V can be written as

V =1

2κqTM(q)q + Udes(x1,x2) + αqTJTΛ(y1 − y2),

where the first two terms consist of a kinetic energy term12κ qTM(q)q, and a desired potential Udes(x1,x2) = ‖y1−y2‖2. These two terms are a re-scaling of the closed-loopHamiltonian

H =1

2qTM(q)q + κ‖y1 − y2‖2. (10)

The Hamiltonian term evolves as follows:1

κH =

1

2κqTM(q)q +

1

κqTM(q)q + Udes

=1

2κqTMq +

1

κqT (u1 + τext −Cq− τg) + Udes

=1

κqT (u1 + τext − τg) +

∂Udes

∂qq +

∂Udes

∂x2u2

by Lemma 1. Applying the interface (8) and noting that[∂Udes

∂q

]T= 2JT (y1 − y2), we have that

1

κH =

1

κ[JKDq]Tu2 +

1

κqT τext +

∂Udes

∂x2u2 −

1

κqTKDq.

(11)

Next, consider the evolution of the cross-term,ddt q

TJTΛ(y1 − y2). This particular cross term waschosen to induce a negative term in V ,

−2ακ(y1−y2)TΛJM−1JT (y1−y2) = −2ακ‖y1−y2‖2.(12)

This term essentially ensures that V < 0 if ‖y1 − y2‖ islarge enough, allowing us to establish an upper error bound.

Combining these elements, V can be written as

V = −qT (1

kKD − αJTΛJ)q− 2ακ‖y1 − y2‖2+

zT1 (y1 − y2) + zT2 u2 +1

κqT τext,

where

zT1 = (2 + α)uT2 + αqT (JTΛ + JT Λ)− αqTKDM−1JTΛ

− αqTCTM−1JTΛ,

zT2 =1

κqTKDJT − qTJTΛ.

Recalling that 1kKD � αJTΛJ, this gives rise a bound,

V ≤ −2ακ‖y1−y2‖2+‖z1‖∞‖y1−y2‖+‖z2‖∞‖u2‖∞+ ‖q‖∞‖τext‖∞,

where ‖·‖∞ represents a supremum over X1,X2. Noting thatthis expression is quadratic in ‖y1 − y2‖, we can establishthe error bound ε =

‖z1‖∞ +√‖z1‖2∞ + 8ακ(‖z2‖∞‖u2‖∞ + ‖q‖∞‖τext‖∞)

4ακ.

The basic idea behind this Theorem is to design aninterface based on energy shaping control [10] that drives y1

to y2. This interface (8) is analogous to a PD controller withgravity compensation. The resulting closed-loop Hamiltonianprovides a starting point for a simulation function, which isthen derived by adding cross-terms that force V ≤ 0 when‖y1 − y2‖ is large.

Remark 1: While a u1 that tracks a nominal CoM tra-jectory could also be chosen using task-space feedbacklinearization, our approach does not require a left inverseof the CoM Jacobian J. This means that our interface (8) iseffective even when the robot is in a singular configuration

Trial 0 1 2 3 4 5 6 7 8 9 10Ours 3 3 3 3 7 3 7 3 3 7 7

Standard 7 7 3 7 3 7 7 7 7 7 7

TABLE IPUSH RECOVERY SIMULATION RESULTS (7= FAIL, 3= SUCCESS).

(i.e., when the legs are fully extended). This enables walkingwith a higher CoM, as shown in Section IV.

Beyond the fact that the interface (8) and simulationfunction (9) certify a robust approximate simulation relation,they also enforce passivity:

Corollary 1: The interface (8) ensures passivity of theclosed-loop system (interconnection of Σ1 and Σ2) withrespect to inputs u2 and external disturbances τext.

Proof: Consider the closed-loop Hamiltonian (10).From (11), we have that

H ≤(2κ(y1 − y2)T + [JKDq]T

)u2 + qT τext. (13)

For u2 = 0, this certifies passivity of the mapping from ex-ternal disturbances τext to joint velocities q. Similarly, in theabsence of disturbances (τext = 0), (13) certifies a passiverelationship between the the input to the reduced-order modelu2 and a unusual output (2κ(y1 − y2) + JKDq), composedof a scaled output error (2κ(y1 − y2)) and a scaled CoMvelocity (JKDq).

These passivity relationships essentially regulate how ad-ditional energy enters the system. This property has im-portant implications for safety and robustness. Specifically,the feedback interconnection of passive systems is alwayspassive, a property that does not hold for general notions ofstability [10]. Furthermore, passivity-based controllers tendto be remarkably robust to modeling errors and externaldisturbances [15]–[19], a critical property for operation inuncertain real-world settings.

Combining Theorem 1 with existing results for linearsystems [9], [11], we can obtain the following main result:

Theorem 2: The whole-body model Σ1 robustly approxi-mately simulates the LIP model Σ3.

Proof: From Theorem 1, Σ1 robustly approximatelysimulates Σ2. This means that we can choose x2(0) andu2(t) such that ‖y1(t)−y2(t)‖ ≤ ε1 for all t ≥ 0. Following[11], we have that Σ2 approximately simulates Σ3 under theinterface (4). The associated error bound, ε2, is defined suchthat ε2 ≥ ‖y2(0) − y3(0)‖ for any x2(0),x3(0) [11]. Thismeans that x1(0) can always be chosen such that ‖y1(0)−y2(0)‖ ≤ ε1 and ‖y2(0)−y3(0)‖ ≤ ε2. Thus for any controlinput for the LIP u3(t) there exists an ε > 0 and a whole-body control input u1(t) such that:

‖y1 − y3‖ ≤ ‖y1 − y2‖+ ‖y2 − y3‖ ≤ ε1 + ε2 = ε, (14)

and the Theorem holds.

C. Optimization-Based Whole-Body Control

In the previous section, we showed how to select u1 totrack the intermediate CoM model while ensuring approx-imate simulation and passivity. But choosing joint torques

Trial 0 1 2 3 4 5 6 7 8 9 10Ours 7 3 3 3 3 3 3 7 3 3 3

Standard 7 7 7 7 7 7 7 7 7 7 7

TABLE IIUNEVEN TERRAIN SIMULATION RESULTS (7= FAIL, 3= SUCCESS).

consistent with this u1 while meeting torque limits andcontact friction constraints is non-trivial. To do so, wepropose the following QP:

minu2,τ ,τ0,q,fext

w1‖u2 − udes2 ‖+ (15)

w2‖Jfootq + Jfootq− xdesfoot‖+ ... (16)

s.t. Mq + Cq + τg(q) = ST τ +∑

JTcj fcj (17)

uV(x1,x2,u2) + NT τ0 = ST τ +∑

JTcj fcj (18)

Jcj q + Jcj q = 0 (19)fcj ∈ friction cones (20)τ ≤ τ ≤ τ (21)

The primary cost function (15) attempts to ensure that u2

matches a nominal input udes2 , given by the interface with theLIP model (4). Additional weighted costs like (16) can beadded to regulate lower-priority tasks like tracking a desiredswing foot trajectory, minimizing angular momentum, andregulating torso orientation.

The constraint (17) enforces the robot’s dynamics, and (forfixed q, q) is linear in the decision variables. (18) ensuresthat the robot’s CoM motion is determined by the interface(8), which is linear in u2. We use the dynamically consistentnull-space projector N [20] to add some flexibility—all jointtorques are not strictly determined by (8)—while ensuringthat the CoM motion matches that requested by the interface.This enables lower-priority tasks like swing foot control.Contact points are fixed in place by (19), while (20) ensuresthat contact forces remain within their respective (Coulomb)friction cones. This friction constraint is linear if pyramidalinner approximations of the friction cones are taken. Finally,(21) enforces torque limits.

This proposed optimization scheme is closely related to theQPs widely used for whole-body control of high-DoF leggedrobots [1]. In such schemes, a desired CoM acceleration

xdescom = vlipcom + KlipP (vlipcom − xcom) + Klip

D (plipcom − xcom)(22)

which tracks the LIP is encoded as a cost, subject tosimilar dynamics, friction, contact, and torque constraints.This standard approach, however, does not enforce passivityor allow for any guarantees on tracking performance.

As we will show in Section IV, our approach is more ro-bust to large push disturbances, uneven terrain, and modelingerrors than these standard QPs. This is due to the fact thatthe interface constraint (18) enforces passivity properties (seeCorollary 1), regulating the energy that can be injected intothe system. At the same time, (18) is minimally restrictive.Intuitively, u2 regulates the CoM, while τ0 regulates all other

(a) The simulation function (9) de-creases except when u2 is large,driving the output error ‖y1−y2‖2to zero.

(b) Both the linear CoM model andthe true CoM converge to the set-point.

Fig. 3. Using our approach to regulate the CoM while standing.

(non-CoM) degrees of freedom, allowing the left hand sideof (18) to take arbitrary values if needed.

IV. SIMULATION RESULTS

We implemented the controller described above in simula-tion using a 30 DoF model of the NASA Valkyrie humanoid.Our implementation1 is in Python, using the Drake [21]simulation and dynamics platform. The whole-body QP (15)was solved at 200Hz with Gurobi [22].

First, we consider a scenario in which the robot movesits CoM to a desired position ([−0.01 0.05 0.85]m) whilebalancing in double support. First, a nominal trajectory ofthe LIP model x3 was generated, holding constant at thedesired position. The CoM model x2 was initialized to matchthe true initial CoM position. The interface (4) was used toselect a nominal input to the CoM model, udes2 . Then thewhole-body QP (15) was solved to select joint torques. Weused κ = 5000 and KD = 1000I. The results are shown inFigure 3. The robot’s CoM moves smoothly to the desiredposition, while the simulation function (9) decreases exceptwhen u2 is large. An approximate error bound of ε = 0.17mwas computed by taking ‖z1‖∞, ‖z2‖∞ to be maxima overthe executed trajectory.

To evaluate our proposed whole-body controller, we com-pare the performance of our approach with a standard whole-body QP. For the standard QP, we generally followed theapproach of [23], using a LIP reference rather than a SLIPreference. We used a QP version of the controller presentedin [24] with the same weighted tasks as our approach. Thismethod of tracking a template model with a QP-based whole-body controller is used broadly for humanoid control [1].

Both methods used the same tuning parameters whereverapplicable. For the standard approach, the parameters of (22)were tuned to match the step response of our approach.For each scenario, we specified a sequence of footstepsand swing foot trajectories a-priori. We then generated acorresponding LIP model trajectory [3], and tracked it usingeither our approach or the standard whole-body QP.

We considered four application scenarios, as shown inFigure 1: recovering from push disturbances, walking over

1Code: https://github.com/vincekurtz/valkyrie_drake

Trial 0 1 2 3 4 5 6 7 8 9 10Ours 3 3 3 7 3 3 7 3 3 3 7

Standard 7 3 3 7 3 7 7 3 7 7 7

TABLE IIIMODEL ERROR SIMULATION RESULTS (7= FAIL, 3= SUCCESS).

CoM Height (m) 0.95 1.00 1.02 1.05Ours (×105) 3.77 2.88 2.60 2.18

Standard (×105) 3.51 2.63 FAILED FAILED

TABLE IVAPPROXIMATE ENERGY USE (

∫τT τdt) FOR DIFFERENT COM HEIGHTS.

uneven terrain, walking despite modeling errors, and walkingwith a high CoM.

To test push recovery, we applied randomly generatedpushes while the robot was walking. The time of eachpush was sampled uniformly from [1.0, 3.5]s. The push’smagnitude was sampled uniformly from [400, 600]N andapplied for 0.05s in the +y or −y direction, each with 50%probability. A trial was considered successful if the robotcould walk for 5s total without falling. The results are shownin Table I. The same push was applied for both approachesin each trial. Our approach was successful in 7 of the 11trials, while the standard approach was only successful in 2.

Next, we tested robustness to uneven terrain by randomlyplacing 15 bricks in the robot’s path. These bricks were fixedin place, but were smaller than the robot’s foot, leading tofrequent shifting of the stance foot while walking. A trialwas considered successful if the robot could cross the brickswithout falling. The results are shown in Table II, with thesame brick placement used for both approaches in each trial.Our approach was successful in all but 2 trials, while thestandard approach was not successful in any of the trials.

Empirically, the primary failure mode for our approachwas when the swing foot got caught behind a brick. Incontrast, the primary failure mode for the standard approachwas when the stance foot shifted significantly, an event thatoccurred many times in each trial. The passivity of ourapproach helps explain our method’s success in this regard: ashift in stance foot essentially injects energy into the system.Our controller’s passivity (see Section III-B) ensures that thetotal system energy does not grow unbounded as a result.

Our third test scenario dealt with modeling error. In thisscenario, the controller assumed the same robot model asin the trials above, but the true robot model was randomlyperturbed. Specifically, the mass of each link, mi, wasmodified according to

mnewi = max {ε,mi +mir} , r ∼ N (0, 1).

A trial was considered successful if the robot could walk for5s without falling. The same randomized model was usedfor both approaches in each trial. The results are shown inTable III. Our approach was successful in every case that thestandard approach was successful, as well as four cases inwhich the standard approach failed.

In the final test scenario, we demonstrated that ourapproach can be used for walking with a higher CoM,which reduces torques at the knees, enabling more efficient

Fig. 4. Output tracking for our approach walking with a high CoM. Theenergy-based nature of our approach enables effective operation in singularconditions, and approximate simulation relations between models allow forsome deviation in the system outputs. Together, these properties enable moreefficient walking with a high CoM.

locomotion [25]. Standard whole-body control approachesdo not perform well in configurations like that shown inFigure 1d, where the CoM is high enough that the back legis fully extended. In such near-singular configurations, thenumerical conditioning of whole-body QPs becomes poor,often resulting in extreme whole-body movements and a lossof balance. Our approach, however, regulates the systemenergy rather than trying to enforce specific accelerations,and is effective in these configurations.

We recorded the integral of squared joint torques, a proxyfor energy use, over 5s of walking for various CoM heights.Results are shown in Table IV. Our approach was successfulwith a higher CoM, enabling lower energy walking.

High CoM walking is facilitated by the fact that approxi-mate simulation allows for some deviation between the LIPCoM position (y3), the nominal CoM position (y2), and thetrue CoM position (y1). These three quantities are plottedin Figure 4. When the CoM is above the stance foot insingle support, the desired LIP CoM height can be achieved(t ≈ {3.5, 5.5, 7.5, 9.5}s). In double support, however, thedesired LIP CoM height is too high, and so the intermediatemodel (x2) height is adjusted downward (t ≈ {3, 5, 7, 9}s).This flexibility reduces the burden on template trajectoriesof being exactly replicated by the whole-body model, and isa natural byproduct of approximate simulation relationships.

V. CONCLUSION

We introduced a template-based whole-body control strat-egy that enforces approximate simulation between the LIPmodel, a linear CoM model, and the whole-body dynamics.In addition to enabling formal guarantees of tracking perfor-mance, our approach is passive, more robust than standardmethods to push disturbances, uneven terrain, and modelingerrors, and allows for high CoM walking. Future work willfocus on implementing these methods on a real robot and onextensions to nonlinear template models. In this last regard,we anticipate that the energy-based approaches put forwardin this letter will continue to enable approximate simulationstrategies to scale to more complex systems.

REFERENCES

[1] P.-B. Wieber, R. Tedrake, and S. Kuindersma, “Modeling and controlof legged robots,” in Springer Handbook of Robotics, B. Siciliano andO. Khatib, Eds. Springer, 2016, pp. 1203–1234.

[2] 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.

[3] S. Kajita, F. Kanehiro, K. Kaneko, K. Yokoi, and H. Hirukawa, “The3D linear inverted pendulum mode: A simple modeling for a bipedwalking pattern generation,” in IEEE/RSJ Int. Conf. on IntelligentRobots and Systems, vol. 1, 2001, pp. 239–246.

[4] I. Poulakakis and J. W. Grizzle, “The spring loaded inverted pendulumas the hybrid zero dynamics of an asymmetric hopper,” IEEE Trans.on Automatic Control, vol. 54, no. 8, pp. 1779–1793, 2009.

[5] E. R. Westervelt, J. W. Grizzle, and D. E. Koditschek, “Hybrid zerodynamics of planar biped walkers,” IEEE Trans. on Automatic Control,vol. 48, no. 1, pp. 42–56, 2003.

[6] J. Liu, P. Zhao, Z. Gan, M. Johnson-Roberson, and R. Vasudevan,“Leveraging the template and anchor framework for safe, onlinerobotic gait design,” in IEEE Int. Conf. on Robotics and Automation,2020, pp. 10 869–10 875.

[7] V. Kurtz, R. R. da Silva, P. M. Wensing, and H. Lin, “Formalconnections between template and anchor models via approximatesimulation,” in IEEE-RAS Int. Conf. on Humanoid Robots, 2019, pp.64–71.

[8] A. Girard and G. J. Pappas, “Approximate bisimulation: A bridgebetween computer science and control theory,” European Journal ofControl, vol. 17, no. 5-6, pp. 568–578, 2011.

[9] V. Kurtz, P. M. Wensing, and H. Lin, “Robust approximate simulationfor hierarchical control of linear systems under disturbances,” inAmerican Control Conference, 2020.

[10] G. A. Folkertsma, S. Stramigioli et al., “Energy in robotics,” Founda-tions and Trends® in Robotics, vol. 6, no. 3, pp. 140–210, 2017.

[11] A. Girard and G. J. Pappas, “Hierarchical control system design usingapproximate simulation,” Automatica, vol. 45, no. 2, pp. 566–571,2009.

[12] S. W. Smith, H. Yin, and M. Arcak, “Continuous abstraction ofnonlinear systems using sum-of-squares programming,” in Conferenceon Decision and Control, 2019.

[13] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: mod-elling, planning and control. Springer, 2010.

[14] H. K. Khalil and J. W. Grizzle, Nonlinear systems. Prentice hallUpper Saddle River, NJ, 2002, vol. 3.

[15] P. Falugi, “Model predictive control: a passive scheme,” IFAC Pro-ceedings Volumes, vol. 47, no. 3, pp. 1017–1022, 2014.

[16] A. Albu-Schaffer and C. O. F. Petit, “Energy shaping control for a classof underactuated euler-lagrange systems,” IFAC Proceedings Volumes,vol. 45, no. 22, pp. 567–575, 2012.

[17] N. S. Manjarekar, R. N. Banavar, and A. G. Kelkar, “Robust passifi-cation using norm bounded uncertainty models,” in IEEE Conferenceon Control Applications, vol. 2, 2003, pp. 1327–1332.

[18] J. Back and W. Ha, “Robust tracking of robot manipulators viamomentum-based disturbance observer and passivity-based controller,”International Journal of Control, Automation and Systems, vol. 17,no. 4, pp. 976–985, 2019.

[19] B. Henze, M. A. Roa, and C. Ott, “Passivity-based whole-body balanc-ing for torque-controlled humanoid robots in multi-contact scenarios,”The International Journal of Robotics Research, vol. 35, no. 12, pp.1522–1543, 2016.

[20] O. Khatib, “A unified approach for motion and force control of robotmanipulators: the operational space formulation,” IEEE journal ofrobotics and automation, vol. 3, no. 1, pp. 43–53, 1987.

[21] R. Tedrake and the Drake Development Team, “Drake: A planning,control, and analysis toolbox for nonlinear dynamical systems,” 2016.[Online]. Available: http://drake.mit.edu

[22] L. Gurobi Optimization, “Gurobi optimizer reference manual,” 2020.[Online]. Available: http://www.gurobi.com

[23] P. M. Wensing and D. E. Orin, “High-speed humanoid running throughcontrol with a 3d-slip model,” in IEEE/RSJ Int. Conf. on IntelligentRobots and Systems, 2013, pp. 5134–5140.

[24] ——, “Generation of dynamic humanoid behaviors through task-spacecontrol with conic optimization,” in IEEE Int. Conf. on Robotics andAutomation, 2013, pp. 3103–3109.

[25] R. J. Griffin, G. Wiedebach, S. Bertrand, A. Leonessa, and J. Pratt,“Straight-leg walking through underconstrained whole-body control,”in IEEE Int. Conf. on Robotics and Automation, 2018, pp. 5747–5754.