Capturability-Based Analysis and Control of Legged Locomotion,

Part 2: Application to M2V2, a Lower Body Humanoid

Jerry Pratt∗ Twan Koolen∗† Tomas de Boer† John Rebula‡ Sebastien Cotton∗

John Carff∗ Matthew Johnson∗ Peter Neuhaus∗

April 2011

Abstract

This two-part paper discusses the analysis and con-trol of legged locomotion in terms of N -step captura-bility: the ability of a legged system to come to astop without falling by taking N or fewer steps. Weconsider this ability to be crucial to legged locomo-tion and a useful, yet not overly restrictive criterionfor stability.

Part 1 introduced the N -step capturability frame-work and showed how to obtain capture regions andcontrol sequences for simplified gait models. In Part2, we describe an algorithm that uses these resultsas approximations to control a humanoid robot. Themain contributions of this part are 1) step locationadjustment using the 1-step capture region, 2) novelinstantaneous capture point control strategies, and 3)an experimental evaluation of the 1-step capturabil-ity margin. The presented algorithm was tested usingM2V2, a 3D force-controlled humanoid robot with 12actuated degrees of freedom in the legs, both in sim-ulation and in physical experiments. The physical

∗J. Pratt, T. Koolen, S. Cotton, J. Carff, M. Johnson, andP. Neuhaus are with the Institute for Human and MachineCognition, Florida†T. Koolen and T. de Boer are with the Delft University of

Technology, the Netherlands‡J. Rebula is with the University of Michigan, Ann Arbor§This work was funded through the Army Tank and Au-

tomotive Research and Development Command (W56HZV-04-C-0072), the Defense Advanced Research Projects Agency(FA8650-05-C-7265), the Office of Naval Research (N00014-09-1-0913-01), NASA (NNX10AG54A), and the Honda ResearchInstitute.

robot was able to recover from forward and sidewayspushes of up to 21 Ns while balancing on one leg andstepping to regain balance. The simulated robot wasable to recover from sideways pushes of up to 15 Nswhile walking, and walked across randomly placedstepping stones.

1 Introduction

Making humanoid robots useful in complex environ-ments requires attaining good disturbance rejectionproperties while performing other tasks, such as walk-ing. Current robots have not sufficiently demon-strated this ability. In Part 1, we proposed to ap-proach this problem using the concept of N -step cap-turability, informally defined as the ability to come toa stop in N steps or fewer. We provided a capturabil-ity analysis of three simplified gait models. This partpresents capturability-based control algorithms forbalancing and walking that are robust to pushes andunexpected ground variations. In the presented con-trol algorithms, we make use of the concepts derivedin the previous part of this two-part paper. Thesecontrol algorithms were implemented on M2V2, aforce controlled 3D lower body humanoid with two6 DoF legs.

The three main areas of contribution of this partare:

1. step location adjustment using an approximated1-step capture region, based on the 3D Linear In-verted Pendulum Model (3D-LIPM) with finite-

1

sized foot;

2. novel instantaneous capture point control strate-gies using placement of the center of pressure(CoP);

3. an experimental evaluation of the control algo-rithms using the N -step capturability margin in-troduced in Part 1.

Furthermore, the current part extends some of thecontrol strategies from previous work [24] to 3D.

To date, we have achieved push recovery duringone-legged balancing on the physical robot. In sim-ulation, we have also achieved push recovery whilewalking on level ground and while walking overstepping stones. These results demonstrate thatcapturability-based control using simplified dynamicmodels may be useful in developing bipedal walkingcontrol algorithms that are robust to disturbances.While performing one-legged balancing, the physicalM2V2 was able to recover from forward and sidewayspushes of up to 21 Ns. The simulated version of therobot was able to recover from sideways pushes of upto 15 Ns while walking on level ground.

The remainder of this paper is structured as fol-lows. Section 2 presents related literature. In Sec-tion 3 we describe the M2V2 robot. Section 4 de-scribes the simulation environment for the robot. InSection 5 we describe the control tasks that we areinterested in achieving with the robot. Section 6describes some of the control concepts that we em-ployed in developing control algorithms. Section 7presents implementation details of our control algo-rithms. Section 8 presents results. In Section 9, wediscuss capturability-based analysis and control andsuggest future work. Finally, we conclude the paperin Section 10.

2 Background

The literature on control algorithms for humanoidrobots is extensive. Here we provide a brief surveyof some widely used control techniques and focus ontheir disturbance rejection properties.

ZMP-based trajectory tracking control TheZero Moment Point (ZMP) is the point about whichthe resultant ground reaction torque has no hori-zontal component [33]. ZMP-based trajectory track-ing control usually encompasses choosing a desiredZMP trajectory based on available footholds and de-sired gait properties, and calculating the center ofmass (CoM) motion that results in that ZMP trajec-tory [13,14]. It is widely used in legged robot controlsince maintaining the ZMP strictly inside the sup-port polygon at all times guarantees that it is physi-cally possible to track the reference joint trajectoriesusing conventional control tools. The distance fromthe ZMP to the edge of the support polygon can beused as a measure of robustness. Modifying the refer-ence ZMP trajectory online has also been explored,including both small local ZMP changes and largerstep placement changes [18]. Another ZMP controlapproach treats the ZMP as a control input, whichis manipulated to produce a desired motion of theCoM. For example, central pattern generators havebeen used to calculate the reference ZMP trajectoryto produce walking [31].

Typical ZMP-based gait generation techniquescannot be used to generate gaits for which the stancefoot rolls from heel to toe, as observed in fast humanwalking. This limitation is due to the ZMP needingto be inside the support polygon to meet the ZMPstability criterion [33]. However, when the robot ro-tates about an edge of its support foot, the area ofthe support polygon decreases to too small of a valueto provide robustness against small modeling errors.Also, the reference joint trajectories themselves mightlead the robot to a fall by design, even if the ZMP iskept inside the support polygon at all times. Hence,the ZMP criterion is not a necessary condition toavoid falling [28]. In addition, the ZMP criterionis not applicable to non-flat terrain [36]. Most im-portantly for the present work, ZMP analysis doesnot answer the crucial question of where to step torecover from large disturbances.

Passive Dynamics Based Control Another ap-proach to walking control explicitly relies upon thepassive behavior of the robot’s mechanical com-

2

ponents. Straight-line periodic walking has beendemonstrated for purely passive devices walkingdown a slope [4, 16]. Adding limited actuation tomachines designed for passive walking can yield acontrolled, efficient gait [3]. Rejection of small dis-turbances has been shown for planar walkers underlimited control [15, 32], as well as locally stable gaitswith purely reflexive control [8]. A major focus ofour current work is endowing robots with the abilityto recover from disturbances large enough to requiresignificant actuation, so we cannot rely on passivedynamics alone to avoid falling.

Hybrid Zero dynamics Another approach to lo-comotion control identifies relationships between thedegrees of freedom of a robot that lead to a steadygait [35]. These relationships are then enforced bya feedback controller, yielding a locally stable, peri-odic gait. This method has been shown to be able tocope with moderately rough terrain [29] [34]. Morerecently, this method has been used to generate threedimensional walking [2]. However, it requires off-linecomputation of a repetitive gait, and therefore it cur-rently has no mechanism for explicitly handling roughterrain with impassable areas. Also, it is still un-clear how a robot using this method will handle largepushes that significantly disturb the state of the ma-chine from the preplanned gait.

Compliant Strategies for Force ControllableRobots Force controllable robots have lead to thedevelopment of compliant control strategies. Virtualmodel control and other intuitive control strategieswere used on the 2D walking robot Spring Flamingo[24]. Coros et al. [5] combine Jacobian transpose con-trol, joint space PD control, and gravity compensa-tion with step planning based on an inverted pendu-lum model to obtain a walking controller that worksfor a range of simulated characters, while performingsecondary tasks. Stephens and Atkeson [30] intro-duced an algorithm that combines joint PD control,virtual model control and Dynamic Balance ForceControl. Dynamic Balance Force Control is an in-verse dynamics approach based on the contact forcesobtained from a CoM planner. Hyon et al. achieved

disturbance rejection using a passivity-based con-troller [11], later complemented by CoM control usinga Dynamic Balancer [12].

Compliant strategies can enhance the robustness ofa walking algorithm since they focus on interactionforces with the environment to achieve higher levelgoals, instead of relying on high gain position con-trol and extremely accurate ground models to achieveperfect kinematic trajectories. The presented workalso uses a compliant control strategy.

3 Description of M2V2 Robot

M2V2 (see Figure 1) is a lower body humanoid withtwo 6 DoF legs [26]. There are three degrees of free-dom at each hip, one at each knee, and two at eachankle. See the Appendix for approximate inertia pa-rameters and joint offsets, which we use in the simula-tion model. It is a second generation redesign of M2,a robot developed at the MIT Leg Laboratory [19,21].

Each degree of freedom is driven by an identi-cal force controllable Series Elastic Actuator (SEA)[22, 27]. These actuators use a spring in series be-tween the drive train and the load. By measuringthe spring deflection, the force exterted by the actu-ator can be measured. Using a feedback controller,the actuator force can be controlled accurately. ForM2V2, each actuator can produce a force of up to1.3 kN, with a smallest resolvable force of approxi-mately 4.4 N, giving it a 300 : 1 dynamic range. Theforce controllable bandwidth of each actuator is ap-proximately 40 Hz. M2V2 has two U.S. Digital EM1-0-500 linear encoders and LIN-500 encoder strips ateach Series Elastic Actuator, one to measure positionand one to measure spring deflection.

