Momentum trajectory generation and control for multi-contactinteraction

Alexander Herzog1, Nicholas Rotella2, Sean Mason2, Stefan Schaal1,2 and Ludovic Righetti1

I. INTRODUCTION

Legged robots are expected to locomote autonomously inan uncertain and potentially dynamically changing environ-ment. Active interaction with contacts becomes inevitable tomove and apply forces in a goal directed way and withstandunpredicted changes in the environment. Therefore, we needto design algorithms that exploit interaction forces andgenerate desired motions of the robot leading to robust andcompliant interaction with the environment. As complexityof tasks increases, generating motion trajectories quickly incombination with feedback control laws that can be executedon the full robot will become increasingly more importantand open the way for agile robots.Dynamic motions that require contact interaction with severalendeffectors at the same time need to be planned over a timehorizon in order to result in trajectories that are realizableon the robot. However, planning with the full robot modelcan result in excessive computation time, even though only asubset of the dynamics might be required to model the robotbehavior well. Execution of planned trajectories, on the otherhand, has been shown to work well when inverse dynamicsis used under consideration of the full robot dynamics in fastcontrol loops [1]. As a consequence, in this work we addressthe problem of dynamic motion generation and control byseparating it into two different time-scales and levels ofmodeling granularities: We use trajectory optimization onreduced robot dynamics and generate motion plans over ahorizon in a few minutes of optimization. The resultingtrajectories are then tracked within a Hierarchical InverseDynamics whole-body controller together with other tasksand constraints in a real-time control loop on the kHz level.

Trajectory Optimization

A direct effect of contact forces is a change of (linear andangular) momentum in the robot. This relationship betweenforces and momentum is expressed in the Newton-Eulerequations and lies at the core of the equations of motionof each floating base robot. The nonlinear relation is oftensimplified further in related work in order to obtain lineardynamics. The probably most widely used example is thelinear inverted pendulum model (LIPM) [2]. However, over

This research was mainly supported by the Max-Planck-Society, Max PlanckCenter for Learning Systems and the European Research Council (ERC) under theEuropean Union’s Horizon 2020 research and innovation programme (grant agreementNo 637935). It was also supported by National Science Foundation grants ECS-0326095, IIS-0535282, IIS-1017134, CNS-0619937, IIS-0917318, CBET-0922784,EECS-0926052, CNS-0960061, the DARPA program on Autonomous Robotic Manip-ulation, the Army Research Office, the Okawa Foundation and the ATR ComputationalNeuroscience 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.

(a) the humanoid traversing a terrain as it is tracking planned momentum trajectories

(b) balance experiments under consideration of the full dynamics model

Fig. 1. Our motion generation framework is demonstrated on a roughterrain walking task as illustrated in (a). The underlying whole-bodycontroller is further tested in various balancing scenarios when pushes areapplied to the robot and its’ ground support (b).

simplifying the dynamics can become problematic in thecontext of more dynamic tasks such as stepping at varyingheight. Thus, in our work we exploit the full relation be-tween contact forces, center of mass (CoM) and (centroidal)momentum. Our dynamics model allows for separate contactwrenches at arbitrary oriented contact surfaces or points. Wephrase an optimal control problem that generates momen-tum profiles and constrained contact forces and centers ofpressure (CoPs). Compared to more expressive models [3],we can generate admissible force and momentum profilesrelatively quickly. In addition, an optimal feedback controlleris designed around momentum trajectories from a LQRdesign to be realized in our whole-body controller togetherwith other tasks and constraints.

Hierarchical Inverse Dynamics

Interactions between a robot and an uncertain environment(e.g. stepping on unsteady ground) require compliant control,which can be achieved well with inverse dynamics on torquecontrolled robots. Extensions to hierarchical inverse dynam-ics allow to construct more complex behaviors consistingof force and motion tasks satisfying physical constraintsand actuator limitations that are consistent with the fulldynamics model of the robot. This class of algorithms hasbeen shown to create physical motions in simulation whereperfect knowledge about the robot model and state is givenand real-time requirements can be relaxed. However, theconstraint-aware feedback control capabilities of hierarchical

