Trajectory generation for multi-contact momentum-control

Alexander Herzog1, Nicholas Rotella2, Stefan Schaal1,2, Ludovic Righetti1

Abstract— Simplified models of the dynamics, such as thelinear inverted pendulum model (LIPM) have proven to per-form well for biped walking on flat ground. However, for morecomplex tasks the assumptions of these models can becomelimiting. For example, the LIPM does not allow to controlcontact forces independently, is limited to co-planar contactsand assumes that the angular momentum is zero. In this paper,we propose to use the full momentum equations of a humanoidrobot in a trajectory optimization framework to plan its centerof mass, linear and angular momentum trajectories. The modelalso allows to plan desired contact forces for each end-effectorin arbitrary contact locations. We extend our previous resultson LQR design for momentum control by computing the(linearized) optimal momentum feedback law in a recedinghorizon fashion. The resulting desired momentum and theassociated feedback law is then used in a hierarchical wholebody control approach. Simulation experiments show that theapproach is computationally fast and is able to generate plansfor locomotion on complex terrains while demonstrating goodtracking performance for the full humanoid control.

I. INTRODUCTION

Humanoid robots locomoting and performing manipula-tion tasks on uneven ground are required to actively applyforces on objects and the floor in order to achieve theirtask successfully. A direct effect of contact forces is achange of momentum in the robot, which on the one hand isnecessary to move the center of mass, but on the other handrestricts the type of limb motion that the robot can perform.The non-linear nature of the angular momentum dynamicsmakes preview based control computationally hard in generaland even with an admissible angular momentum trajectoryopen control parameters like the joint motion and feedbackcontrol remain problematic for use in a whole body controlframework.

Successful applications of simplified momentum modelshave been shown on robots walking quite robustly over flatground. A common approach is to use the linear invertedpendulum model (LIPM), which has been exploited forpreview control since it was introduced by Kajita et. al [1].In [2] a model predictive control (MPC) approach is formu-lated that finds foot steps on a flat ground together with acompatible CoM location on a horizontal plane. CoM profilescan then be realized on the full body together with other limb

This research was mainly supported by the Max-Planck-Society and the EuropeanResearch Council (ERC) under the European Union’s Horizon 2020 research andinnovation programme (grant agreement No 637935). It was also supported by NationalScience Foundation grants ECS-0326095, IIS-0535282, IIS-1017134, CNS-0619937,IIS-0917318, CBET-0922784, EECS-0926052, CNS-0960061, the DARPA program onAutonomous Robotic Manipulation, the Army Research Office, the Okawa Foundationand the ATR Computational Neuroscience Laboratories.

1Autonomous Motion Department, Max Planck Institute for Intelligent Systems,Tubingen, Germany. first.lastname@tuebingen.mpg.de

2CLMC Lab, University of Southern California, Los Angeles, USA.

motion (e.g. swinging leg) controllers [3], [4] [5], [6]. Theassumption of a horizontal CoM motion and flat ground canbe relaxed to a pre-designed CoM height profile [7], [8].Although, less restrictive than the original LIPM, theseapproaches are either built for point feet or leave parts ofthe dynamics uncontrolled. Depending on the terrain, pre-defining the CoM along a fixed direction may result insuboptimal or infeasible reaction forces. Approaches thatleave the regime of linear models have been proposed, e.g.for long jumps [9] leading to more complex task behavior,but require task specific models that e.g. take into account theswing leg dynamics. Going even further, we have seen workthat optimizes over the whole joint trajectories and the fullmomentum together [10]. The impressive near-to physicalmotions, however, come at a high computational cost. Time-local controllers, have shown that balancing performanceof robots is improved, when overall angular momentum isdamped out directly [11], [12], however it has remainedunclear how angular momentum profiles should be chosenin non-static configurations.