Onboard computation is provided by a PC104 witha dual core Pentium-M processor. Sensor readingis done by several AccesIO 104-Quad8 encoder in-put boards. Desired current is output as a PWMsignal through two Real Time Devices 6816 PWMboards. Body orientation and angular rate are mea-sured using a MicroStrain 3DM-GX3-25 inertial mea-surement unit. Current control is provided throughtwelve Copley Controls Accelnet module ACM-180-20 amplifiers. The PC104, I/O cards, and current

3

Figure 1: M2V2, a 3D force-controllable humanoidrobot with twelve actuated degrees of freedom.

amplifiers are all located in the body of the robot.A custom designed push stick equipped with a dig-

ital Loadstar ILoad Pro load cell was constructedfor measuring pushing forces applied to the robot.This load cell is connected to to the robot to elimi-nate data synchronization issues, but the control al-gorithm does not have access to its output.

4 Simulation environment

We have developed a simulation model of M2V2 us-ing the Yobotics Simulation Construction Set [37].This software package allows for rigid body dynam-ics modeling and simulation using the ArticulatedBody Algorithm [6, 17] and a fourth order Runge-Kutta integrator. The integration step size we use is0.1 ms. Ground contact forces are determined usingspring-damper ground models, sometimes referred toas penalty-based methods.

Pushing disturbances are modeled as high intensityforces of constant magnitude that are applied for a

hardware sensors simulated sensors

raw sensor data

sensor processor

processed sensor data

hardware actuators simulated actuators

desired motor currents

output processor

desired joint torques

ControlAlgorithm

Figure 2: Overview of the software architecture. Themajority of the software runs both in simulation andon the real robot, eliminating the need to maintainseparate versions, and allowing for significant devel-opment and testing in simulation.

short duration. The force is applied to the midpointbetween the hip joints. In future work we intend toinvestigate recovery from pushes at any location onthe robot. Stepping stones are modeled as polygons.

Our software architecture (see Figure 2) is designedto have a common control algorithm that is used bothin simulation and on the physical robot. The onlydifferences between the simulation and the physicalrobot are the source of the raw sensor data and thedestination of the desired motor currents. The sim-ulated sensors include noise and discretization error.The actuators are modeled as low pass filters to sim-ulate the bandwidth of the SEAs and have maximumoutput force limits. The control algorithm threadsrun at the same rate in simulation and on the em-bedded computer. By having a shared code base, weeliminate the effort and bugs that are typical whenporting from a simulation environment to real hard-ware.

4

5 Control Tasks

The controller was designed for two separate con-trol tasks: 1) balancing on one leg and 2) walking.These tasks require slightly different implementationsof some of the modules in the controller, although themain control algorithm is the same.

5.1 Balancing

Balancing on one leg (subsequently called ‘balanc-ing’) entails going from a double support configura-tion to single support, and remaining in single sup-port for as long as possible. If the robot is signifi-cantly perturbed while in single support, it will needto take a step to prevent a fall. Possible distur-bances include pushes of significant magnitude and,to a lesser extent, sensor and actuator imperfections.Pushes of a magnitude requiring the robot to take astep will be assumed to have a direction that does notrequire a cross-over step. For example, if the robot isbalancing on its left leg, then significant pushes couldbe directed forward, backward or to the right, but notto the left. Balancing was chosen as a precursor towalking because it is challenging and requires goodfoot placement, but it is not as difficult as walkingsince sustained forward progression is not required.To date we have achieved balancing on the physicalrobot, recovering from forward and sideways pushesup to 21 Ns.

5.2 Walking

The walking task requires the robot to move for-ward and be able to change walking direction. Inaddition, the controller needs to be able to handlewalking over stepping stones. To date, walking onflat ground without pushes has been achieved on thephysical robot. Walking in the presence of pushes upto 15 Ns and walking over stepping stones have beenachieved in simulation.

6 Control Concepts

This section describes three key concepts we use inour control strategies, regardless of the control task.

These concepts are 1) capturability-based control us-ing an approximate model, 2) force control and 3)virtual model control.

6.1 Capturability-based Control usingan Approximate Model

We consider bipedal locomotion to be an inherentlyrobust control problem, which does not require greataccuracy in controller design. This motivates our useof an approximate model. Out of the three simplifiedgait models described in Part 1, we have chosen touse the 3D-LIPM with finite-sized foot for approxi-mations. See Section 9.1 for the motivation for thischoice. We base our control strategy on the instan-taneous capture point and the approximated 1-stepcapture region. Although no guarantees on captura-bility can be made for the robot using this approxi-mation, we have found that it works well in practice.

Considering the 3D-LIPM with finite-sized foot,the instantaneous capture point is the point on theground where the CoP should be placed instanta-neously and maintained to come to a rest with theCoM directly above the CoP. We do not specify a de-sired CoM trajectory; instead, desired instantaneouscapture point paths are used as a basis for control.The linear dynamics of the instantaneous capturepoint allows us to find a desired CoP location withinthe base of support that ‘pushes’ the instantaneouscapture point along the desired path.

For the balancing task, we determine whether tak-ing a step is necessary based on whether the instan-taneous capture point has left the base of support(see Corollary 2 in Part 1 and [23]). For both con-trol tasks described in this part, the controller willattempt to step to a desired step location in the 1-step capture region. If the robot is disturbed signifi-cantly in mid-swing, the desired step location will beadjusted so that it always lies in the 1-step captureregion. Section 9.2 provides a discussion on why wechose to base the controller on the 1-step capture re-gion, as opposed to using an N -step capture regionwith N > 1.

5

6.2 Force Control

Force controllable actuators allow a biped to walksmoothly and naturally. These actuators allow forcompliant control methods that are forgiving to ex-ternal forces and unknown terrain. Traditionally,many humanoid robots use high-gain position con-trol to track prescribed joint trajectories using non-backdrivable actuators. This approach typically re-quires near perfect knowledge of the terrain, a nearperfect dynamic model of the robot, and no externalforces. When pushed or encountering unexpected ter-rain, these robots may no longer be able to follow theprescribed joint trajectories, and either a new trajec-tory must be computed on the fly, or the robot falls.1

Our control approach avoids the use of joint tra-jectory tracking for the stance leg. Instead, we uselow-impedance feedback controllers that control thefundamental aspects of walking (foot placement andbody height, orientation and speed), rather than at-tempting to rigidly control each degree of freedom.Force control provides some robustness to rough ter-rain since the exact foot/ground contact configura-tion is less important than the interaction forces be-tween the feet and the ground. In addition, forcecontrollable actuators simplify control of the CoP lo-cation, and allow compliant control techniques, suchas virtual model control.

6.3 Virtual Model Control

Virtual model control is a tool that allows a designerto control a robot by choosing virtual components,such as springs and dampers, to intuitively achievetask goals [1, 9, 10, 24, 25]. Once these componentsare chosen, the kinematic model of the system andadditional user defined constraints allow direct cal-culation of the joint actuation required to simulatethe effect of the desired components.

For example, in previous work on Spring Turkey, aplanar walking robot with 4 actuated degrees of free-dom, we decomposed the requirements of walking and

1Trajectory tracking bipedal robots often use compliantfoot pads, force sensors, and real time modification of ankletrajectories to control the CoP location. This technique essen-tially converts the ankle actuators to lower impedance forcecontrollable actuators.

designed simple virtual components to achieve eachone [24]. CoM height was maintained by a virtualvertical spring-damper “granny walker” and forwardtravel was achieved with a virtual “track bunny” withconstant forward velocity connected to the robot’sbody by a virtual horizontal damper. The joint ac-tuation was calculated using the transpose of a Ja-cobian that spans the joints between two virtuallycontrolled points. While kinematic singularities re-move degrees of freedom from the possible actuation,the joint torques for the remaining degrees of freedomcan still be calculated. Similar techniques were usedin the control of Spring Flamingo, a planar walkerwith 6 actuated degrees of freedom. Virtual modelcontrol is used in the control of M2V2 to maintainCoM height and body orientation, and to achieve ap-proximate CoP control.

7 Controller Implementation

We now present a detailed description of the balanc-ing and walking controller. The controller’s inputis comprised of joint angles and angular velocities,and the orientation and orientation rates of the up-per body. The controller’s output is comprised ofdesired torques at each joint. The controller consistsof five main parts:

1. the state machine, which keeps track of the gaitphase for each leg and acts as a supervisory sys-tem that calls the appropriate lower level rou-tines (see Section 7.1),

2. the capture region calculator, which determinesthe instantaneous capture point and the 1-stepcapture region (see Section 7.2),

3. the desired footstep calculator, which determineswhere to step to next (see Section 7.3),

4. the swing sub-controller, which computes thetorques for the swing leg joints, if any (see Sec-tion 7.4), and

5. the stance sub-controller, which computes thetorques for the stance leg joints (see Section 7.5).

6

StateMachine

DesiredFootstep

Calculator

desired joint torques

joint torques forswing leg, if any

joint torques forstance leg(s)

Control Algorithm

s rstep, Rstep

Wcomp

CaptureRegion

Calculator

C

processed sensor data

StanceSub-controller

SwingSub-controller