inverse dynamics controller have not been verified on a realhumanoid before. In face of unpredicted events like a pushor unsteady ground, constraints, such as support limits, haveto be guaranteed by resolving hierarchies of tasks with thefull model online. A prerequisite for such behavior is therealization of hierarchical inverse dynamics controllers thattake into account inequality constraints in a fast controlcycle. Although, computationally demanding, we implementa variant of cascades of Quadratic Programs (QPs) to runsufficiently fast and solve a hierarchy of tasks in a tight 1kHzcontrol-loop on a torque controlled humanoid [1] leading toconstraint-aware multi-task control.In our robot experiments, we observe robust balancing be-havior when we control the momentum of the system leadingto the momentum centered control framework presented inthis paper: a combination of momentum trajectory optimiza-tion together with hierarchical inverse dynamics whole-bodycontrol [4]. We present an optimizer that creates momentumtrajectories that are tracked with LQR feedback gains insidea hierarchy of tasks. We show simulation results wherethe robot traverses a terrain consisting of stepping stonesvarying in height and orientation (cf. Fig 1). The discussionof the presented work focuses on planning and control ofmomentum, however as we move towards robot experimentswith the full framework, we also address estimation ofmomentum [5].

II. MOMENTUM TRAJECTORY GENERATION

Our model of the robot is reduced to its momentumdynamics

M r = l (1a)

l = Mg +∑

fi (1b)

κ =∑

τ i +∑

(pi − r)× fi, (1c)

where M is the mass of the robot and the state of thedynamics is composed from the CoM r and the overalllinear and angular momentum l,κ. A change of momentumis generated through the controls composed from contactforces fi, torques τ i acting at CoPs pi. We are interestedin momentum trajectories x(t) =

[r l κ

]Tand contact

wrenches u(t) =[. . . fi τ i . . .

]that are consistent with

Eqs (1). They are formulated as the solution of an optimalcontrol problem

min.x,u

T∑t

Jt(x(t),u(t)) (2a)

s.t. Eqs (1) satisfied, (2b)u(t) ∈ K, (2c)

where J is a cost on the CoM and momentum andK is the space of admissible contact forces, i.e. Eq (2c)guarantees that contact forces are inside of the friction coneand CoPs are bounded by the support polygon. As we solvethe optimization problem (2) we obtain a (locally) optimalmomentum trajectory that satisfies the dynamics equations(1) and admissible contact forces. The objective function

of our optimal control problem is a sum of tracking errorsbetween r, l,κ and corresponding desired reference CoM andmomentum trajectories. Desired trajectories are not requiredto be dynamically feasible and can be created from a naivekinematic plan, for instance using inverse kinematics. Ouroptimizer then finds admissible forces, torques and CoPswith compatible CoM and momentum that are as closeas possible to the reference trajectory. In our experiments,we could execute resulting trajectories on the full robot insimulation even with naive initialization of the optimizer. Inour formulation, x(t),u(t) are expressed in form of time-continuous polynomials guaranteeing smooth trajectories byconstruction. Solutions are found quickly after aprox. 5 min.

A. LQR feedback designOnce optimization (2) converged to a solution, we com-

pute feedback gains around the resulting trajectories. Wecompare a naive PD design with diagonal gain matrices to aLQR design that computes optimal gains automatically froma quadratic performance cost. A desired closed-loop behavioron the change of momentum is typically written [6] in formof a spring-damper. There are, however, several issues withsuch an approach. First, the tuning of the PD controllercan be problematic. In our experience, on the real robotit is necessary to have different gains for different contactconfigurations to ensure proper tracking which leads to atime consuming tuning process with many open parameters.Second, such a controller does not exploit the coupling be-tween linear and angular momentum rate of change throughinteraction forces (cf. Eq (1c)). We propose to use the modelof Eqs (1) to phrase a LQR problem and compute optimalfeedback gains. We linearize the dynamics and compute aLQR controller by selecting a desired performance cost. Acontrol law is computed with the form[

fankleτ ankle

]= −K

rlκ

+ k(xref , lref ,κref ) (3)

that contains both feedback and feedforward terms. A desiredclosed-loop behavior for the momentum that appropriatelytakes into account the momentum couplings (1c) is thencomputed. In our experiments we demonstrate that such adesign leads to better performance than a naive PD controldesign. Moreover, gains are computed automatically for anycontact configuration, significantly simplifying the imple-mentation of the controller. When applied to a more dynamicstepping task in simulation, the advantage of the LQR designwas necessary for stable execution.

III. EXPERIMENTS

Our control framework consisting of trajectory generationand whole-body control is demonstrated in a stepping task insimulation. Additionally, we perform balancing experimentswith our Hierarchical Inverse Dynamics controller on thereal robot. We implement the momentum control task in aQP cascade variant [?] and run the balance controller in a1kHz control loop on the lower part of our Sarcos humanoid1

1A video summarizing the experiments can be found onhttp://www.youtube.com/watch?v=jMj3Uv2Q8Xg