In this paper we consider the full momentum dynamics ofthe robot for generation of CoM and momentum trajectoriesand the according reaction force profiles at each contact. Weformulate the problem as continuous time optimal controlproblem in a sequential form. A mode schedule is predefinedtogether with end effector trajectories. A desired angularmomentum trajectory, which is required for the limb mo-tion, is generated from a simple inverse kinematics forwardintegration and realized with admissible contact forces in aleast squares optimal sense. Since the kinematics informationis used before the optimization over the dynamics, ouroptimization procedure is relatively fast (e.g. compared to[10]). In our previous work [12] we have proposed a way tocompute control gains for the momentum task using a LQRdesign approach. We showed that it was able to significantlyimprove performance for balancing tasks on a real humanoid.However, this approach was only used for stabilization. Inthis paper, we extend the approach to receding horizontracking control. We use the planned trajectories to generatefeedback gains using a LQR design, where we linearize thenon-linear momentum dynamics around the optimized tra-jectory. In a simulated stepping task our humanoid traversesa terrain with changing height and tilted stepping stonessuccessfully. A whole-body controller finds joint torquesthat realize feedback loops on momentum as well as theswing leg motion and respect additional balancing, hardwareconstraints and generates physically realizable contact forces.A good tracking of overall momentum is achieved using ourreceding horizon LQR design.

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2015 IEEE-RAS International Conference on Humanoid Robots.Received July 7, 2015.

p2

p1

f2

f1, ⌧ 1h =

l

�

r

Fig. 1. A sketch of the model used through out this paper. It shows thecenter of mass (CoM) of the system and contacts with the environment. Flatcontacts that consist of a force, center of pressure and normal torque canbe modeled (e.g. a hand touching a wall) as well as point contacts. Thereis no restriction on the pose of contacts nor the CoM. Momenta generatedaround the CoM are not necessarily zero.

This paper is organized as follows. In Sec II we formulate anoptimal control problem to obtain momentum trajectories ofthe robot. Resulting trajectories are controlled with a LQRfeedback design as discussed in Sec III. It is incorporated intoa whole-body control approach as explained in Sec IV. InSec V we demonstrate our control framework on a simulatedstepping task on rough terrain. We discuss our results inSec VI and finish with a conclusion in Sec VII.

II. MOMENTUM TRAJECTORY OPTIMIZATION

In this section we describe how the robot momentum isplanned together with admissible contact forces. We will firstdiscuss the reduced dynamics model which will then be usedto phrase an optimal control problem to generate CoM andmomentum profiles. At the end of the section we describehow a desired angular momentum is chosen for optimization.

A. Dynamic Model

We reduce the state of a robot to its center of mass (CoM)r and overall momentum

h =

[lκ

], (1)

with linear l and angular κ momenta. The state of the systemis controlled by forces fi and torques τ i acting at points pi

on the mechanical structure. The momentum dynamics asillustrated in Fig 1, can be written

M r = l (2)

l = Mg +∑

fi (3)

κ =∑

τ i +∑

(pi − r)× fi, (4)

where M is the mass of the robot. The contact points pi

can for instance be point foot locations that are touching theground or they can represent the position of a handle that therobot holds on to. Flat feet can be modeled by several contactpoints on the foot surface. Equivalently, we can represent theeffective force at the center of pressure (CoP). Depending onthe type of contact, it will be necessary to express constraintson the forces. In the case of point feet, no torques τ i can

be generated. Torques that are generated at the CoP of a flatfoot are required to be normal to the foot surface. Further, ifwe assume stationary contact points, we need to restrict thewrench to remain in a friction cone.

B. Optimal control formulation

In the following we use the momentum dynamics of therobot to plan for admissible contact forces that generatedesired (linear and angular) momentum trajectories. Ourgoal is to compute force trajectories fi(t), τ i(t),pi(t) thatsatisfy the momentum dynamics in Eqs (2-4) and contactconstraints at all time. Also, they are required to steer themomentum through out desired states over a time horizonT . Given the initial state of the robot CoM and momentumr(0), l(0),κ(0), we can integrate Eqs (2-4) to obtain

l(t) =

∫ t

0

(∑

fi(δ) +Mg)dδ, (5)

l(t) = l0 +

∫ t

0

l(δ)dδ, (6)