Figure 3: Overview of the control architecture. Ar-rows represent data flow. Arrows that point to theedge of a block signify that the corresponding infor-mation is available to all subblocks. The state ma-chine produces the controller state s. The captureregion calculator approximates the 1-step capture re-gion C. The desired footstep calculator uses the cap-ture region to compute a step location rstep and ori-entation Rstep. The swing sub-controller computestorques for the swing leg if the robot is in a sin-gle support state. It also produces a wrench Wcomp

that the stance sub-controller uses to compensate theswing motion. The stance sub-controller computesthe torques for both legs in double support, or justthe stance leg in single support.

See Figure 3 for an overview of the control archi-tecture. The same state machine structure is used forboth control tasks (balancing and walking), but withdifferent control actions and transition conditions foreach task. While the swing and stance sub-controllersare different for each task, they share many of the un-derlying control modules. The desired footstep cal-culator is also task-specific.

The capture region calculator is task independentand is the module most linked with capturability-based analysis and control. This module may also beuseful in other legged robot control architectures andwith other walking control techniques.

stop in double support

transfer to L support

processed sensor data

s

L loading - R pre-swing

L early stance - R initial swing

L late stance - R mid swing

L terminal stance - R terminal swing

transfer to R support

R loading - L pre-swing

R early stance - L initial swing

R late stance - L mid swing

R terminal stance - L terminal swing

State Machine

Figure 4: The state machine, which produces the con-troller state s. Blocks represent states and dashedlines represent available state transitions. When thecontrol algorithm is started, the robot is in the ‘stopin double support’ state.

7.1 State Machine

The state machine structure, shown in Figure 4,is based on the gait phases that a single leg goesthrough during human walking, as described in thebiomechanics literature [20]. See Figure 5 for a graph-ical depiction of these gait phases. The gait phasescan be grouped into stance phases and swing phases.Below we provide a short description of each gaitphase. See [20] for more detailed descriptions.

7.1.1 Stance phases

During loading response, the shock of initial groundcontact is absorbed by bending the knee and usingthe heel as a rocker. The leg is loaded, while thetrunk is kept upright. Once the opposite leg is lifted,the robot transitions into mid stance, in which theCoM moves forward over the stance foot as the legis straightened. The robot transfers into terminalstance when the CoM is in front of the support foot.During this state, the heel rises and the knee is firststraightened further and then begins to flex slightly.

7

Loading Response Early Mid Stance Late Mid Stance Terminal Stance Initial Swing Mid Swing Terminal SwingPre-Swing

Figure 5: The gait phases upon which the state machine is based. The labels correspond to the phase of thehighlighted right leg. Adapted from [20], which describes the phases of gait for human walking.

7.1.2 Swing phases

When the opposite leg makes contact with theground, the robot transitions into pre-swing. Theleg is unloaded and bent more in preparation of theswing phase. Initial swing begins as the foot lifts offthe floor. Foot clearance is achieved and the leg isswung forward. When the swinging limb is oppositethe stance limb, the robot enters mid swing. The hipis flexed further and the knee is allowed to extendin response to gravity. Finally, the robot transitionsinto terminal swing, in which the knee is extended aslimb advancement is completed.

Transitions between gait phases for the right leg aredirectly coupled to those for the left leg. The statesshown in Figure 4 were hence created by combiningone gait phase for the left leg and one gait phasefor the right leg, e.g. ‘left early stance - right initialswing’. In addition to the eight walking states, thereis also a state in which the robot is stopped in doublesupport and states in which weight is transferred toone leg, allowing the robot to start from a stop.

While this state machine is based on walking, it iseasily adapted to one-legged balance, through appro-priate selection of control actions in each state andtransition conditions between states.

7.2 Capture Region Calculator

The capture region calculator approximates the 1-step capture region C, the set of locations where thecontact reference point, a chosen point on each foot,may be placed at the next step so that the robotis able to reach a captured state. The algorithm isbased on the capturability analysis of the 3D-LIPMwith finite-sized foot, as presented in Part 1, and werefer to that part for the underlying theory; here weonly present the implementation details of the cap-ture region calculator. See Algorithm 1 for pseu-docode. Figure 6 describes the algorithm graphically.

The algorithm can handle limitations on wherethe CoP may be placed by specifying allowable CoPregions, represented as a set of polygons, denotedA = {A0, A1, . . . }. The allowable CoP regions maybe used to model stepping stones, as well as to avoidcross-over steps and overlapping footsteps.

Algorithm 1 assumes that the orientation of theswing foot may be chosen without limitation, whichmeans that the maximum distance rmax from the con-tact reference point to the edge of the foot polygon(see Figure 7) fully specifies which contact referencepoint locations allow a given CoP location to be at-tained. We further specify a maximum step lengthlmax, defined as the maximum distance between sub-sequent contact reference point locations.

The output of the algorithm is a tuple(CP , rmax, lmax), consisting of a set of polygons

8

CP , the maximum offset distance to these polygonsrmax, and the maximum distance to the contactreference point lmax. The basic idea of the algorithmis to determine the set of polygons CP consistingof the parts of A where the instantaneous capturepoint may be located when a step is taken, so thatthe CoP may be placed there after the step, lettingthe robot reach a captured state. The capture regionC is then found by determining allowable contactreference point locations that enable the CoP to beplaced inside CP as shown in Figure 7.

Algorithm 1 1-step capture region calculator

Input: S, r, r, A, ts,min, t, rmax, lmax

1: ric ⇐ Pr +√

z0g r

2: CP ⇐ A3: if ¬pointInPolygon(S, ric) then4: Q = (q0, . . . ,qn)⇐ visibleVertices(S, ric)5: for i = 0 to n do6: pi ⇐ [ric − qi]e

ts,min−t + qi

7: end for8: CP ⇐ bound(CP ,p0, ric)9: CP ⇐ bound(CP , ric,pn)

10: for i = 1 to n do11: CP ⇐ bound(CP ,pi−1,pi)12: end for13: end if14: return (CP , rmax, lmax)

In Algorithm 1, S is the current support polygonrepresented as a clockwise ordered sequence of ver-tices, r is the CoM position, ts,min is the estimatedearliest time at which a step can be taken, t is thecurrent time, rmax is the maximum distance from thecontact reference point to the edge of the supportpolygon, and lmax is the maximum step length, asmeasured from the contact reference point.

Line 1 in the algorithm is the instantaneous cap-ture point definition, (8) in Part 1. Line 6 describesthe instantaneous capture point motion for a constantCoP and originates from (22) in Part 1.

The function pointInPolygon(X,p) determineswhether point p is contained within the polygon de-fined by the sequence of vertices X.

Function visibleVertices(X,p) computes a clock-

A1

A0

A2

S

r .rq0

q1

q2

ric

8

9

11a

11b

CP

p0

p1

p2

Figure 6: Graphical depiction of Algorithm 1. Theinstantaneous capture point ric is first calculated (line1). The ordered set of support polygon verticesthat are visible from the instantaneous capture point,(q0,q1,q2) is then determined (line 4), as well as thecorresponding possible locations of the instantaneouscapture point at ts,min, (p0,p1,p2) (lines 5–7). Theset of polygons CP is finally determined by boundingthe set of allowable CoP regions, A, using the linesdenoted 8, 9, 11a and 11b in the figure. The numberscorrespond to line numbers in Algorithm 1.

C

ric CP

r max

rmax

contact reference point

rmax

lmax

Figure 7: The 1-step capture region C, as found by1) offsetting the set of polygons CP by rmax, and 2)intersecting the result with a disk of radius lmax andthe contact reference point as its center to take themaximum step length constraint into account. Notethat for this figure, the centroid of the foot was chosenas the contact reference point.

9

wise ordered sequence of vertices Q = (q0, . . . ,qn)taken from X that ‘can be seen’ from p, in the sensethat a line segment starting from p and pointing toqi does not intersect the polygon spanned by X. Thefirst element of the returned sequence is the rightmostvisible vertex as seen from p, and the last element isthe leftmost.

The function bound(P, r0, r1) takes a set of poly-gons P and returns a modified version of P , whereeach individual polygon is intersected with a half-plane containing all points to the right of a bound-ary line through r0 and r1. Note that the order ofthe arguments r0 and r1 determines the ‘direction’of the boundary line and hence the direction of thehalf-plane. This function is used to cut off parts ofthe allowable CoP regions to which the instantaneouscapture point may not be directed or which are notreachable given the minimum step time.

Note that if the current support foot is only par-tially placed inside the allowable CoP regions A, thesupport polygon S is the intersection of the foot poly-gon and A, and Algorithm 1 can be used withoutmodification. Furthermore, note that if there are lim-its on allowable foot orientations, the step of offset-ting the set of polygons CP by rmax should be re-placed by convolving the allowable configurations ofthe swing foot polygon about CP .

7.3 Desired Footstep Calculator

The desired footstep calculator (see Figure 8) deter-mines the desired position of the swing ankle rstepand orientation of the swing foot Rstep at the end ofthe upcoming step. The desired position and orien-tation are expressed in a frame fixed to the supportfoot. Because footstep planning depends greatly onthe control task, we have created two separate im-plementations of this module: one for the balancingtask and another for the walking task. The generalpattern used for both implementations is to choosea desired initial footstep (rstep,init,Rstep,init) at thestart of the swing phase and adjust it during theswing phase if necessary, for example if the robotis significantly perturbed, to obtain the final output(rstep,Rstep).

7.3.1 Balancing

The desired footstep calculator for the balancing taskis a minimal implementation. The initial desired steplocation is computed using a fixed step length lstep inthe direction of the instantaneous capture point:

rstep,init = rankle + lstepric − rankle‖ric − rankle‖

(1)

where rankle is the position of the stance foot ankle.The initial foot orientation is chosen to be the sameas the stance foot orientation and is never adjusted.The direction in which the robot steps is adjustedby recomputing (1) during the first 0.1 s after theinstantaneous capture point has left the foot polygon.After that it remains fixed for the remainder of swing.

7.3.2 Walking

The desired footstep for the walking task is chosen insuch a way that forward progression is made, whilethe robot remains 1-step capturable. The initial steplength is determined by multiplying the desired walk-ing velocity by a proportional gain, while the initialstep width is set to a constant value. On flat ground,the step height is set to zero but may be changedto any feasible desired value when necessary, for in-stance to climb a slope. Footstep yaw is set equal tothe desired walking direction. Footstep pitch and rollare set to zero on flat ground but may be used to walkon rough terrain to reduce the need for compliancein the ankle joint.

If the initial desired step location ceases to bewithin the capture region by a desired margin at anyinstant during the swing phase, an adjusted desiredstep location is computed by projecting the initial lo-cation inside the 1-step capture region by the desiredmargin. The orientation is not adjusted.

7.4 Swing Sub-controller

The task of the swing sub-controller (see Figure 9)is to compute the desired torques for the swing legjoints. It accomodates changes in the desired steplocation, obtained from the desired step location cal-culator, on the fly. The controller contains a trajec-tory generator for the position and orientation of the

10

Desired footstep calculator

Yes

CAt start of

swing phase?Compute initial step

location and orientationstart

Adjust step locationif necessary

Initial step locationand orientation buffer

processed sensor data

rstep, Rstep

rstep,init

rstep,init

Rstep,init

Rstep,initNo

Figure 8: Basic structure of the desired footstep cal-culator. The desired footstep calculator determinesthe desired position rstep and orientation Rstep of thefoot for the upcoming step. Solid lines represent dataflow and dash-dotted lines represent flow of control.

swing foot, which is used to compute desired jointangles, velocities and accelerations through inversekinematics. The trajectory is tracked using inversedynamics augmented by PD position control in jointspace.

7.4.1 Trajectory generation

The swing foot trajectory is updated, even in midswing, to allow for changes in step location in reac-tion to external disturbances. We chose a second-order system as a reference model to determine thedesired linear position xd ∈ R3 and velocity xd of theankle of the swing foot at each control time step byintegrating an appropriately chosen desired acceler-ation xd. Initial values for xd and xd are set equalto the actual position and velocity of the swing footankle.

The swing foot reference model goes through threephases: take-off, cruise, and landing. Limits areplaced on the magnitudes of acceleration and velocity,and we specify a clearance height and a take-off andlanding slope. During the take-off phase, the desiredacceleration xd is chosen to have maximal magnitudeuntil the velocity limit is reached, and be directed atthe current desired step location rstep, while movingupwards using the specified take-off slope. Once the

Swing Sub-controllerrealtime

trajectorygenerator

joint levelPD control

inversekinematics

inversedynamics

τPD τID

xd, ẋd, ẍd Rd, ωd, ωd.

qd..qd, qd

. Wcomp

joint torques forswing leg, if any

s processed sensor data

rstep, Rstep

Figure 9: Swing sub-controller. A real-time trajec-tory generator computes the desired position, linearvelocity and acceleration (xd, xd, xd respectively), aswell as the desired orientation, angular velocity andangular acceleration (Rd, ωd, ωd respectively), ex-pressed in world frame. Inverse kinematics is used totranslate this trajectory from Euclidean space to de-sired joint positions, velocities and accelerations (qd,qd and qd respectively). This information is used byan inverse dynamics algorithm, which computes pinjoint torques τ id and an upper body compensationwrench Wcomp, used by the stance sub-controller.The output τPD of a joint-space PD controller isadded to τ ID to obtain the swing leg joint torques.

minimum clearance height is reached, the dynamicsystem transitions into the cruise phase, in which xd

is chosen to maintain constant height while still ad-justing for changes in the desired step location. Thetransition into the landing phase takes place based onthe landing slope, and the desired position deceler-ates maximally while the height is gradually reducedaccording to the landing slope.

For the orientation trajectory of the swing foot,we use a simple interpolation between the measuredorientation at the start of the swing phase and the de-sired final foot orientation, Rstep. The interpolationparameter is obtained from a quintic spline startingand ending with zero velocity and acceleration, andis not updated during the step.

11

7.4.2 Position control

The desired position and orientation of the swing footat every control time step are used to compute thecorresponding joint angles of the swing leg using in-verse kinematics. The desired linear and angular ve-locity and acceleration of the swing foot specify cor-responding desired joint velocities and accelerationsthrough the inverse of the swing leg Jacobian. Prob-lems due to the singularity that occurs when the kneeis stretched are circumvented by gradually scaling thedesired joint velocities and accelerations back to zero(that is, using pure damping) based on the value ofthe Jacobian determinant.

An inverse dynamics algorithm [7], augmented byPD position control in joint space is used to computethe desired torques for the swing leg joints. We ex-clude the stance leg joints in computing the inversedynamics. The desired spatial acceleration of the up-per body is set to zero. In addition to the torquesacross the swing leg pin joints, the inverse dynam-ics algorithm also returns a wrench that should beexerted across the ‘floating joint’ that connects theupper body to the world to achieve the desired zerospatial acceleration. This wrench will be used in thestance sub-controller as a feed-forward term to com-pensate the swing leg torques and reduce upper bodyoscillations.

Previously, we have also implemented a vir-tual model control-based swing sub-controller, butthe current implementation outperforms the virtualmodel control implementation in terms of accuracyand swing speed, which we consider to be key ingre-dients for dynamic walking and push recovery.

7.5 Stance Sub-controller

The stance sub-controller controls balance by com-puting desired torques for the stance leg(s). Thegoals of the stance sub-controller are to control 1)instantaneous capture point location, 2) upper bodyorientation, and 3) upper body height. The stancesub-controller is an implementation of virtual modelcontrol. For each stance leg, it computes a desiredwrench on the upper body, to be exerted by the leg,that satisfies these control goals. Jacobian transpose

control is then used to find corresponding desired legtorques. The stance sub-controller consists of multi-ple control modules, as shown in Figure 10. The fol-lowing sections will describe these modules in moredetail.

7.5.1 Upper body height control module

The upper body height control module regulates theupper body height by determining the net verticalforce, fz, that the legs should exert on the upperbody. In double support, fz is set to a constant valuethat slightly overcompensates the estimated gravita-tional force acting on the entire robot, simulating avirtual constant force spring. In single support, fzis set to the weight of the stance leg and the bodyplus the z-component of the force from the swing legcompensation wrench Wcomp, cancelling out some ofthe dynamic effects due to the swing leg motion.

7.5.2 Upper body orientation control module

The upper body orientation control module deter-mines a desired torque τ that the legs should exerton the upper body. It is used to control the orien-tation of the upper body with respect to the world(as perceived by the inertial measurement unit). Thedesired pitch of the upper body is constant and setto zero. Both the desired yaw and the desired roll de-pend on the gait phase and state of the support leg.Yawing and rolling are used to obtain a longer reachfor the swing leg and to make the gait look more hu-manlike. In addition, the desired yaw also dependson the desired walking direction.

The desired upper body torque is computed usingPD control on the roll, pitch and yaw correspond-ing to the rotation matrix that describes the orien-tation of the actual upper body with respect to thedesired orientation. In single support, the torque partof Wcomp is added to the result to compensate theswing leg motion.

A lunging strategy based on the results of the 3D-LIPM with finite-sized foot and reaction mass has notyet been implemented and is part of future work.

12

orientation

Stance Sub-controller

τ

fz

joint torques for stance leg(s)

virtualsupportactuatorcontrolmoduleheight

Upper Body Control Modulesw0, w1 rvtp,0, rvtp,1

virtual toe pointcalculator

rCoP, desinstantaneouscapture point

control module

rstep, Rstep

Wcomp

s processed sensor data

Figure 10: Stance sub-controller. The instantaneouscapture point control module computes the desiredCoP (rCoP,des) within the base of support. This de-sired CoP is used by the virtual toe point calcula-tor to compute leg support fractions (w0, w1) andvirtual toe points (rvtp,0, rvtp,1), which determineeach leg’s contribution to supporting the upper body.The orientation and height upper body control mod-ules determine the torque τ and the vertical forcefz to be exerted on the upper body. In single sup-port, these modules use the swing leg compensationwrench Wcomp to compensate swing leg motion. Fi-nally, the virtual support actuator control modulecomputes joint torques for the stance legs which re-sult in the desired virtual toe points, leg strengths,upper body torque and force.

7.5.3 Instantaneous capture point controlmodule

The goal of the instantaneous capture point controlmodule is to regulate the location of the instanta-neous capture point. Its output is the desired lo-cation of the CoP, rCoP,des. This control moduleswitches between two modes of operation, dependingon whether the instantaneous capture point is insidethe support polygon.

Instantaneous capture point inside supportpolygon See Figure 11. When the instantaneous

capture point is inside the support polygon, a desiredinstantaneous capture point, ric,des, is determined.Given the current and desired location of the instan-taneous capture point, the following simple controllaw is used to obtain the tentative location of thedesired CoP, rCoP,des:

rCoP,des = ric + kic(ric − ric,des) (2)

