1

Full Dynamics LQR Control With Multi Contact Phases For BipedalWalking

Sean Mason1, Stefan Schaal1,2 and Ludovic Righetti21 University of Southern California, USA

sean.mason@usc.edu2 Max Planck Institute, Tuebingen, Germany

ludovic.righetti@tuebingen.mpg.de , sschaal@usc.edu

I. INTRODUCTION

Humanoid robots are expected to both locomote andinteract with objects within unstructured environments. Akey component to realizing this goal is the ability for ouralgorithms to handle multiple contact switching scenarioswithin a whole-body control framework. To complement themany advances in trajectory optimization we must recognizewhich tools work best to robustly track these trajectorieswhile providing a well-defined disturbance rejection behav-ior. Over recent years, optimal control strategies have showedvery promising results in simulation and on real systemsfor torque controlled humanoids that use operational-spacetechniques to achieve whole-body manipulation and loco-motion. Previous work, [5], [12], [1], [11], [2], has utilizedQuadratic Programs (QPs) that optimize over a variety ofconstraints (e.g. dynamic consistency, joint tracking, frictioncones, etc.) in order to compute joint torques. Trajectories areoften planned using operational-space techniques and thenconverted to joint torques using QPs, achieving whole-bodymanipulation. QPs can further be organized into hierarchiesto solve whole-body optimal control problems such that thereis a set priority in goals that the robot should achieve andtasks of higher priorities will always be achieved first [4].Unfortunately, coupled with the growing flexibility of thesemethods there is also the added computational overhead,complexity in tuning, and a lack of theoretical disturbancerejection metrics, such as the gain and phase margin seenin classical control, that prevent these algorithms from beingcompared with one another.

Currently, it remains unclear what level of complexityin controllers is needed on real systems. Real systemshave problems such as model error, sensor noise, actuatorsaturations, backlash, stiction, and imperfect state estimation.Because of theses issues, the advances seen by algorithms insimulation do not always transfer to real systems. To addressthis unknown, we propose testing simpler optimal controlstrategies that still offer whole-body control and can handlemultiple contact constraints. We are interested in studyingto what extent contact-consistent linearized dynamic modelscan be used for the control of whole-body behaviors on realrobots. In particular, we are interested in understanding howthe performance compares with more advanced operational-space control approaches on real robots. Previously in [7],

Fig. 1. Balancing experiment conducted on the hydraulic torque controlledSarcos humanoid. The weighted torso tracked a sine trajectory while thelower body balanced using a LQR controller with multiple foot constraints.

we derived a linear quadratic regulator (LQR) making useof the linearized full robot dynamics which are consistentwith the contact constraints. It is well-known that LQRcontrollers provide very good gain and phase margins andby extension a certain tolerance to non-linearities. With morecomplex optimization-based algorithms, it becomes difficultto provide similar stability analyses. The advantage of theconstrained LQR controller is that it explicitly takes intoaccount the coupling between the different joints to createoptimal feedback controllers. To address the complexity oftuning this high-dimensional controller in joint space, wehave shown in [7] how this framework also allows costs onoperational-space quantities (i.e. end effector tracking, centerof mass (CoM) tracking, and momentum) to be embeddedinto the total cost function. We do this through a change ofvariable in states and rewrite the LQR cost function to bein terms of these new variables. Experiments on a torque-controlled humanoid robot (Fig. 1) have demonstrated thatour computationally light weight control policy had push-recovery and tracking performances competitive with moresophisticated balance controllers [11], [2], [8], [6] rejectingimpulses up to 11.7 Ns with peak forces of 650 N. Wehave extended the original approach [7] to include contactswitching and re-linearization of the dynamics for differentcontact situations. Our recent results show that our approachcan be extended to more complex scenarios such as walkingas detailed in the following section.

2

II. INFINITE HORIZION LQR USING A SINGLELINEARIZATION