r(t) = r0 +1

M

∫ t

0

l(δ)dδ, (7)

κ(t) =

∫ t

0

(∑

τ i(δ) + (8)∑

(pi(δ)− r(δ))× fi(δ))dδ,

κ(t) = κ0 +

∫ t

0

κ(δ)dδ (9)

Note that the states are expressed as (nested) functions of thereaction forces. They are constructed by integrating, sum-ming and applying the cross-product on fi(t), τ i(t),pi(t).Given a naive idea of what the desired CoM rdes(t) andmomentum hdes(t) should be (e.g. coming from an initialkinematic plan), we want to find contact forces that minimizethe error

J =∑T

t0( ‖l(ti)‖2W1

+ ‖l(ti)− ldes(ti)‖2W2+

‖r(ti)− rdes(ti)‖2W3+ ‖κ(ti)‖2W4

+

‖κ(ti)− κdes(ti)‖2W5), (10)

where we compute the errors at ti ∈ [0;T ] and Wi arediagonal weighting matrices that allow trade-offs betweenthe cost terms.

Adaptive end effector location: Through out the discussionin this paper we assume that the end effector remains at apredefined location. However, this assumption can be relaxedwithout making the optimization problem harder. For thatwe simply substitute pi = pi + pi, where pi is the footsole location that remains stationary through out a contactphase and pi is the (time-varying) center of pressure insideof the foot sole. In this reformulation both, pi and pi, willbe optimization variables and are chosen automatically. Thesupport planes still have to be decided before-hand, e.g. usinga dedicated acyclic contact planer [13], [14].

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2015 IEEE-RAS International Conference on Humanoid Robots.Received July 7, 2015.

C. Optimization procedure

We will now formulate the described problem into an opti-mization problem. First, the reaction forces are formulated asweighted basis functions, more specifically as polynomialsof the form

f(t;w) = α(t)

N−1∑

k=0

wktk = ΦT (t)w, (11)

α(t) =