where kic is the proportional gain. This proportionalcontrol law is motivated by the linear instantaneouscapture point dynamics for the 3D-LIPM with finite-sized foot described in Part 1. If rCoP,des lies insidethe support polygon, then the final output of thiscontrol module is rCoP,des = rCoP,des. Otherwise,rCoP,des is obtained by projecting rCoP,des to the edgeof the support polygon along a line through ric andric,des, as shown in Figure 11b.2 The idea behind thiscontrol law is that the instantaneous capture point isalways pushed away from the CoP, and hence towardsthe desired instantaneous capture point.

The desired instantaneous capture point is deter-mined as a function of the state and the controltask. For the balancing task, the desired instanta-neous capture point coincides with the centroid ofthe support polygon during the double support state.When the robot is commanded to start balancing onone leg, the desired instantaneous capture point ismoved to the centroid of the upcoming support foot,where it remains as long as the robot is able to main-tain its balance without taking a step. This locationmaximizes robustness against external disturbancesfrom unknown directions.

For the walking task, the desired instantaneouscapture point is located near the toes of the lead-ing foot during the double support states, promotingforward motion. At the start of the swing phase, thedesired capture point is moved outside the stance footin the direction of the upcoming step location.

2In practice, we use a slightly smaller support polygon whenprojecting the tentative desired CoP, to prevent the feet fromtipping at times when this is not desired. The use of a smallersupport polygon is necessary because of unmodeled dynamics,inability to perfectly track the desired CoP, and model uncer-tainty.

13

ric,desb)

rCoP,des

ric

ric,des

rCoP,des

ric

a)

rCoP,des

_

S S

Figure 11: Action of the instantaneous capture pointcontrol module when ric is inside the support polygonS. a) the tentative desired CoP rCoP,des, as deter-mined using (2), is inside the support polygon, so itcoincides with the final output rCoP,des. b) rCoP,des

is outside the support polygon and is projected ontoits edge to obtain rCoP,des.

Instantaneous capture point outside supportpolygon See Figure 12. The instantaneous cap-ture point will move outside the support polygon ifthe robot is pushed significantly, or because it hasbeen driven outside the stance foot polygon duringthe walking task. When ric is outside the supportpolygon, it is not possible to track a desired instan-taneous capture point location, since the instanta-neous capture point will always exponentially divergeaway from the stance foot. We therefore only con-trol the direction in which it diverges away from thefoot. This is done by specifying a guide line, Lg, andchoosing the desired CoP such that the instantaneouscapture point is kept along this guide line.

We define the guide line by two points. The first isthe contact reference point on the support foot. Thesecond is a point ∆x in front and ∆y to the inside ofthe contact reference point for the planned footstep,with

∆x = kxxvdes,x (3a)

∆y = kxy|vdes,x| (3b)

where kxx and kxy are positive gains and vdes,x is thedesired average velocity of the robot in the forwarddirection, in a frame oriented to match the plannedstep location. The effect of this simple control law isthat the instantaneous capture point is pushed for-ward and to the inside of the upcoming support footas desired forward velocity is increased.

plannedstep location

S

rCoP,des

ric

Lg

Lc

d(Lg, ric)

kic d(Lg, ric)

∆x

∆y

contactreference point

Figure 12: Action of the instantaneous capture pointcontrol module when ric is outside the support poly-gon S. The guide line Lg passes through the contactreference point on the support foot and a point ∆xin front and ∆y inside of the contact reference pointfor the planned footstep location, see (3). The con-trol line Lc is parallel to the guide line; the distancebetween the two is determined by (4). The desiredCoP is chosen as the intersection of Lc and S.

Given the guide line and the instantaneous capturepoint location, a second line, called the control line, isdefined. The control line is parallel to the guide line.The distance d(Lg, Lc) between the guide line Lg andthe control line Lc is set to be proportional to thedistance between the guide line and the instantaneouscapture point, d(Lg, ric):

d(Lg, Lc) = kicd(Lg, ric) (4)

where kic is a positive gain.Finally, the desired CoP is computed by finding the

intersection of the stance foot polygon and the controlline that is closest to ric. If there are no intersections,the support polygon vertex closest to the control lineis used. The control law (4) causes the instantaneouscapture point to be pushed back onto the guide lineafter a deviation.

7.5.4 Virtual toe point calculator

The virtual toe point calculator uses the desired CoPto compute a virtual toe point (VTP) and a leg sup-port fraction for each leg [27]. Controlling VTP loca-tions and leg support fractions results in approximatecontrol of the overall CoP of the robot.

14

w1

w0

1.0

0.5

1.0

0.5

rvtp,0 rvtp,1rCoP,des

L1

L2

Lvtp

Figure 13: Implementation of the virtual toe pointcalculator during double support. Line Lvtp is firstconstructed. It passes through the desired CoP andthe intersection of lines L1 and L2, the edges of thesupport polygon that connect the foot polygons. Theintersections of line Lvtp with the foot polygons de-fine two line segments. The VTPs rvtp,0 and rvtp,1are found as the center points of these line segments.The leg support fractions are then found based onthe distances between the VTPs and the desired CoPusing (5), which also has a clear geometric interpre-tation as shown in the figure. If the desired CoP lieson the outside of a foot, so that it lies farther to theedge of the support polygon than the center of theline segment for that foot, then the VTP for thatfoot is chosen to be equal to the desired CoP and theleg is assigned a leg support fraction of 1 (this caseis not shown in the figure).

The virtual toe point of a foot is the point aboutwhich no torque is commanded in the horizontalplane. VTPs are similar to the centers of pressurefor each foot, except that a VTP is a commandedquantity, not a measured one, and is only based ona static analysis. Details on how the VTPs are usedare given in Section 7.5.5.

In single support, the VTP for the stance leg isplaced at the location of the desired CoP. In dou-ble support, we use a heuristic based on geometricrelations, depicted and explained in Figure 13, to de-termine VTPs rvtp,0 and rvtp,1 in such a way that thedesired CoP and both VTPs lie on one line.

The leg support fractions are two scalars, denoted

w0 and w1, such that w0+w1 = 1 and w0, w1 ∈ [0, 1].A leg support fraction wi describes the fraction of thedesired torque τ and vertical force fz to be exertedby stance leg i ∈ {0, 1}. The distances between theVTPs and the overall desired CoP determine the legsupport fractions:

wi =‖rCoP,des − rvtp,1−i‖‖rvtp,0 − rvtp,1‖

, i ∈ {0, 1} (5)

This equation stems from a moment balancearound the desired CoP: if a VTP is far removed fromthe desired CoP, a force exerted at this VTP has alarge moment arm, and hence the associated leg sup-port fraction should be small. Support is graduallytransferred from one leg to the other in double sup-port due to continuously changing leg support frac-tions, associated with a continuous desired CoP tra-jectory.

7.5.5 Virtual support actuator control mod-ule

The virtual support actuator control module dis-tributes the torque τ and the vertical force fz overthe support leg(s) using the leg support fractions wi

as weighting factors:

fz,i = wifz i ∈ {0, 1}τ i = wiτ

(6)

where fz,i and τ i are the z-component of the forceand the torque to be exerted by leg i, respectively.We use these partial wrenches to compute a completedesired wrench Wi for each leg, where

Wi =

(fi

τ i

)with

fi = (fx,i, fy,i, fz,i)T

τ i = (τx,i, τy,i, τz,i)T.

(7)

The remaining x- and y-components of the force fifor each leg are computed using the virtual toe points.The virtual toe point constraint, which requires thatno torque should be applied about either horizontalaxis at the virtual toe point, can be enforced as fol-lows. We consider the virtual toe point for a footto be the intersection of the axes of two virtual pinjoints, located on the sole of the foot. Their orthog-onal axes of rotation lie in the plane of the foot. The

15

virtual pin joints do not exist on the physical robot,but provide a simple way of computing fx,i and fy,isince the torques across these joints should be zero.The virtual pin joints come after the real joints of therobot in the kinematic chain from upper body to foot.Their rotation angles are set to zero, but their loca-tion on the foot changes in time, depending on thelocation of the virtual toe point. We use τ vtp,i ∈ R2

to denote the vector of virtual joint torques exertedat the virtual pin joints for stance leg i. A staticanalysis results in

τ vtp,i = JTvtp,iWi. (8)

In this equation, Jvtp,i ∈ R6×2 is the Jacobian thatmaps the joint velocities of the virtual pin joints tothe twist of the foot with respect to a virtual bodyattached ‘after’ the virtual pin joints in the kinematicchain, expressed in an upper body-fixed frame.

Splitting the Jacobian Jvtp,i into a 2 × 2 blockJvtp,i,2×2 and a 4 × 2 block Jvtp,i,4×2 and using (7),we can rewrite (8) as

τ vtp,i = JTvtp,i,2×2

(fx,i

fy,i

)+ JT

vtp,i,4×2

(fz,i

τ i

).

(9)We require that the torques at a leg’s virtual toe

point be zero. We can hence solve (9) to find thevalues of fx,i and fy,i:(

fx,i

fy,i

)= −J−T

vtp,i,2×2JTvtp,i,2×4

(fz

τ i

)(10)

The matrix JTvtp,i,2×2 is invertible as long as the

following two conditions hold: 1) the z-coordinate ofthe origin of the upper body, expressed in the foot-fixed frame, is not zero, and 2) the z-axis of the upperbody-fixed frame does not lie in the horizontal planeof the foot-fixed frame. These conditions are alwayssatisfied during normal operation.