Fig. 2. The robot state, when it was balancing in single support andperforming a swing leg motion. The grey bar marks the moment of pushthat was applied at the hip. Position and velocity tracking (top two plots) ofthe swinging hip joint remains unaffected, while the momentum (3rd plotfrom top) is kept bounded and damped out shortly.

(see Fig 1). Physical constraints such as consistency with therobot dynamics and contact force limitations are put in higherpriority whereas posture control and force regularization isput in lower priority. Various kinds of disturbances wereapplied to the robot. It was put on a rolling platform andtilting platform and balanced out rapid displacement of thesupport as can be seen in the video. We excerpt a sequenceof systematic pushes of up to 9Ns at different points onthe robot structure and compare the disturbance rejection ofthe PD control to the LQR feedback design (as discussedin Sec II-A). For both momentum control tasks, the robotwas able to withstand impacts with high peak forces andstrong impulses without falling. For every push, the changein momentum was damped out quickly and the CoG wastracked after an initial disturbance. With the LQR gains wesee a significant improvement in recovering the CoG eventhough the robot was pushed harder than with the controllerusing diagonal gain matrices.In an additional experiment, we made the robot move onone foot, while it was performing a swing motion with theother leg. The swing leg task was put in the same priority asthe momentum control task. Not only was the robot able totrack the swing leg motion, but it was also able to balance outstrong shocks of peak forces up to 290N . Fig. 2 shows theswing leg tracking and momentum of the robot at the momentof impact. Here, we can see the advantage of realizing severaltask feedback loops simultaneously. Due to the momentumcontrol the injected disturbance momentum remains boundedand is damped out eventually while at the same time trackingof the swing leg is not visibly affected. The reaction forcesand CoPs stay inside their bounds at all times and are nevertraded-off due to the higher priority.

We now move to a more dynamic task where motiongeneration with the momentum dynamics becomes inevitabletogether with a tracking controller that considers the fulldynamics model. A rough terrain walking task is designed

with a predefined stepping pattern as can be seen in Fig 1.The robot is to walk on angled stepping stones that areincreasing in height. In this setup assumptions required forthe LIPM do not hold anymore, but instead independentCoPs and contact forces need to be taken into account aswell as a changing CoM height. Our trajectory generator(2) converges after aprox. 5 min with a naive initializationand returns momentum trajectories together with constrainedcontact wrenches that lead to successful execution in thewhole body. Our LQR performance cost consists of diagonalgain matrices that are chosen once and resolve to timevarying optimal feedback gains as we linearize the plannedtrajectories. Using a PD gain design, we could not findparameters that would lead to a stable behavior over the fullhorizon. The resulting momentum controller was embeddedin a hierarchy of tasks. At the highest two priorities we putthe Newton-Euler equations of the robot and constraints oncontact forces and joint limits. In the third priority we putour momentum task together with a swing foot controllerand a posture controller with relatively small weight. In theremaining two priorities redundancies on the base orientationand contact forces are resolved. We observe that the robotis able to track the planned momentum well and managesto traverse the terrain. The computed LQR feedback gainscontain off-diagonal gains that create for instance a changein angular momentum in order to correct for CoM trackingerrors. Our QP cascade is able to realize desired closed-loopbehaviors sufficiently well while constraints on forces andjoint limits are satisfied.

IV. CONCLUSION

In this work we present a framework for motion generationand control for multi-contact interaction. We combine atrajectory generator that considers the momentum dynamicsof the robot together with a whole-body controller basedon hierarchical inverse dynamics. An optimal LQR feedbackcontroller is generated that tracks optimized momentumprofiles on the full robot. Experiments are show on the realrobot that demonstrate robust behavior of the whole-bodycontroller. We then demonstrate the combination of trajectoryoptimizer and whole-body controller on a rough terrain insimulation which the robot is able to cross using the proposedmethod.

REFERENCES

[1] A. Herzog, N. Rotella, S. Mason, F. Grimminger, S. Schaal, andL. Righetti, “Momentum Control with Hierarchical Inverse Dynamicson a Torque-Controlled Humanoid,” Autonomous Robots, 2015.

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

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

[4] A. Herzog, N. Rotella, S. Schaal, and L. Righetti, “Trajectory generationfor multi-contact momentum-control,” in Humanoids, 2015.

[5] N. Rotella, A. Herzog, S. Schaal, and L. Righetti, “Proceedings of the2015 IEEE-RAS International Conference on Humanoid Robots,” inHumanoids, 2015.

[6] S.-H. Lee and A. Goswami, “A momentum-based balance controller forhumanoid robots on non-level and non-stationary ground,” AutonomousRobots, vol. 33, pp. 399–414, 2012.