{1 if in contact at t0 else

, (12)

with weights wi and basis functions tk summarized invectors Φ(t),w. We define mode-scheduling α(t), whichspecifies contact activation and deactivation and is set before-hand, e.g. it could be obtained by a higher level plan-ner.Polynomials have the advantage of generating smoothforce trajectories by construction. The forces can then bewritten as

f ji (t;wji ) = ΦT (t)wj

i , j ∈ x, y, z (13)τi(t;vi) = niΦ

T (t)vi (14)pji (t;u

ji ) = ΦT (t)uj

i , j ∈ x, y (15)

where the subscript identifies the end effector, the super-script represents the coordinates and n is the normal vector ofthe foot sole. Contact forces, torques and CoPs, now becomelinear functions of polynomial coefficients. Expressing thestates in Eqs (5-9) with Eqs (13-15) substituted, gives usthe states as functions of time and polynomial coefficients.The operations required to carry out the result boil down tomultiplication, summation and integration of polynomials,which can be computed analytically. As a result we phrasedour optimal control problem in sequential form, where ourcontrols evaluate directly to states and the dynamics equa-tions (cf. Eqs (2-3)) are implicitly incorporated. This allowsus to phrase our optimization problem

min.x

J(x) (16)

s.t. |pj(ti;x)| ≤ p, j = x, y (17)|τ(ti;x)| ≤ τ , (18)

0 ≤ −|f j(ti;x)|+ µfz(ti;x) ≤ fz, j = x, y (19)ti = t0 . . . T

where x is a concatenation of variables wji ,vi,u

ji . We try

to find polynomial coefficients x that minimize the error costJ , while satisfying friction and support bound constraintson forces. Eqs (17-18) bound the CoPs and torques. InEq (19) we impose a friction cone constraint approximated aspyramid and upper bound it by a sufficiently large value fz tokeep the polynomials from penetrating the lower bound andescaping above. In our constraints in Eqs (17-19), we wish tohave bounds that hold at all t ∈ [0, T ], in practice however,we express them at a finite number of time steps. Given the

limited flexibility of polynomials, we get minor penetrationof those constraints in intervals (ti; ti+1). Note that all ourconstraints are linear, whereas the objective function hasquadratic terms as well as higher order (non-convex) termsdue to the costs on angular momentum. Here we have thebenefit that all terms in the momentum dynamics can beincluded in the cost and none are left uncontrolled.

Receding Horizon: Given that the problem in Eq (16)is non-convex, we cannot expect to find a global optimumin general, but need to find a local optimum starting froman initial guess x0. In our approach we use a recedinghorizon technique. We start out with solving our problemfor a horizon T < T and obtain an optimal solution x∗

[0,T ].

Typically we start our desired motion such that it is easyto solve in the interval [0, T ], i.e. quasi static solutions arealready optimal. Then, we formulate our problem for theinterval [∆, T + ∆], where we initialize our optimizer withthe previous solution. This has the advantage that an initialsolution can be bootstrapped from an initial easier to solveconfiguration and then moved over the horizon to the difficultparts of the motion. Further as we push our implementationto more speed, we seek to use it in a receding horizon controlsetting.

Desired Angular Momentum: Most simplified momentummodels assume that angular momentum is desired to remainat zero as much as possible. This however ignores the factthat momentum might be required to move the robot limbs,e.g. in a stepping task we need to swing a leg, which in turngenerates a momentum around the hip. Thus, it is not trivialto decide what the desired angular momentum should looklike. In order to overcome this issue, we integrate forward ourdesired swing leg trajectories using inverse kinematics. Fromthe resulting joint motion we then compute rdes(t), κdes(t)(explained e.g. in [15]). Although, dynamically not feasible,this method generate angular momentum profiles requiredto perform the task imposed motion, e.g. swinging a leg.We iterate between kinematic forward integration of anoptimized r(t) and optimizing a desired κ with dynamicconstraints (by solving the problem in Eqs (16-19)). In ourexperiments two passes already lead to convergence. Withthis iteration we not only generate admissible force profilesthat obey the momentum dynamics equations, but also wecan bootstrap angular momentum trajectories which are notobvious to design otherwise. Note that neither CoM normomentum and neither components of the two are predefinedin advance. Instead an initial guess is given and the finaltrajectories are found automatically.

III. MOMENTUM LQR

In the previous section we described how reference tra-jectories in accordance to the momentum dynamics areobtained. In order to track those trajectories on the full robot,we propose a feedback law using a LQR design, which hasshown superior performance, compared to a naive PD gainapproach in experiments on the real robot [12]. From ourtrajectory optimizer we obtain admissible states and controls

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2015 IEEE-RAS International Conference on Humanoid Robots.Received July 7, 2015.

y∗ =

rlκ

,λ∗ =

...fiτ i

...

, (20)

y∗ = f(y∗,λ∗) =

1M l

Mg +∑

fi∑τ i +

∑(pi − r)× fi,

(21)

where we transform wrenches fi, τ i to the stationary polespi.The dynamics function in Eq (21) is discretized an linearizedaround the desired trajectories x∗,λ∗. The resulting timevarying linear dynamics are then used to formalize a finitehorizon LQR problem. This yields a control policy

λ = λ∗ + Kt(x− x∗) (22)

with time varying feedforward and feedback terms that maperrors in states into contact wrenches. Controlling the mo-mentum with this feedback law requires 6 DoF for wrenchesat each contact. Since our whole-body controller incorporatesadditional control objectives and force constraints, we com-pute directly the momentum rate that the wrenches generate

href =

[I3×3 03×3 . . .

[pi − r]× I3×3 . . .

]Kt(x− x∗) + h∗, (23)

where [�]× turns a cross-product into a matrix multiplica-tion. The resolution of momentum rate to contact wrenchesis then left to our whole-body controller as described in thenext section. In this LQR design a quadratic performancecost is set once and then optimal gains are computed ateach time step. As we discussed in our previous work, inorder to achieve compatible results with diagonal PD gainmatrices, we had to design different sets of gains, whereasthe LQR design requires one performance cost and generatessuitable gains at different contact configurations. Different toour previous work we linearize around desired trajectories,whereas in our robot experiments we used only two keyconfigurations, which may become limiting in more versatiletasks such as walking over a rough terrain.

IV. WHOLE-BODY CONTROL

The trajectories generated with the model in Eqs (2-4)define the CoM, momentum and end effector forces of ourhumanoid. In order to track these trajectories on the fullrobot, we need to generate joint torques accordingly andat the same time control the limb motion and guaranteethat other constraints are obeyed, e.g. joint limits. For thistime-local control problem, we use inverse dynamics in QPCascades, which we applied successfully on the real robotbefore [6]. It allows us to phrase feedback controllers andconstraints as functions of joint accelerations q, externalgeneralized forces λ and joint torques τ . For instance, wecan express a cartesian controller on the swing foot as affine

Fig. 2. The humanoid robot traversing a terrain with stepping stones ofdifferent hight and orientation.

function of joint accelerations or a momentum controller asa function of reaction forces. The latter e.g. would be

[I3×3 03×3 . . .

[pi − r]× I3×3 . . .

]λ +

[Mg0

]= href . (24)

A reference momentum rate href can be chosen from aLQR design as demonstrated in our previous work [12] andextended in Sec III. Given a set of controllers and tasks,we find torques that satisfy the dynamics equations of thefull robot and at the same time generate the desired taskfeedback as good as possible. Tasks, however, may conflict,e.g. moving the CoM may require moving the swing legand vice versa. In these cases, QP cascade allow two typesof trade-offs, where we can either weigh tasks against eachother or we can prioritize them strictly. Especially, whenit comes to trade-offs between constraints, tasks of interestand redundancy resolution, prioritization can facilitate thecontrol design.

V. SIMULATION RESULTS

This section describes our simulation results of the pro-posed control framework. We use a model of our Sarcoshumanoid in the SL simulation environment. Contacts aresimulated with a penalty method and stiff springs. Allexperiments are performed on a 2.7 GHz intel i7 processorwith 16gb ram. A task is generated where the robot is to walkon stepping stones that increase in height from one step to theother as visualized in Fig 2 and shown in the attached video1.The z-axis of the inertial frame points up and the robot walksalong the y-axis to the front. Two out of the four steps aretilted by 25o. Both, the change in CoM height as well as thetilted support break the assumptions made in LIPM modelsand require to consider the two contact wrenches in separate.We generate swing leg trajectories using cubic splines on thepose of the foot. The humanoid starts out in double supportat rest. After the first 3 seconds the left foot breaks contactand moves on a tilted stone located to the front left-handside of the robot at a lifted height (as shown in Fig 2). Itfollows a contact switch after each second changing fromsingle- to double support or vice versa. The second step isagain a stone tilted inwards to the robot and located to thefront right-hand side. Finally, the robot takes two steps onto

1An extended version of the video is available on www-amd.is.tuebingen.mpg.de/˜herzog/15_07-Humanoids.mp4

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2015 IEEE-RAS International Conference on Humanoid Robots.Received July 7, 2015.

Rank Nr. of eq/ineq con-straints

Constraint/Task

1 6 eq Newton Euler Equation2 2× 6 eq Contact constraints

2× 4 ineq Center of Presure2× 4 ineq Friction cone2× 14 ineq joint acceleration limits

3 6 eq LQR momentum control6 eq Cartesian swing foot control14 + 6 eq PD control on posture

4 2× 6 eq Contact forces control5 3 eq Base link orientation

TABLE IHIERARCHY FOR TASKS IN THE STEPPING EXPERIMENT

a horizontal plateau located slightly below knee height.The planner is initialized with a naive idea of the robot mo-tion, where we simply keep the base at a certain height abovethe feet. After integrating the desired swing foot positionstogether with the base height using inverse kinematics (asdescribed in Sec II-C), we obtain resulting rdes(t),κdes(t)trajectories that take into account the angular momentumrequired to swing the legs from one stepping stone to theother. The inverse kinematics solution, which is physicallynot consistent, is then adjusted in the trajectory optimizationstep to be admissible with constrained forces and CoPs. Noneof the force constraints were notably violated. As can be seenin Fig 3 the inverse kinematics generated CoM trajectoryis modified significantly in the lateral direction and as aconsequence the angular momentum in y-direction as well.After a second iteration of inverse kinematics integration andtrajectory optimization the results have sufficiently convergedand we stop. With polynomials of order 3 the control problemhad a total of 72 variables. The planning process took 4min and converged after only two iterations. This allows usto generate a complex motion rather quickly compared tomotion planners that are based on more extensive models ofthe robot. The numerical optimization problem in Eq 16 issolved with SNOPT, a Sequential Quadratic Programmingmethod.

Next, we construct a hierarchy of feedback controllersand constraints in order to realize the momentum profileon the full humanoid. At the highest priority we expressthe physical model of the full robot to obtain physicallyconsistent torques. It follow force and joint limits togetherwith contact constraints. In the third priority we control theswing leg motion and the momentum and add a posturePD control with a relatively low weight. In the prioritiesbelow we regularize end effector forces and stabilize the baseorientation. A summary of the task setup is given in Tab I.The momentum is controlled with feedback gains designedas described in Sec III, where our performance cost is fixedfor the whole run. We penalize state errors by 10, forcesby 0.1 and torques by 0.5. Gains are generated over a 2sechorizon with a granularity of 200 time steps. A sequence ofgains is recomputed every 10ms.The robot was able to traverse the terrain successfully. Themomentum trajectories, which were both, dynamically con-

18.6

11.2

3.8

3.6

X

CoM Position [cm]

optimizedIK 2IK 1

33.4

55.6

77.8

Y

2 3 4 5 6 7 8 9 10

22.6

14.2

5.8

2.6

Z

6

4

2

0

2

4

6

X

Angular Momentum [Nms]

optimizedIK 2IK 1

2 3 4 5 6 7 8 9 106

4

2

0

2

4

6

Y

Fig. 3. Plans for the CoM and horizontal angular momentum. The resultinglateral CoM (top plot) was adapted quite significantly over the planningprocess to make it physically compatible with the reaction forces. Angularmomenta are found that allow for stepping motions required to traverse theterrain.

sistent as well as compatible with the robots limb motions,could be tracked well as shown in Figs 6 5. Torques weregenerated that are in the bounds of the physical robot. Inthe beginning of the task the planned CoM height forcesthe knees to stretch, which prevents the robot from using itsknees to lift the CoM further, but instead it lifts the arms uprapidly. This can be avoided by adding a box constraint onthe CoM in the optimization problem in order to considerkinematic limitations.The LQR gain matrices have non-zero off-diagonal valuesas expected (cf. Fig 4). For instance we can see that angularmomentum is generated in order to correct for errors in CoMand linear Momentum, which would not be possible withdiagonal PD gains. In fact we tried to track the plannedmotion using diagonal PD gains. We started with valuessimilar to the diagonal of the LQR gain, but we could notfind parameters that were stable through out several contactsituations as it was the case for the LQR gains. Increasing theterrain difficulty was mainly problematic due to kinematiclimits, because our naive swing leg trajectories required tokeep the heel on the ground during the whole support phasethus limiting the stepping height. Further, we saw problemswith the simulators contact model, which caused sporadicspikes in the force profile when we applied strong forces onthe ground.

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2015 IEEE-RAS International Conference on Humanoid Robots.Received July 7, 2015.

(a) Momentum LQR Gain, Eq (23) (b) Force LQR Gain, Eq (22)

k@ l/@rkk@/@rkk@ l/@lkk@/@lkk@ l/@kk@/@k

Fig. 4. Here we show a heat map visualization of the momentum gain(top left) and the force gain (top right). The top 6 rows in b) correspond tothe wrench of the right foot, wheras the bottom rows correspond to the leftfoot. As we can see the gains contain off-diagonal terms leading to couplingterms between linear and angular momentum. These terms are ignored in anaive diagonal PD gain design. The bottom plot shows the norm of 3x3 sub-blocks of the momentum gain plotted over time. The vertical dashed lines att = 3 and t = 4 indicate contact switches. Gain profiles change significantlyover time and contact configurations. The gains and momentum dynamicsare discontinuous at contact switches. Nevertheless, discontinuities in jointtorques were negligible.