Now that fx,i and fy,i are also known, we know thecomplete wrench Wi to be exerted on the upper bodyby stance leg i, and we can use a different Jacobian,Jleg,i ∈ R6×6 to find the joint torques τ leg,i:

τ leg,i = JTleg,iWi, (11)

where Jleg,i maps the joint velocities of the real jointsof leg i to the twist of the upper body with respectto the foot, expressed in the upper body-fixed frame.

Computing fx,i and fy,i based on virtual toe pointsinstead of specifying these forces directly has as anadvantage that virtual toe points are closely relatedto the CoP, which plays a major role in the instan-taneous capture point dynamics described in Part 1.This relation to the CoP also means that limits due tothe finite-sized support polygon are straight-forwardto accomodate. We simply make sure that each foot’svirtual toe point lies inside its convex polygon.

8 Results

This section presents results obtained for both thebalancing task and the walking task. Balancing andwalking without pushes was achieved on the realM2V2 robot. Walking while recovering from pushesand walking over stepping stones was achieved on thesimulated M2V2 robot. Note that figures are labeledeither [REAL] if the data is from the physical robotor [SIM] if from the simulated robot. See Extension1 for a video of results from both real-world experi-ments and simulations.

In this section, we evaluate the N -step capturabil-ity margin proposed in Part 1, that is, the area ofthe N -step capture region. Here we use N = 1 sincethe presented algorithm is based on remaining 1-stepcapturable.

8.1 Balancing task

On the real M2V2 robot we achieved balancing on oneleg and recovering from sideways and forward pushes.Figure 14 shows time-elaspsed images of M2V2 recov-ering from a push. Figure 15 shows the norm of thedisturbance force, as recorded from the push stick,and the 1-step capturability margin. We see that therobot was able to recover from pushes of approxi-mately 21 Ns.

16

Figure 14: [REAL] M2V2 recovering from a pushwhile standing on one leg. Images are from left toright starting at the top left.

0 0.1 0.2 0.30

50

100

time [s]

Dis

turb

ance

forc

e no

rm [N

]

Cap

tura

bilit

y m

argi

n [m

2 ]

0 0.1 0.2 0.30

0.2

0.4

0.6

0.8disturbance

Figure 15: [REAL] Norm of disturbance force and1-step capturability margin of M2V2 recovering froma push while standing on one leg. The capturabilitymargin is not shown after transition to the ‘stop indouble support’ state (gray area) to avoid cluttering.

8.2 Walking task

On the real M2V2 robot we achieved flat groundwalking without disturbances. On the simulated

M2V2 robot we achieved walking while recoveringfrom pushes and walking over stepping stones.

Figure 16 shows time elapsed images of M2V2walking on flat ground. For this walk, the robot usesa constant step length and width. During the walk,the capture region is computed, but since there areno pushes, the robot does not have to change whereit steps.

Figures 17 through 20 show plots of a single dataset obtained from simulation for walking on flatground while recovering from pushes. Three differ-ent pushes to the left occur at approximately 4, 8.5,and 13 seconds. These pushes are modeled as forcesapplied to the midpoint between the hip joints andare 300 N in magnitude for a duration of 0.05 sec-onds. This corresponds to an impulse of 15 Ns. Notethat pushes were to the left while the left foot wasswinging. Pushes to the opposite side would requireeither a cross-over step or two quick steps, both ofwhich are more difficult to achieve and are an area offuture work.

Figure 17 shows the sideways pushes applied to theM2V2 simulation, and the resulting change in veloc-ity while recovering to pushes during walking. Sincethe pushes were mostly to the side, the change in lat-eral velocity is more prominent than forward velocity.After each push we see that the robot recovers withone step.

Figure 18 shows side and overhead views of therobot. Each time a push occurs, the robot steps tothe left to recover from the push. Also plotted in theoverhead view are the instantaneous capture pointtrajectory and the trajectory of each ankle.

Figure 19 shows the 1-step capturability marginduring walking of the M2V2 simulation while recov-ering from pushes. We see that the capturability mar-gin significantly decreases after each of the 3 pushes,corresponding to the decrease in area of the captureregion as seen in Figure 20.

Figure 20 shows a time-lapsed overhead view of therobot. In the first two frames the capture region isrelatively large during the beginning of swing. Therobot is then pushed between the first and secondframe, decreasing the size of the capture region andrequiring the robot to choose a different place to step.In frames 3-6 the robot steps further to the left than

17

Figure 16: [REAL] M2V2 robot walking on flatground. Images are from left to right starting at thetop left. In this walk, the robot uses a constant steplength of 0.35 m and a constant step width of 0.20m. The average walking velocity is 0.21m/s.

0 2 4 6 8 10 12 14050100150200250300

time [s]

Late

ral d

istu

rban

ce fo

rce

[N]

0 2 4 6 8 10 12 14−0.4

−0.2

0

0.2

0.4

Late

ral C

oM v

eloc

ity [m

/s]

disturbance

Figure 17: [SIM] Lateral velocity and disturbances ofthe M2V2 simulation while recovering from pushesduring walking. After each push, the lateral CoMvelocity increases by up to 0.44 m/s.

a)

b) desired heading

Figure 18: [SIM] Robot as it walks while beingpushed laterally to the left every second step. a) Sideview. Trajectories of the ankles are indicated withdashed lines. b) Overhead view. Actual ankle tra-jectories connect the sequence of footprints for eachfoot. The instantaneous capture point trajectory isindicated with a dashed line.

0 2 4 6 8 10 12 14050100150200250300

time [s]D

istu

rban

ce fo

rce

norm

[N]

Cap

tura

bilit

y m

argi

n [m

2 ]

0 2 4 6 8 10 12 140

0.2

0.4

0.6

0.8 disturbance

Figure 19: [SIM] 1-step capturability margin duringwalking while recovering from pushes. After eachpush, the 1-step capturability margin significantly de-creases showing that the robot is in danger of falling.After each recovery step, the capturability margin re-covers. Note that during double support and periodsduring single support when the instantaneous capturepoint is inside the support polygon the capturabilitymargin is not plotted (gray areas).

18

C

C

C C

C C

Center of massCenter of mass velocityInstantaneous capture point

Initial step locationAdjusted step location

Stance foot (right)Swing foot (left)

Capture regionC

t = 0.00 t = 0.12

t = 0.24 t = 0.36

t = 0.48 t = 0.60

Figure 20: [SIM] Time-lapse overhead view of therobot during the walking task, showing one step. Therobot is perturbed laterally at the start of the step(t = 0).

Figure 21: [SIM] M2V2 walking over stepping stonesthat are clustered in groups of three. Snapshots aretaken at 1.5 second intervals.

originally intended, landing in the 1-step capture re-gion, and successfully recovering from the push.

Walking over various stepping stones was achievedon the simulated M2V2 robot. Figure 21 shows anexample of walking over stepping stones that are clus-tered in groups of three. The robot was given exactknowledge of the stepping stones. To ensure thatthe entire foot rested on each stone, the allowableCoP region for each stepping stone was computed byshrinking the stepping stone based on the size of thefoot. However, due to inaccuracies in foot placement,the foot would sometimes slightly overhang the edgeof a stone.

9 Discussion and Future Work

9.1 Using Simple Models for ComplexRobots

In this part we used the 3D-LIPM with finite-sizedfoot, described in Part 1, to estimate the 1-step cap-ture region, which was then used to help control bal-ancing and walking in M2V2, a 3D lower body hu-manoid. This simple model was sufficient for con-trolling balancing on one foot, walking, and recover-ing from pushes while walking. The simplified modelwas sufficient for three reasons:

• The model accounts for the dynamics of the CoMwith respect to the CoP, which are the key dy-namics of walking.

• The model allows for the use of feet, and themodulation of the CoP in real time. This addsrobustness, as opposed to an algorithm that pre-determines a CoP trajectory.

• The 1-step capture region is relatively large formoderate speed walking. Therefore, there is alarge degree of robustness to modeling errors.

The presented control algorithms did not exploitangular momentum of the upper body as a meansof control. As we start to address more challengingtasks, like walking over rough terrain and over nar-row beams, we will likely need to use more complexmodels for computing capture regions and develop-ing control strategies. In Part 1, we analyzed the3D-LIPM with finite-sized foot and reaction mass.

19

This model should be useful in control when upperbody angular momentum is used to prevent falling.In future work we will use this model and will in-vestigate other strategies for using multi-joint upperbody angular momentum in walking and disturbancerecovery.

9.2 1-Step Versus N-Step CaptureRegions

In this part we developed controllers that alwaysstep into the 1-step capture region. This restric-tion is overly cautious in general, and it is likely thecase that fast walking may require periods when thelegged system is only 2-step, or perhaps only 3-stepcapturable. However, M2V2 currently has a longswing time, making the 2-step capture region notmuch larger than the 1-step capture region.

For M2V2, the falling time constant is 1/ω0 =√z0/g ≈ 0.32 seconds, where g = 9.81m/s2 is the

gravitational acceleration and z0 ≈ 1.0m is the CoMheight. The minimum step time ∆ts is currently ap-proximately 0.6 seconds for M2V2. The geometric ra-tio governing the diminishing returns for the N -stepcapture region as N increases is exp(−

√z0/g∆ts) ≈