Previously in [7], we conducted balancing experimentsby using a single linearization and static gains provided byan infinite horizon control problem. We derived a contact-consistent LQR formulation using the linearized model of thefull dynamics, similar to [13]. The robot was pushed with arigid rod instrumented with an ATI force/torque sensor, fromwhich the maximum perturbation impulse was computed.We have since extended this approach to a decoupled tasksituation. It is often the case that the upper body of therobot is interested in a task that can be considered decoupledfrom the lower body (i.e. working at a table) and the lowerbody should stabilize the full system. In one task, the CoMis stabilized to keep balance and the other task consists intracking a fast torso motion. These two operational-spacetasks are embedded in the cost used to derive the LQRcontroller. Fig. 1 shows the lower body balancing while thetorso (equipped with weights to simulate the upper bodymass) tracked sinusoidal trajectories. The resulting upperbody motion simulated forces which may be experiencedduring a whole-body manipulation task. With just the simpleLQR feedback the robot was able to balance while theweighted torso moved at frequencies between 0-1.5 Hz. Notethat just performing the same task using PD control for eachjoint would lead to a fall.

Fig. 2. Snap shots of the robot walking using 5 key linearization posesand changing contact constraints.

III. TIME VARYING DYNAMICS AND CONSTRAINTS

We have extended our approach to more complex scenar-ios such as walking. Rather than using a single linearizationas in the balancing experiments, we linearize the full robotdynamics around key contact poses such as those shownin Fig. 5. One of our assumptions is that only a small setof linear models is necessary to control many interestingtasks. By using a small set of linearized models we areable to compute efficient controllers using LQR design

ZMPTrajectoryGenerator

Robot+

- +

+

GravityCompensationw/ Contact ForceOptimization

LQR FeedbackPolicy

Fig. 3. Block diagram depicting the control architecture used in the walkingexperiments. θ is the joint and base state, τ f f is the feed-forward torque,and Rc , Lc denote the time varying contact trajectories for the right andleft foot.

0 5 10 15 200.000.010.020.030.040.050.060.070.08

COM

Error(m

)

ZMPCOMDesired COM

0 5 10 15 200.10

0.05

0.00

0.05

0.10

Base

Error(m

)

XYZ

0 5 10 15 200.800

0.795

0.790

0.785

0.780

0.775

LeftFo

otZ(m

)

Foot ZDesired Foot Z

0 5 10 15 20

Time (s)

0.0

0.2

0.4

0.6

0.8

1.0

FootConstraints Right

Left

Left Foot Step

Fig. 4. Plot of the robot taking a single step. In this plot there ismultiple LQR gain transitions and contact switching. Tracking of the CoMis shown along the frontal plane with the ZMP plan showed for reference.Additionally, the base error, the foot step z direction (upwards), and thecontact switching plan are shown.

methods. In Fig. 5 we show the different feedback gainmatrices resulting from linearizations around different posesand contact configurations. This plot highlights the synergiesamong the joints that are used to regulate the CoM motionfor different contact situations. The plot also provides insightinto the tracking strategies used by the LQR controller (i.e.which joints react to error in which states), an analysis toolthat is not available by many other optimization methods.While any walking trajectory generation method would beappropriate, we started by considering the popular zeromoment point (ZMP) walk modeled after Kajita’s ZMP walkwith preview control [3]. Using the resulting CoM trajectorywith predefined foot step trajectories, we generate desiredjoint space trajectories using inverse kinematics and feed-forward torques provided by gravity compensation. While inthe double support phase torque redundancy can be exploitedto optimize contact forces. The gravity compensation termuses this redundancy to generate task-consistent interactionforces [10]. We found that this step is crucial in order to moreclosely realize the desired ZMP trajectory. An overview ofthe control scheme is shown in Fig. 3.

Our experiments have demonstrated very good perfor-mance in simulation and preliminary experiments on thereal robot demonstrate the ability of this control architectureto handle stabilization over multiple steps. Fig. 2 shows asequence of the robot walking and Fig. 4 show the plot

3

Fig. 5. Gain matrices produced from key poses and constraint conditions. From these gain matrices, one can gain intuition about the relationship betweenerror in states and the torques that the optimal controller produces. This type of analysis is not available in other strategies that return joint torques ratherthat a local feedback policy.

of a single step, highlighting the tracking during a changein the contact states. However, sensor noise, stiction andbacklash problems currently limit the feedback gains that canbe used in our LQR designs. We are currently investigatingfiltering methods to reduce these issues and improve realrobot performance.