Overall, a complex task could be planned quickly includinga non trivial angular momentum profile that respected theend effector motion. The proposed feedback control lawon the momentum showed good performance when it wasembedded in a inverse dynamics task hierarchy.

VI. DISCUSSION

Relation to simplified Momentum Dynamics: The momen-tum model in Eqs (2-4) is often simplified further in order toobtain linear dynamics, which then leads to computationallymore efficient algorithms. However, turning multiplicativeterms between variables in Eq (4) into linear terms requirespotentially restrictive assumptions. E.g. in the LIPM the CoMheight rz is assumed to be constant, there is one effective pi,which lies in one horizontal plane, and the force is requiredto act along r− p.Since a constant CoM height may be limiting for tasksthat require to move vertically, the authors in [8] allow apredefined (not constant) rz(t). Substituting this assumptioninto Eq (4), and assuming constant pi, turns the horizontalangular momentum into a linear function of forces andtorques. Assuming in addition, that the non-linear vertical an-gular momentum can be neglected, the authors end up againwith a linear dynamics. If we restrict our dynamics model

201510

505

1015

X

Lin. Momentum [Ns]

432101234

Ang. Momentum [Nms]

measureddesired

202468

1012

Y

432101234

2 3 4 5 6 7 8 9 10time [s]

201510