0.15 (see (16) in Part 1). This means that the ra-dius of the 2-step capture region is only about 15%larger than the radius of the 1-step capture region.Therefore, there is not much to be gained in con-sidering 2-step capturability over 1-step capturabilitywith M2V2, until we can get the robot to swing itslegs more quickly.

With human walking, on the other hand, the min-imum swing time is approximately 0.3 seconds. Thisresults in a geometric ratio of approximately 0.4.Hence, the 2-step capture region for human walkingshould be relatively large, and even 3-step captura-bility should be considered.

9.3 Capturability Margin

We presented an experimental evaluation of the N -step capturability margin, where we used N = 1.Figure 15 and Figure 19 showed a significant decreasein this capturability margin when the robot was per-turbed, as expected. For both the balancing task and

the walking task, the capturability margin recoveredcompletely after taking one step.

According to the theoretical considerations pre-sented in Part 1, the capturability margin shouldnever increase unless a step is taken or an externalforce is exerted on the robot. Any other increasesin capturability margin are due to modeling errors,sensor noise and lack of exact knowledge about whenthe swing foot hits the ground.

9.4 Estimation of CoM position andvelocity

The algorithm presented, like most feedback controlalgorithms for bipedal walking, relies on a good esti-mation of the CoM location and velocity, particularlyin the horizontal plane. Obtaining such an estimateon a physical robot is difficult for several reasons.

Knowing the CoM projection on the ground re-quires knowing which way is down. A small error inthe perceived orientation of the body can result in asignificant error in the CoM projection. For exam-ple, if the CoM is at a height of 1 meter and thereis a 0.01 radian error in the body orientation, thatwill result in approximately a 1 cm error of the CoMon the ground. For 3D robots, orientation is typi-cally determined using an inertial measurement unit(IMU), and therefore having a good IMU and relatedsensor processing is important.

Using leg kinematics to estimate the CoM velocityhas the problem that one must assume that the footis not moving. However, if the foot is slipping, thisassumption will make the CoM seem to be movingin the opposite direction. On the other hand, inte-grating an accelerometer to estimate velocity has theproblem of error accumulation. For M2V2 we haveused a combination of leg kinematics and accelerom-eter measurements. We have not yet determined howaccurately our CoM velocity estimate is, but we be-lieve we can do much better and therefore improveperformance on the physical robot.

9.5 Uneven Ground

Both the models presented in Part 1 and the controlalgorithm presented in Part 2 assumed flat ground.

20

We have performed some preliminary simulationswith moderate slopes and relatively small steps, andso far it appears that the same models are applicableto these types of terrain. However, for large slopesand steps, and very rough ground, the models willlikely need to be expanded and further control strate-gies will need to be developed. In addition, we stillneed to develop capturability-based models and con-trol strategies for situations where hands can pushagainst walls or hold handrails, or where feet can beon different slopes. In these cases concepts like forceclosure will need to be taken into account, rather thanonly using the CoP.

9.6 Controlling Velocity versus Com-ing to a Stop

Capturability measures the ability of a legged systemto come to a stop in N steps or fewer. However, weare not usually interested in coming to a stop, butrather maintaining an average speed. Even thoughcapturability is based on the ability to come to astop, using tools based on capturability does not re-quire the legged system to come to a stop. Instead,capturability only specifies bounds on what is per-missible. Within these bounds, there is ample roomfor such things as controlling velocity, for example byCoP or foot placement.

9.7 Cross-over Steps

In this study, we only considered pushes during sin-gle support which did not require a cross-over step.Cross-over steps are challenging for a number of rea-sons. The swinging leg needs to make sure to not con-tact the support leg. To do that, the path of the legmay be longer, requiring longer swing time. Also, thelength of the step will be smaller than it can be whenthe leg swings to the outside. An alternative is toquickly step straight down with the currently swing-ing leg and then quickly swing the other leg to pre-vent a fall. This two-step recovery strategy requiresextra time to execute and for significant pushes willlikely only be successful for robots with a relativelyshort swing time, on the order of how fast humanscan swing their legs.

9.8 Virtual Toe Points and Center ofPressure

The presented control algorithm relies on the use ofvirtual toe points to track a desired CoP. Virtual toepoints can be interpreted as the attachment pointsof virtual actuators on the feet, and each virtual toepoint is used to gain approximate control of each in-dividual foot’s CoP. The virtual toe point and a foot’sCoP will be theoretically identical if the robot is insingle support, the vertical force of the virtual actu-ator equals the weight of the robot, and the verticalacceleration is zero. In simulation, the two pointsalways remain close (within a few cm) during sin-gle support. During double support, there can bea large error between the desired and actual CoP,particularly when one of the legs is stretched. Forexample, if the hind leg is completely stretched, andthe desired CoP is on the heel of the hind leg, thenthe virtual actuator on the hind leg will be assigneda large leg support fraction. However, since the legis straight, the actual joint torques that the virtualactuator produces will be small, and the CoP will belocated more forward than desired.

One way to obtain a better match between the de-sired and actual CoP is to keep the knees of the robotbent to avoid losing kinematic range. However, wewish to avoid that solution since human walking doesnot rely on bent knees and because it requires unnec-essarily high torques at the knees. Another solution,which we will investigate in future work, is to use toeoff on the rear leg to better control the CoP duringdouble support. Currently, some toe off occurs atthe end of the stride, but it is simply the result ofthe dynamics of the walk, rather than the result ofan intentional control action.

9.9 Foot Placement Speed and Accu-racy

In this paper, we showed how foot placement canbe used to regain balance after a push. Doing sorequires a fast swing that is accurate enough to makethe foot land in the capture region. However, due tothe use of SEAs with very compliant springs, we havehad difficulty to quickly and accurately swing the leg.

21

Although SEAs enable compliant control, they canmake joint position trajectory tracking challenging.We believe that we can achieve the same favorablecompliant control characteristics and better trackingby increasing the stiffness of the series springs. Onthe other hand, we also believe that swing can beperformed in a more compliant manner, determinedmostly by the passive pendulum dynamics of the leg,as opposed to using traditional high-gain trajectorytracking. Determining swing strategies that allow forfast and accurate steps while exploiting the naturaldynamics of the leg is an area of future work.

9.10 Application to Other Robots

We believe that capturability concepts can be ap-plied to the analysis and control of other legged sys-tems. Estimating capture regions and determiningcapturability-based robustness metrics should be pos-sible for all legged systems. While we advocate com-pliant force control for legged robots, most of thetechniques described in this paper should also ap-ply to high-gain position trajectory tracking robots.Stepping strategies that take the capture region intoaccount should be applicable to any robot that canchange where it steps on-the-fly. Control of the in-stantaneous capture point should be applicable toany robot that can control its CoP location on theground. We are currently expanding the algorithmspresented in this paper and working toward their ap-plication on several different humanoid robot plat-forms.

10 Conclusion

This paper showed an application of the N -step cap-turability framework to a 12-DoF bipedal robot. Themain contributions of this part were step locationadjustment using the 1-step capture region and in-stantaneous capture point control by CoP placement,which are key ingredients to the presented control al-gorithm. In addition, the 1-step capturability marginwas experimentally evaluated.

A Index to multimedia Exten-sions

See Table 1 for a description of the multimedia con-tent attached to this paper.

Table 1: Index to multimedia ExtensionsExtension Media Type Description1 Video Results of control algorithm im-

plementation, both on the phys-ical robot and in simulation.

B Robot parameters

Table 2 gives the joint layout and inertia parametersof M2V2. Each row represents a joint and its associ-ated rigid body. We use a coordinate system in whichx is forward, y is to the left, and z is up. Note thatthe bottom 6 rows represent the left leg joints andmasses (marked with the letter ‘L’). The right leg isa mirror image of the left leg, and thus is identicalto the left leg except for the y values, which are allthe additive inverse. For pin-type joints, the letterfollowing “Pin” refers to the rotational axis to whichthe joint is aligned.

Acknowledgment

The authors would like to thank A. Goswami and E.Westervelt for their helpful comments.

M2V2 is a second generation version of the M2robot, developed by G. Pratt, D. Paluska, J. Pratt,and D. Robinson at the MIT Leg Laboratory. M2V2was designed by B. Krupp, V. Ragusila, I. Olaru, T.Craig, and J. Pratt. M2V2 electronics and interfacesoftware was designed by G. Watkins, S. Emami, T.Hutcheson, J. Pratt, J. Smith, and S. Nayak. M2V2was assembled and maintained by T. Craig and J.Taylor. The force sensing push stick used in push re-covery experiments on M2V2 was designed and con-structed by J. Joyner.

A team at Bucknell University, led by S. Shooterand K. Buffinton, have designed improved feet and a

22

Table 2: M2V2 simulated robot joint and mass layout. Approximated from CAD models of real M2V2robot. The joint offsets are with respect to each joint’s parent joint and defined when all joint angles are 0.The CoM offsets are with respect to each joint origin. The moments of inertia are with respect to the CoMof each link.

Offset from Parent [m] COM offset [m] Moment of Inertia [kg m2]

Joint Parent x y z Joint Type Mass [kg] x y z Ixx Iyy Izz

Root N/A 0.0 0.0 0.0 Floating 24.0 0.0 0.0 0.0 2.756 2.756 0.380

L Hip Yaw Root 0.0 0.092 -0.381 Pin-Z 0.2 0.0 0.0 0.0 5.3E-4 5.3E-4 5.3E-4

L Hip Roll L Hip Yaw 0.0 0.0 0.0 Pin-X 0.0 0.0 0.0 0.0 0.0 0.0 0.0