IV. CONCLUSION AND FUTURE WORK

The goal of this project is to understand how linearoptimal control approaches compare to more complex whole-body approaches for multi-contact tasks. In particular we areinterested to get insight on the difference of performanceon real robotic systems with model inaccuracies and sensornoise. We have shown that for a bipedal system, using a sim-ple constraint consistent LQR controller yields disturbancerejection results competitive with more complex methods.We have additionally shown that by using a small numberof linear models and changing contact conditions we cantrack a simple walking trajectory. Our current research isrelated to the work of [9] and investigates how to exploit boththe lightweight computational advantages of our approachwhile adding the flexibility that advanced operational-spacemethods based on QPs offer. By varying the complexity ofour feedback controllers, we intend to better understand thebenefits of added complexity for real robot control.

REFERENCES

[1] S. Feng, X. Xinjilefu, C. Atekson, and J. Kim. Optimization basedcontroller design and implementation for the atlas robot in the darparobotics challenge finals. In Humanoid Robots (Humanoids), 201515th IEEE-RAS International Conference on, Nov 2015.

[2] A. Herzog, L. Righetti, F. Grimminger, P. Pastor, and S. Schaal. Bal-ancing experiments on a torque-controlled humanoid with hierarchicalinverse dynamics. In IEEE/RSJ Intl Conf on Intelligent Robots andSystems, 2014.

[3] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi,and H. Hirukawa. Resolved momentum control: humanoid motionplanning based on the linear and angular momentum. In IntelligentRobots and Systems, 2003. (IROS 2003). Proceedings. 2003 IEEE/RSJInternational Conference on, volume 2, pages 1644–1650 vol.2, Oct2003.

[4] O. Kanoun, F. Lamiraux, and P.-B. Wieber. Kinematic control ofredundant manipulators: Generalizing the task-priority framework toinequality task. Robotics, IEEE Transactions on, 27(4):785–792, Aug2011.

[5] T. Koolen et al. Summary of team ihmc’s virtual robotics challengeentry. In Humanoid Robots (Humanoids), 2013 13th IEEE-RASInternational Conference on, pages 307–314, Oct 2013.

[6] S.-H. Lee and A Goswami. A momentum-based balance controller forhumanoid robots on non-level and non-stationary ground. AutonomousRobots, 33(4):399–414, 2012.

[7] S. Mason, L. Righetti, and S. Schaal. Full dynamics lqr control of ahumanoid robot: An experimental study on balancing and squatting.In Humanoid Robots (Humanoids), 2014 14th IEEE-RAS InternationalConference on, pages 374–379, Nov 2014.

[8] C. Ott, M.A. Roa, and G. Hirzinger. Posture and balance controlfor biped robots based on contact force optimization. In HumanoidRobots (Humanoids), 2011 11th IEEE-RAS International Conferenceon, pages 26–33, Oct 2011.

[9] M. Posa, S. Kuindersma, and Tedrake R. Optimizationand stabilization of trajectories for constrained dynamicalsystems. 2015. Submitted paper under review. Available athttp://groups.csail.mit.edu/robotics-center/public_papers/Posa15.pdf.

[10] L. Righetti and S. Schaal. Quadratic programming for inversedynamics with optimal distribution of contact forces. In HumanoidRobots (Humanoids), 2012 12th IEEE-RAS International Conferenceon, pages 538–543, Nov 2012.

[11] B.J. Stephens and C.G. Atkeson. Dynamic balance force control forcompliant humanoid robots. In Intelligent Robots and Systems (IROS),2010 IEEE/RSJ International Conference on, pages 1248–1255, Oct2010.

[12] R. Tedrake et al. A summary of team mit’s approach to the virtualrobotics challenge. In Robotics and Automation (ICRA), 2014 IEEEInternational Conference on, pages 2087–2087, May 2014.

[13] K. Yamane. Systematic derivation of simplified dynamics for hu-manoid robots. In Humanoid Robots (Humanoids), 2012 12th IEEE-RAS International Conference on, pages 28–35, 2012.