505

101520

Z

2 3 4 5 6 7 8 9 10time [s]

1.5

1.0

0.5

0.0

0.5

1.0

1.5

Fig. 5. Linear and angular momentum are tracked well as the robot walksover up the terrain. The oscillations in the angular momentum at t=2 comefrom rapid arm motions, when the robot was trying to move the CoM upand the knees were stretched. This can be avoided, e.g. by adding boxconstraints on the CoM in the trajectory optimization step to account forkinematic limits.

0.250.200.150.100.050.000.050.10

X

CoM [m]

measureddesired

0.10.20.30.40.50.60.70.8

Y

2 3 4 5 6 7 8 9 10time [s]

0.300.250.200.150.100.050.000.05

Z

Fig. 6. Measured and desired CoM of the robot, when it was traversingthe terrain. As the plots show, good tracking performance can be achievedwith the proposed LQR design on the momentum.

further into a LIPM and use discretized dynamics (instead ofpolynomial trajectories) we can recover the approach of [2].However, this can be restrictive, if we want to step on variousslopes or when a non-zero angular momentum is required,e.g. to consider limb motion.

Computation time: In our current work, we focused onthe capabilities of the multi-contact dynamics model togenerate dynamic motions on uneven terrain and we showedthat these behaviors can be controlled on a full humanoidrobot. However, the goal of separating the control processinto a predictive control generation on a lower dimensionalmodel and time-local control on the full dynamics, has thepotential for a fast implementation in a MPC fashion. In ourexperiments we saw potential drawbacks in our numericaloptimization in Eq (16). Increasing the order of polynomials,leads to slower convergence rates, whereas reducing thepolynomial dimensions too far may lead to poor flexibilityof the trajectory representation. This may be explained bythe discrepancy of basis functions evaluated close to 0 and

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2015 IEEE-RAS International Conference on Humanoid Robots.Received July 7, 2015.