L Hip Pitch L Hip Roll 0.0 0.0 -0.029 Pin-Y 4.6 -0.0318 0.006 -0.229 0.044 0.044 0.004

L Knee L Hip Pitch 0.0 0.0 -0.450 Pin-Y 4.3 -0.025 0.0 -0.229 0.054 0.054 0.003

L Ankle Pitch L Knee 0.0 0.0 -0.467 Pin-Y 0.11 0.0 0.0 0.0 2.6E-4 2.6E-4 2.6E-4

L Ankle Roll L Ankle Pitch 0.0 0.0 0.0 Pin-X 0.727 0.0127 0.0 -0.051 3.63E-4 0.002 0.002

vision head for the robot. Students involved includeM. Kandler, D. Snyder, L. Markison, J. Ricci and C.Hubicki.

Various low-level control modules for M2V2 weredeveloped by C. Shake, N. van Nieuwenhuizen, F.Canas, D. Garg, S. Tamadoni, R. van Doesburgh, T.Koolen, J. Pratt, M. Johnson, and P. Neuhaus. Ateam from NASA JSC, consisting of J. Braman, D.Gooding, S. Tamblyn, M. Goza and A. Hulse, hasalso contributed in this area.

M2V2 filming and video editing was performed byW. Howell. Part acquisition and lab managementwas performed by B. Layton.

Simulation Software and user interfaces were im-proved by J. Carff, M. Fortenberry, D. Reyes Duran,G. Barr, B. Waxler, and P. DeMonaco.

References

[1] J. Chen, J. Cheng, and G. Yu. Virtual modelcontrol of a quadruped walking robot. J. Shang-hai Jiaotong Univ., 35(12):1771–1775, 2001.

[2] C. Chevallereau, J. W. Grizzle, and C. Shih.Asymptotically Stable Walking of a Five-LinkUnderactuated 3-D Bipedal Robot. IEEE Trans.Robot., 25(1):37–50, Feb. 2009.

[3] S. H. Collins, A. Ruina, R. Tedrake, andM. Wisse. Efficient Bipedal Robots Based

on Passive-Dynamic Walkers. Science,307(5712):1082–1085, Feb. 2005.

[4] S. H. Collins, M. Wisse, and A. Ruina. A Three-Dimensional Passive-Dynamic Walking Robotwith Two Legs and Knees. Int. J. Robot. Res.,20(7):607–615, July 2001.

[5] S. Coros, P. Beaudoin, and M. van de Panne.Generalized biped walking control. In Proc. SIG-GRAPH 2010, pages 1–9, New York, NY, USA,July 2010. ACM Press.

[6] R. Featherstone. Robot dynamics algorithms.Kluwer Academic Publishers, Norwell, MA,USA, 1987.

[7] R. Featherstone. Rigid body dynamics algo-rithms. Springer US, Boston, MA, USA, 2008.

[8] T. Geng, B. Porr, and F. Worgotter. Coupling ofneural computation with physical computationfor stable dynamic biped walking control. NeuralComput., 18(5):1156–1196, 2006.

[9] J. J. Hu, J. E. Pratt, C.-M. Chew, H. M. Herr,and G. A. Pratt. Adaptive virtual model con-trol of a bipedal walking robot. In Proc. 1998IEEE Int. Joint Symp. Intell. Syst., pages 245–251, 1998.

[10] J. J. Hu, J. E. Pratt, C.-M. Chew, H. M. Herr,and G. A. Pratt. Virtual model based adaptive

23

dynamic control of a biped walking robot. Int.J. Art. Intel. Tools, 8(3):337–348, 1999.

[11] S.-H. Hyon and G. Cheng. Disturbance Rejec-tion for Biped Humanoids. In Proc. 2007 IEEEInt. Conf. Robot. Automat., pages 2668–2675,Apr. 2007.

[12] S.-H. Hyon, R. Osu, and Y. Otaka. Integrationof multi-level postural balancing on humanoidrobots. In Proc. 2009 IEEE Int. Conf. Robot.Automat., pages 1549–1556. IEEE, May 2009.

[13] S. Kagami, T. Kitagawa, K. Nishiwaki, T. Sug-ihara, M. Inaba, and H. Inoue. A Fast Dynam-ically Equilibrated Walking Trajectory Gener-ation Method of Humanoid Robot. J. Auton.Robots, 12(1):71–82, 2002.

[14] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara,K. Harada, and K. Yokoi. Biped walking pat-tern generation by using preview control of zero-moment point. In Proc. 2003 IEEE Int. Conf.Robot. Automat., pages 1620–1626, Sept. 2003.

[15] A. D. Kuo. Stabilization of Lateral Motion inPassive Dynamic Walking. Int. J. Robot. Res.,18(9):917–930, 1999.

[16] T. McGeer. Passive walking with knees. In Proc.1990 IEEE Int. Conf. Robot. Automat., pages1640–1645, Cincinnati, OH, USA, May 1990.

[17] B. V. Mirtich. Impulse-based dynamic simula-tion of rigid body systems. PhD thesis, Univer-sity of California, Berkeley, 1996.

[18] K. Nishiwaki and S. Kagami. Strategies for Ad-justing the ZMP Reference Trajectory for Main-taining Balance in Humanoid Walking. In Proc.2010 IEEE Int. Conf. Robot. Automat., pages4230–4236, May 2010.

[19] D. J. Paluska. Design of a Humanoid Biped forWalking Research. M. eng. thesis, MassachusettsInstitute of Technology, 2000.

[20] J. Perry. Gait Analysis: Normal and Pathologi-cal Function. Slack, Thorofare, NJ, USA, 1992.

[21] G. A. Pratt. Legged Robots at MIT: what’snew since Raibert? IEEE Robot. Autom. Mag.,7(3):15–19, 2000.

[22] G. A. Pratt and M. M. Williamson. Series elasticactuators. In Proc. 1995 IEEE/RSJ Int. Conf.Intell. Rob. Syst., pages 399–406, 1995.

[23] J. E. Pratt, J. Carff, S. V. Drakunov, andA. Goswami. Capture Point: A Step towardHumanoid Push Recovery. In Proc. 2006 IEEE-RAS Int. Conf. Humanoid Rob., pages 200–207.IEEE, Dec. 2006.

[24] J. E. Pratt, C.-M. Chew, A. Torres, P. Dilworth,and G. A. Pratt. Virtual model control: an in-tuitive approach for bipedal locomotion. Int. J.Robot. Res., 20(2):129–143, Feb. 2001.

[25] J. E. Pratt, P. Dilworth, and G. A. Pratt. Vir-tual model control of a bipedal walking robot.In Proc. 1997 IEEE Int. Conf. Robot. Automat.,pages 193–198, Apr. 1997.

[26] J. E. Pratt and B. T. Krupp. Design of a BipedalWalking Robot. In Proc. 2008 SPIE, volume69621F, 2008.

[27] J. E. Pratt and G. A. Pratt. Intuitive control ofa planar bipedal walking robot. In Proc. 1998IEEE Int. Conf. Robot. Automat., volume 3,pages 2014–2021, May 2002.

[28] J. E. Pratt and R. Tedrake. Velocity-basedstability margins for fast bipedal walking. InM. Diehl and K. Mombaur, editors, Fast Mo-tions in Biomechanics and Robotics, volume 340of Lecture Notes in Control and InformationSciences, chapter 14, pages 299–324. SpringerBerlin Heidelberg, 2006.

[29] K. Sreenath, H.-W. Park, I. Poulakakis, andJ. W. Grizzle. A Compliant Hybrid Zero Dy-namics Controller for achieving Stable, Efficientand Fast Bipedal Walking on MABEL. Int. J.Robot. Res., submitted for publication.

[30] B. J. Stephens and C. G. Atkeson. Dynamicbalance force control for compliant humanoid

24

robots. In Proc. 2010 IEEE/RSJ Int. Conf. In-tell. Rob. Syst., Oct. 2010.

[31] T. Sugihara. Consistent Biped Step Controlwith COM-ZMP Oscillation Based on Succes-sive Phase Estimation in Dynamics Morphing.In Proc. 2010 IEEE Int. Conf. Robot. Automat.,pages 4224–4229, May 2010.

[32] F. Tan, C. Fu, and K. Chen. Biped Blind Walk-ing on Changing Slope with Reflex Control Sys-tem. In Proc. 2010 IEEE Int. Conf. Robot. Au-tomat., pages 1709–1714, May 2010.

[33] M. Vukobratovic and B. Borovac. Zero-MomentPoint — Thirty Five Years of its Life. Int. J.Human. Robot., 1(1):157–173, 2004.

[34] E. R. Westervelt, G. Buche, and J. W. Griz-zle. Experimental Validation of a Frameworkfor the Design of Controllers that Induce StableWalking in Planar Bipeds. Int. J. Robot. Res.,23(6):559–582, 2004.

[35] E. R. Westervelt, J. W. Grizzle, and D. E.Koditschek. Hybrid Zero Dynamics of PlanarBiped Walkers. IEEE Trans. Autom. Control,48(1):42–56, Jan. 2003.

[36] P.-B. Wieber. On the stability of walking sys-tems. In Proc. Int. Workshop Humanoid Hum.Friendly Robot., pages 53–59, Tsukuba, Japan,2002.

[37] Yobotics, Inc. Yobotics Home. LeggedRobots, Simulation, Actuators., Mar. 2011.http://www.yobotics.com/.

25