close to T leading to s poorly conditioned problem. Piecewiseconstant trajectories may overcome this problem. Anotherpoint for improvement of the numerical procedure is theformulation of the optimal control problem. In the currentformulation, the objective function in Eq (10) has terms upto 4th order (squared norm of cross products). Our numericalsolver is based on approximations up to second order, whichmay limit the region, in which approximations are valid. Itis possible (but out of the scope of this paper) to rewritethe problem into a (non-convex) Quadratically ConstrainedQuadratic Program, where the objective function as well asthe constraints are quadratic, leading to a better approxi-mation in second order methods and potentially improveconvergence. Pushing the implementation towards an onlinecontrol algorithm is part of our future work.

VII. CONCLUSIONS

We presented an approach to control contact forces andmomentum for humanoid robots. CoM and momentum pro-files were obtained in an optimal control framework togetherwith admissible contact forces. Feedback gains are generatedfrom a LQR design, which generates time and contact-configuration dependent gains from a single performancecost. The resulting controller is embedded in an inversedynamics based whole-body controller together with otherlimb controllers and constraints. We demonstrated the controlframework on a simulation of the Sarcos humanoid traversingrough terrain. Physically admissible momentum and forcetrajectories could be found relatively quickly and weretracked well during the task execution.In future work, we will implement the discussed speed-upand push the optimal controller to run online.

REFERENCES

[1] S. Kajita, F. Kanehiro, and K. Kaneko, “Biped walking patterngeneration by using preview control of zero-moment point,” in ICRA,2003.

[2] A. Herdt, N. Perrin, and P.-B. Wieber, “Walking without thinking aboutit,” in IROS, 2010, pp. 190–195.

[3] A. Sherikov, D. Dimitrov, and P.-B. Wieber, “Whole body motioncontroller with long-term balance constraints,” IROS, 2014.

[4] S. Feng, X. Xinjilefu, W. Huang, and C. Atkeson, “3d walking basedon online optimization,” in Humanoids, 2013.

[5] S. Faraji, S. Pouya, and A. Ijspeert, “Robust and Agile 3D Biped Walk-ing With Steering Capability Using a Footstep Predictive Approach,”in R:SS, Berkeley, USA, July 2014.

[6] A. Herzog, L. Righetti, F. Grimminger, P. Pastor, and S. Schaal, “Bal-ancing experiments on a torque-controlled humanoid with hierarchicalinverse dynamics,” in IROS, 2014.

[7] J. Englsberger, C. Ott, and A. Albu-Schaffer, “Three-DimensionalBipedal Walking Control Based on Divergent Component of Motion,”IEEE Transactions on Robotics, vol. 31, no. 2, pp. 355–368, 2015.

[8] H. Audren, J. Vaillant, A. Kheddar, A. Escande, K. Kaneko,and E. Yoshida, “Model preview control in multi-contact motion-application to a humanoid robot,” in IROS, 2014, pp. 4030–4035.

[9] P. M. Wensing and D. E. Orin, “Development of high-span runninglong jumps for humanoids,” in ICRA, 2014, pp. 222–227.

[10] H. Dai, A. Valenzuela, and R. Tedrake, “Whole-body Motion Planningwith Simple Dynamics and Full Kinematics,” Humanoids, 2014.

[11] S.-H. Lee and A. Goswami, “A momentum-based balance controllerfor humanoid robots on non-level and non-stationary ground,” Au-tonomous Robots, vol. 33, pp. 399–414, 2012.

[12] A. Herzog, N. Rotella, S. Mason, F. Grimminger, S. Schaal, andL. Righetti, “Momentum Control with Hierarchical Inverse Dynamicson a Torque-Controlled Humanoid,” Autonomous Robots, (acceptedfor publication). [Online]. Available: http://arxiv.org/abs/1305.2042v1

[13] T. Bretl, S. Rock, J. Latombe, B. Kennedy, and H. Aghazarian, “Free-climbing with a multi-use robot,” in International Symposium onExperimental Robotics ISER, 2004.

[14] S. Tonneau, N. Mansard, C. Park, D. Manocha, F. Multon, andJ. Pettre, “A reachability-based planner for sequences of acycliccontacts in cluttered environments,” in International Symposium onRobotics Research ISRR, 2015.

[15] D. E. Orin and A. Goswami, “Centroidal Momentum Matrix of ahumanoid robot: Structure and Properties,” in IROS, 2008.

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2015 IEEE-RAS International Conference on Humanoid Robots.Received July 7, 2015.