Optimization based Exploitation of the Ankle Elasticity of HRP-2 forOverstepping Large Obstacles

Kai Henning Koch1 Katja Mombaur1 Olivier Stasse2 Philippe Souères2

Abstract— This paper proposes a new generic strategy toinvestigate the dynamic limits of the humanoid robot HRP-2based on whole body optimal control optimization. In this studywe exploit the intuitive access to complex motion characteristics,given by optimal control, to effectively resolve a major technicalcoupling effect, namely between the ankle elasticity and thestabilizing algorithms. Control efforts are reduced to get aclearer view of the actual system limits and to exploit itscapacities at maximum. As showcase we decided to focus ona stepping motion over a cylindrical obstacle. This study isfurther supported by real experiments on the HRP-2 14 roboticplatform and the present maximum of a dynamically over-stepped obstacle was extended to 20cm (height) x 11cm (width)(including safety margin) without multi-contact support.

I. INTRODUCTION

Humanoid robots are some of the most exciting machinesin the world and they come, at the same, time with a highlycomplex design of its overall system architecture. At onehand this complexity is essential to the capacity of bipedallocomotion (e.g. redundancy & under-actuation). At the otherhand, it poses further difficulties as soon as one tries toinvestigate or even improve its motion characteristics towardsmore human-like/natural locomotion trajectories.Even though humanoid robots have been conceived in thehope to efficiently exploit the advantage of bipedal locomo-tion to robustly cope with irregular terrain conditions, theserobots are eventually struggling with serious problems, assoon as it comes to more challenging situations like, step-ping over large obstacles. The possibility to overcome largeobstacles is a particular advantage of legged, and especiallybipedal systems over their wheeled counterparts. Limitationsto a flexible and robust operation for these robots are atsome point certainly related to the given hardware and thecontrol system. Besides, some limits may as well originatefrom the employed algorithms for motion generation andthe technological inter-dependencies between control systemand the underlying hardware. As some of the frequentlyemployed well-performing algorithms for computing step-ping trajectories are very conservative, full exploitation ofthe capabilities of hardware and control system may not bepossible at some point.

henning.koch at iwr.uni-heidelberg.de,katja.mombaur at iwr.uni-heidelberg.de, 1ORB

Optimisation in Robotics and Biomechanics, IWR University of Heidelberg,INF 368, 69120 Heidelberg, Germanyostasse at laas.fr,psoueres at laas.fr, 2GEPETTO, LAAS-CNRS

TOULOUSE, avenue du Colonel Roche 7, 31031 Toulouse cedex 4,France

Fig. 1. Image sequence of the robot stepping over the obstacle

A. Related research

The research field of motion generation based upon aphysical model - more specifically walking motions (see[23]) - has the common target to efficiently identify themost suitable motion trajectory, among a sub-manifold offeasible candidate solutions, with the desired characteristics(e.g. stability). Depending on the desired characteristicsof the final generation algorithm one may adopt differentsimplifications. For example on the physical model - invertedpendulum [7] and heuristics to lower the complexity of thegeneric problem formulation.On one hand, a very successful heuristic method for stablewalking motions is the ZMP-based pattern generation[7],[21]. From a desired ZMP trajectory and planned foot-steps, based on a simplified model, one may compute thetrajectory of the center of mass (in the following CoM)and, with further assumptions, the whole body motion by

Fig. 2. Image sequence of the robot stepping over the obstacle

inverse kinematics or resolved momentum control. Apartfrom the ZMP based pattern generation, Englsberger et.al. [2] have introduced a method featuring the divergentcomponent of motion. Most of these pattern generators areeasily parametrized, re-actively controllable, but are implic-itly bound to the previously made assumptions in form ofmodel-simplifications and heuristics. How these limitationsfinally translate to the resulting motion is however not alwaysclear.On the other hand, optimization based approaches keep amore generic form of the problem formulation. In conse-quence their solution generally involves the efficient com-putation of non-linear programming problems. Thus thehigh mathematical complexity of these approaches does notpermit motion generation in real-time, but gives a moreintuitive access to desired motion characteristics based onhigh-level objectives. Early works on gait cycle optimizationbased on forward dynamics of whole body optimal controlhave been conducted by Goswami et. al.[18]. Mombauret.al. [19] investigated human-like running based on wholebody optimal control using multiple shooting techniques. Thestack of tasks [17] is a highly efficient whole body dynamicsmotion generation method with task prioritization. Erez et.al[3] generates running gaits based on inverse dynamics underexternal contacts.Since bipedal locomotion devices are meant to move around

in unstructured environment, various approaches exist toclear obstacles with humanoid robots - ASIMO [14] andHRP-2 [5], [20]). Guan et. al. [5] conducted a kinematicfeasibility analysis of obstacles to be overstepped concludingon a maximum obstacle of 24.21cm x 5cm (width x height).Dynamic overstepping was then achieved in [20] as partof a walking pattern, in simulation and real experimentpushing the maximal values to 18cm x 11cm (width x height)including a safety margin of 3cm. The motion is planned forankle and hip (including twist) trajectories from a previousgeometric feasibility analysis. The whole body motion isthen generated based on the powerful ZMP preview controlpattern generator from Kajita et.al. [7].Apart from the general on-line or off-line motion generationprocess a very difficult task is to stabilize the humanoid robotin real-time against external perturbations during the actualmotion performance. In all robots from HRP (HumanoidRobotics Project - METI [6]) this on-line control componentis called the stabilizer [9] and accomplishes most commonlya posture and a force control loop. This control is achievedover a passive elasticity [10] that parallely reduces the shockeffects from foot to ground impacts [15]. From the viewpointof motion performance the complete system mostly operatesas if the elasticity would not be present [20].

B. Contribution of this article

This article proposes a new approach based on wholebody optimal control to investigate the potential limits fordynamically highly challenging motions. It is based on thecommon idea that, in case one would like to push the robotto its dynamic limits, the stress on the control system shouldby minimized. In consequence a candidate solution should becomputed such that particularly the control action of the sta-bilizer is minimal. In this respect, the control system shouldbe able to operate safely, despite perturbations, originatingfrom model deficiencies and the external environment.The concept is applied to the dynamically highly challengingcase of overstepping obstacles with the robotic platformHRP-2, performing only two steps and with a static postureas initial and final boundary condition. The problem isformulated as a multi-phase optimal control problem, basedon the whole body dynamic model, including limitations onjoint angle ranges, velocity as well as the actuation systems.Furthermore, abstract criteria such as dynamic stability andintensity of stabilizing control actions are considered. Thesolution is solely based on the governing physics - any otherinformation such as phase timing and joint angle trajectoriesare computed during the process and do not need to bespecified in advance.Finally, the strategy is verified with motion experimentson the real robotic platform HRP-2 14 of LAAS-CNRS inToulouse.

II. MODELING

A. Dynamic Modeling of the robot HRP-2

The chosen robotic platform for this analysis is the HRP-2 14 from LAAS-CNRS Toulouse. Technical information is

given in [11]. As it is essential to modeling in optimal controlthat simplifications are carefully chosen to preserve thegoverning physical characteristics at maximum - a detailedoutline is given below:The robot HRP-2 [11] is controlled by high gain positioncontrol driving brushed DC motors through gearboxes with ahigh reduction ratio. As the position control carefully rejectsexternal perturbation, the joint trajectory reference governsthe motion characteristic, while the torque profile only givesan estimation about the mechanical energy characteristics.Thus, dry and viscous friction representing an offset to thetorque profile is not considered. The reduction ratios aresufficiently high to neglect dynamic coupling of the rotorinertia and the kinematic structure. Further decision wasmade to assume the gearbox as perfectly rigid to reducemathematical complexity.In contrast to its predecessors [6], the kinematic structureof the robot has been conceived for maximum stiffness [11]and mass concentration in the pelvis (hosting the batteries).Consequently, the kinematic structure is assumed perfectlyrigid.The feet of the robot are rubber coated [15] and mountedwith elastic bushings underneath a 6D force sensor to theankle joint complex. While the elastics of the rubber coatingare mostly negligible, the ankle elasticity mechanism [10],[9] serves not only as shock absorber during bipedal locomo-tion. It is the physical component of the force torque controlloop, built on top of the underlying joint position controlloop to track the ZMP [21] reference for dynamically stableoperation. The employed two Dof torque controller [10] isoptimized for maximum backdriveability, hence distortion-free frequency response for the reference signal is onlyguaranteed in low frequency space. Hence the elasticity ismodeled as 2D rotational spring-damper system. The verticalelasticity effect will be neglected during this analysis for thesake of simplicity.The previously mentioned control system is part of a stabi-lizing algorithm that accomplishes the following tasks [9]:Control of the pelvis postures based on the estimation of theorientation from the IMU in the upper torso and heuristics-based distribution of the vertical force on both feet as wellas the contact torque. Torque and vertical force are directlyrelated to the local center of pressure (in the followingCoP) and the ZMP as long as the feet are firmly placedon the ground [21]. The control reference is obtained fromthe dynamics of the linear inverted pendulum [9]. In theformer variant, the angular momentum change of the bodyis not considered for control. The correction of the trajectoryprofile is distributed over the lower body by means of inversekinematics while turning around the CoM of the robot [8].As it is not clear which of the present stabilizer variants iscurrently implemented on HRP-2 14, the most probable andconstraining option is considered for this analysis.

B. Stepping over motion

The generation process is formulated as multi-phase hy-brid dynamic, optimal control problem with continuous, as

0 2 4

−5

0

5

·10−2

t in [s]

Ang

lein

[RA

D]

Torso - roll

MUSCOD

MS Nodes

OpenHRP

HRP-20 2 4

−5

0

5

·10−2

t in [s]

Torso - pitch

0 2 4

0

0.2

0.4

t in [s]

ZM

Pin

[m]

ZMP in X

Real ZMP MS Nodes

Table Cart ZMP MS Nodes

0 2 4

−0.1

−5 · 10−2

0

5 · 10−2

0.1

t in [s]

ZMP in Y

Fig. 3. Comparison of horizontal components of the torso orientationthat are computed during optimization (MUSCOD), and estimated in thesimulation of the virtual robot (OpenHRP), as well as during the experimenton HRP-2. Below are plots of the different trajectories of real whole-bodyZMP and the highly simplified table cart ZMP.

well as discontinuous phase transitions (see Figure 1):The motion initiates from a static posture towards unloadingthe right foot (1-3, highlighted in grey). The right foot liftsoff ground and travels over the obstacle (4-6). Following theground impact of the right foot, a short double support phase(highglighted in grey) is used to transfer the body supporton the right leg and unload the left foot (6-7). Then theleft foot lifts off ground and is retired over the obstacle (7-9). After ground impact of the left foot, the robot enters are-stabilization phase that ends in a static posture (10-12,highlighted in grey).The employed problem formulation has the advantage thatthe motion characteristics (joint trajectories, phase timings,torque profiles) are determined during the optimization pro-cess, based on abstract high-level objectives and the gov-erning physics. Phase transitions are determined by implicitswitching constraints e.g. a foot may lift off ground assoon as the contact reactions vanish. Discontinuous phasetransitions are assumed to be perfectly inelastic. A foot thatreaches ground level collides.

C. Equations of Motion

The robot is modeled in minimal coordinates withoutcontacts, with 30 Dof for its internal branched tree structure,4 Dof to account for its elasticity in the ankle, as well asfurther 6 Dof to model the free motion in the global refer-ence. This coordinate configuration is preserved throughoutthe complete phase cycle featuring position contacts andkinematic loops. The resulting dynamic equation is thenexpressed as a DAE system of index 3 formulated in the

0 2 4−40

−20

0

20

t in [s]

Torq

uein

[Nm

]Left Foot GCR ::Mx::

0 2 4

0

200

t in [s]

Forc

ein

[N]

Left Foot GCR ::Fx::

0 2 4−40

−20

0

20

t in [s]

Torq

uein

[Nm

]

Left Foot GCR ::My ::

0 2 4

0

200

t in [s]

Forc

ein

[N]

Left Foot GCR ::Fy ::

0 2 4−40

−20

0

20

t in [s]

Torq

uein

[Nm

]

Left Foot GCR ::Mz ::

MUSCOD MS Nodes

OpenHRP HRP-2

0 2 4

0

200

400

600 Limit

t in [s]

Forc

ein

[N]

Left Foot GCR ::Fz ::

Fig. 4. Comparison of the left foot ground contact reactions for optimiza-tion results (MUSCOD), the simulation of the virtual robot (OpenHRP), aswell as during the experiment on HRP-2. Close trajectories are observablefor the contact forces Fx,Fy, Fz (vertical force) and the Mz (vertical torque).The stabilizing algorithms manipulate the components Mx, My as well asFz. The vertical force limit should be kept firmly, however small peaks arenot a problem.

so-called descriptor form:

M (q, p) q̈ + NLE (q, q̇, p)

+ C (q, p)− J (q, p)T λ = τ(1)

g (q, p) = 0 (2)

In this equation M (q) represents the joint space inertia ma-trix which consists of the inertia matrix from the kinematictree structure of the robot

M =

(∑k

JTk IkJk

)+Mm (3)

and an additional diagonal term Mm (4) containing thesimplified dynamic effects of the spinning coils in the jointactuators (where Imi is the rotational inertia of the coil iabout its spinning axis and Ri the ratio of transmission ofthe joint i):

Mmii =

[R2i I

mi i < N

0 i ≥ N

](4)

NLE (q, q̇) represents the nonlinear effects such as coriolisforces. Furthermore, we include here the dynamic effects

0 2 4

0

1

2

Limit

Security

t in [s]

Ang

lein

[RA

D]

Right leg - Knee Y

0 2 4

0

1

2

t in [s]

Left leg - Knee Y

0 2 4

−0.5

0

0.5

t in [s]

Ang

lein

[RA

D]

Right leg - Ankle Y

0 2 4

−0.5

0

0.5

t in [s]

Left leg - Ankle Y

0 2 4

−0.4

−0.2

0

0.2

Security

t in [s]

Ang

lein

[RA

D]

Right leg - Ankle X

MUSCOD MS Nodes

OpenHRP Ref-OpenHRP

HRP-2 Ref-HRP-2

0 2 4

−0.4

−0.2

0

0.2Security

t in [s]

Left leg - Ankle X

Fig. 5. Comparison of some joint trajectories for the lower torso part,for optimization results (MUSCOD), the simulation of the virtual robot(OpenHRP), as well as during the experiment on HRP-2. The controlreference (scattered line) and the system’s output (solid line) are drawnseparately for verification. The limit marks the absolute maximum anglerange for the corresponding joint. The security limit was the actual limitchosen during the optimization to enforce safe operation. The dashedgray/red rectangle show parts of the trajectory where the stabilizer activelymanipulated the motion to preserve dynamic stability. In red rectangles eventhe simulation and the real robot act substantially different.

of the spring and damper systems. The following analysis isbased on parameters chosen to suite the model requirements.

NLE =∑k

JTk

(IkJ̇kq̇ − q̇TJTk × IkJkq̇

)+KD q̇ +Kpq

(5)

The spatial Jacobian to the local reference frame of the link kis represented by Jk, respectively. The spatial inertia matrixof link k is Ik. KD and KP are matrices that hold springand damping constants with respect to each of the degrees offreedom [15]. Gravity effects are considered with the termC (q). The system’s actuation is represented by τ as torqueon the joint-level (rotational). The term g (q) expresses thescleronome position constraints.For higher computational efficiency, this is transformed into

0 2 4

−5

0

5

Limit

·10−2

t in [s]

CoP

in[m

]Local CoP left foot in X

0 2 4

−5

0

5

Foot

Limit

·10−2

t in [s]

Local CoP left foot in Y

0 2 4

−5

0

5

Limit

·10−2

t in [s]

CoP

in[m

]

Local CoP right foot in X

MUSCOD MS Nodes

OpenHRP HRP-2

0 2 4

−5

0

5

Limit

Foot

·10−2

t in [s]

Local CoP right foot in Y

Fig. 6. Plot of the trajectories of the local center of pressure of eachfoot in X and Y for optimization results (MUSCOD), the simulation of thevirtual robot (OpenHRP), as well as during the experiment on HRP-2. Highpeaks occur during the transition phase caused by the vertical contact forcedropping to 0. The mark foot represents the physical boundary of the foot.As soon as the CoP reaches this boundary the contact stability is breached.The mark limit represents the constraint that was used during optimization.

a DAE system of index 1:

q̇ = v (6)v̇ = a (7)[

M −JTJ 0

] [aλ

]=

[−NLE − C + τ

−γ

](8)

In forward dynamics algebraic variables λ, equivalent tothe contact constraint forces and acceleration q̈ values arecomputed from given a system’s state q, q̇, τ . Additionally,the following conditions arising from the index reductionhave to be respected for the sake of consistency:

g (q (0)) = 0 (9)J (q (0)) q̇ (0) = 0 (10)

The term J is the classical (not the spatial!) analytical 6DJacobian and γ = ∂∂t (J) q̇ its time derivative multipliedby the velocity vector, respectively, that contains all activeconstraint directions in the contact points. When a foot entersin contact with the ground, this is modeled as an inelasticimpact causing discontinuities in the joint velocities whichcan be computed by the following equation. The velocityconfiguration before and after the impact is represented byq̇− and q̇+, respectively and Λ holds the complex impactimpulsion: [

M −JTJ 0

] [q̇+

Λ

]=

[Mq̇−

0

](11)

The dynamic equations are composed analytically, optimizedand converted into C-code by means of our own dynamic

model builder DYNAMOD based on 6D spatial geometry[4] and symbolic code generation [22].

III. GENERATION OF OPTIMAL STEPPING MOTIONS BYOPTIMAL CONTROL TECHNIQUES

In the following, the implementation of abstract motioncharacteristics in the problem formulation will be intensivelyexploited to generate motions that will rely on the wholebody dynamics, but at the same time minimize the controleffort of the stabilizing algorithm.

A. The optimal control problem

In order to generate the stepping motion over the obstacle,a multiple-phase optimal control problem is formulated. Anoptimal control problem is an optimization problem wherethe unknown variables are functions in time and dynamicequations are respected as constraints. The general formu-lation (12-19) of this problem uses the time t, the systemstates x(t), the controls u(t) and system parameters p.

mint,x,u,p

k∑i=1

∫ t̄it̄i−1

Φi (x(t), u(t), p) dt+ Ψi (t̄i, x (t̄i) , p) (12)

subject to ẋ(t)− fi (t, x(t), u(t), p) = 0 (13)x(t̄+i)− hi

(x(t̄−i))

= 0 (14)

req(x(0), T, x(T ), t̂0, x

(t̂0), ..., t̂s, x

(t̂s, p

))= 0 (15)

rineq(x(0), T, x(T ), t̂0, x

(t̂0), ..., t̂s, x

(t̂s), p)≥ 0 (16)

gi (x(t), u(t), p) ≥ 0 (17)u ≤ u ≤ u (18)x ≤ x ≤ x (19)

The objective function (12) consists of a continuousLagrange-term (Φ) and an end time related Meyer-term(Ψ) for each phase. Optimization is done with respect tothe system dynamics (13), equality and inequality boundaryconstraints (15, 16) for all phases, implicit phase switches(see II-B) are implemented as boundary constraints here,continuous path constraints (17) as well as box-constraints(limits) on system states (19) and controls (18).i ∈ M, M = {0...k} regroups all indices of the discretephases. t̄+i and t̄

−i denote the specific instant of phase

transition, the former before and the latter after the phasetransition. Without loss of generality it is assumed thatt̂s = 0. fi is the phase dependent right hand side of a firstorder ODE.Specifically in this problem formulation the state vector ofthe system x = [q, q̇, τ ]T ∈ R110 is comprised of jointpositions q ∈ R40, velocities q̇ ∈ R40 (reduction to firstorder ODE) and the joint torques τ ∈ R30. The controlsu ∈ R30 are injected into the system as torque derivatives τ̇to assure continuity of joint torque trajectories during phasetransitions. In previous studies, this characteristic improvedas well the quality of the approximated system input. hiexpresses characteristics of the phase transition (i−1)→ (i)(e.g. inelastic impact). fi, req, rineq, gi are vector functions

with correspondent dimension. Besides the desired final mo-tion characteristics a decision whether a characteristic mustbe enforced as constrained or objective must be carefullymade.

B. Objectives

The objectives are divided into essential (dominant) andoverall (sub-dominant) motion characteristics achieved, byindividual weights ωx in the objective function. Theseweights are tuned, for the time being, manually, to minimizethe overall tracking error of the control system, analysedfrom successive experiments on the hardware.

1) Minimal excitement of ankle elasticity: From the previ-ous presentation (see I-A and II) it is clear that the quality ofthe backdriveability of the controller is only preserved in thelow frequency space, hence allowing only small values forthe acceleration and velocity of the corresponding degreesof freedom. As the incorporated elasticity model is only alinear approximation, it is only expected to be valid in asmall region around the neutral working point:

min ΦElasticity = ωAcc∑

i∈Elastq̈i

2

+ωV el∑

i∈Elastq̇i

2 + ωPos∑

i∈Elastq2i

(20)

2) Minimal linear vertical momentum of CoM: The qual-ity of the simplified modeling of the elasticity may befurther improved when excessive oscillation of the verticalforce on the elasticity is prevented, as these effects areassumed to be substantially correlated. Furthermore, thisassumption minimizes the height variations of the CoM andthus deviation from the linear inverted pendulum mode [7]a central assumption of the stabilizer [9].

min ΦLinear Momentum = ωLinMomZ (LzCoM )

2

+ω∂LinMomZ

(∂

∂tLzCoM

)2 (21)3) Minimal vertical angular momentum change about

CoM & vertical contact shear torque: As a motion todynamically overstep an obstacle may drive the foot contactduring single support to its adhesive limits, this objectivecombination helps, to minimize the necessary vertical torqueto be canceled in the foot contact and at the same timeexcessive motions with the upper torso.

min ΦAngular Momentum = ω∂AngMomZ

(∂

∂tHzCoM

)2(22)

min ΦContact Torque = ωContactZ(fmZContact foot

)2(23)

4) Minimal deviation between whole body ZMP and tablecart ZMP: The difference between the whole body ZMP[21] and the 3D-LIPM [9], as it seems to be applied in thestabilization algorithm, is the effect of the change of thehorizontal components of the angular momentum about theCoM. These could be used to cancel out the accelerationeffect of the CoM about the ground. In the reverse conclu-sion, it is possible that the optimization tends to exploit this

effect to improve the dynamics of the motion, but hence failsto comply with the assumption of the stabilizer. This wouldprovide the risk of highly unstable motions, a situation thatshould be avoided if possible. Consequently, the euclideannorm of the geometric difference is minimized.

min ΦZMP - 3D-LIPM =∣∣ZMP x,ywhole body − ZMP x,yTable Cart∣∣22 (24)

5) Minimal performance time: As motion capacities ofhumanoid robots are still far behind those of humans, it willbe interesting to investigate how fast the robot platform HRP-2 is capable to clear the obstacle in simulation within its strictkinematic and dynamic limits:

min Ψtime = TGlobal End-time (25)

6) Head stabilization: In [16] it was reported that angularstabilization of the head and gaze is essential for a dynamicpostural control during various manipulation and locomotiontasks. At one hand, the desired motion represents a bipedallocomotion problem and at the other hand a tedious posturalbalance problem. Apart from the fact that HRP-2’s IMUis not located in the head, but in the chest [11], it isconcluded that head stabilization, towards the global target,is an essential characteristic to improve postural stability andthe general appearance (go, global orientation).

min ΦHead = (go)Yaw(x)2 + ((go)Pitch(x)− pPitch-Offset)2

+(go)Roll(x)2(26)

7) Minimal variations of squared torques: As stated in[19] it is possible to improve the motion-quality further byminimizing sub-dominantly the squared first derivative of thejoint torques. This criteria was mostly integrated for technicalreasons to prevent huge control oscillations:

min Φtorque =

30∑j=1

(uj)2 (27)

C. Path constraints

For computational efficiency it is crucial to keep math-ematical complexity at the lowest possible level. Besidesneglected physical effects, one needs to employ variouscontinuous path constraints to prevent unrealistic physicalbehavior of the system during the simulation.

1) Foot contact/clearance: As the foot to ground contactis uni-lateral, a valid contact state necessitates pressure in thecontact surface. Identification of this condition is done basedon the vertical contact constraint force and the CoP boundto a subspace of the contact surface [17]. For this analysisthe feasible area is reduced to square of 1[cm] x 1[cm] aswe will explain in the result section.Slipping was not explicitly modeled in the foot contact andhence an adequate contact state needs to be enforced. In afirst approximation the vertical contact force component isused to establish a maximal admissible horizontal force com-ponent and a maximal admissible vertical torque component,base on the Coulomb friction cone.Friction effects, between foot and ground, are not consideredduring swing. Consequently, a foot clearance is enforced

based on a time dependent smooth curve of minimal heightof the lowest point of the foot fold to ground distance.

2) Self Collision: An important issue in humanoid robotmotion generation is self-collision. As joints are positioncontrolled, unexpected contact during a performed motionusually leads to mechanical deterioration of either the kine-matic structure or the concerned actuation system. From thecomplex kinematic structure the collision free workspace ishighly scattered and even during simple dynamic motions,like reaching or walking, self collision between variouslinks is very likely to occur. Additionally, possible collisionswith the given obstacle the robot has to step over need tobe avoided. For collision detection a simple line geometryapproach based on cylinders [12] but with rounded caps isemployed.

3) Pelvis stabilization: In preliminary trials, the optimalcontrol problems relayed mainly on the upper torso dynamicsto stabilize the whole body motion. As firstly the quality ofthe dynamic model in the upper torso section is considerablydoubted and secondly the effect of a highly dynamic re-orientation of the pelvis section on the stabilizing algorithmsis not yet clear, the decision was made to keep roll and pitchorientation in tight angular ranges (-0.005, +0.005) [RAD].

D. Solving optimal control problems

The proposed optimal control problem formulation issolved with the framework MUSCOD II, developed at theUniversity of Heidelberg. Based on early works of Bock andPlitt [1], it has been implemented by Leineweber [13].The core algorithm consists of a direct multiple shootingmethod. First, a discretization of the system controls intoa finite set of additional system parameters is performed,based on an approximation with parametric model functions(piecewise constant/linear, splines) on a given time grid. Thechoice of the time grid determines the quality governingthe approximation of the controls. The system trajectory isfurther parametrized as a series of initial value problemswith additional matching conditions to assure continuityacross the multiple shooting intervals. Values for the objec-tive function, boundary and path constraints, including allsensitivities of the trajectory with respect to initial conditionsand parameters are then computed through internal numericaldifferentiation to allow for high accuracy [13]. Upon thisinformation a large but highly structured general nonlinearprogram is formulated.This NLP is then solved based on a sequence of equivalentlycondensed QP sub-problems, where additional parameters ofthe system trajectory are recursively eliminated. In our set-up, time grids for control and state trajectory discretisationare the equivalent.Optimization was carried out in three subsequent steps. First,the simulation setup was employed to compute a two stepwalking motion from static half-sitting posture. As soon as afeasible walking trajectory was found after a few iterations,the obstacle was included in the setup to optimize a steppingmotion over the obstacle until a certain obstacle heightwas reached. Finally, the desired trajectory characteristics

were optimized. The approach is generic such that differentobstacles and heights as well as step lengths are directlyadjustable from constraints and objectives, respectively.

IV. RESULTS

As the interest of this analysis is not to investigate themaximum obstacle height, but to assess the quality of this ap-proach, optimization has not been tuned towards a maximumobstacle height, but a suitably smooth motion combined witha reasonably obstacle height for safe investigation during realexperiments. Thus we chose an obstacle height 20cm (height)x 11cm (width) including safety margin.The obstacle is successfully cleared in 4.32 [s] (result ofoptimization), in 4.33 [s] with the virtual robot in simulationand 4.34 [s] for the real robot platform, respectively. Theoptimization freely chooses a step-length of 0.415 [m] anda relatively narrow foot arrangement of 0.17 [m] betweenfeet to clear the obstacle. The robot starts from a nearlysymmetric posture and a narrow feet arrangement (0.16 [m]lateral offset, 0.005 [m] sagittal offset) into a slightly largerarrangement to re-stabilize the robot (0.19 [m] lateral offset,0.09 [m] sagittal offset) into the final static posture (seeFigure 1 and 2). The simulation of the virtual robot and thereal experiment showed a smooth motion without slipping,medium ground impact collisions and no destabilizing.The analysis of the objectives revealed that the optimizationconverged mostly with desired motion characteristics asdominant objectives: minimum excitement of the ankle elas-ticity in horizontal and vertical direction, minimal differencebetween real ZMP and table cart ZMP.From Figure 4 it is clearly observable that the whole bodymodel in our optimization framework, the virtual robot aswell as the real robot show relative similar motion char-acteristics. The force/torque control loop only concerns theground contact reactions Mx, My and Fz . Apart from noiseand different initial conditions in the optimization and thesimulator OpenHRP, the ground contact reactions Fx, Fy ,Mz and even Fz closely follow the computed results. Thus,it is concluded that perturbation from vertical excitement ofthe elasticity has been canceled out sufficiently. Contrary, Mxand My show large deviation and consequently the stabilizerstill shows high level of activity.This is further confirmed from Figure 5. Deviation fromthe computed reference during the real experiment and eventhe simulation are clearly observable in the ankle joint aswell as in the knee joint of both legs. This verifies as wellthe aspect that the whole lower body is used to apply themanipulation of the trajectories of the stabilizer. Besidesthese deviations, the overall trajectories coincide relativelywell most of the time. Furthermore, the deviation of theposture control, visible in the global orientation of the torsostays small (see Figure 3). Thus we conclude, that ouremployed strategy was successful, despite, modeling errorsin elasticity, mass distribution and the fact that the referencetrajectory still seems to excite the heuristics of the stabilizingalgorithms. Furthermore, it has even to account for potentialdifferences between simulation and real robot (see Figure 5

red mark).A comparison between Figure 3 and 6 reveals an interestingfact. Figure 3 shows deviations between the real ZMP andthe Table Cart ZMP. As this objective was not triggered to bethe first dominant one, these deviations are reasonable. Apartfrom the high peaks during the phase transitions betweensingle support and double support, a high correlation betweenthe deviations in Figure 3 and 6. The same tendency is visibleon Figure 4 (see plot My !). Even though the CoP wasconstrained to move only in a squared area of 1 [cm] x 1 [cm](blue line), the final result of the stabilizer moves the CoMin a much larger area of approximately 10 [cm] x 10 [cm],which is however still compatible with the geometry of thefoot (see Figure 6). Despite simplifications in the modeling ofthe elasticity, this might give evidence about the stabilizingalgorithm following the table cart ZMP reference [9]. Thelocal CoP in the left foot - especially during single support -moves to the opposite direction of the observable deviationbetween the real and the table cart ZMP. The overall behaviorgives the impression of a control system tuned to its dynamiclimits (high control actions - risk for saturation).

V. CONCLUSIONS & PERSPECTIVE

This paper proposes a new generic strategy to investigatethe hardware and control limits of a given robotic platformbased on a dynamically challenging motion. This strategymassively exploits the possibility to formulate desired ab-stract motion characteristics based on high-level objectives.Despite unavoidable modeling errors and small incompatibil-ity of our computed reference trajectories to the stabilizingalgorithms, the real robotic platform was still able to safelyoperate during the motion performance.Based on the previous analysis, further adjustments to theformulation of desired motion characteristics should includea robustification to efficiently cope with the highly sensi-tive output of the stabilizing control algorithms and jointcontrolers to lead to a more predictable motion behaviorand limit its influence to the dynamic equilibrium of therobot. This should allow to investigate the actual limits evenmore precisely. Future prospects will be further refinementsto the whole body dynamics model as well as extension toother dynamically challenging motions, such as dynamicallystepping through a narrow passage.

ACKNOWLEDGMENT

Financial support of HGS MathComp and the KOROIBOTProject for this work is gratefully acknowledged. We thankthe SimOpt team (Hans Georg Bock) of the University ofHeidelberg for providing the optimal control code MUS-COD II. The authors would like to thank as well MaximilienNaveau for his valuable help during the experiment-phase onthe HRP-2 robot.

REFERENCES

[1] H. Bock and K. Plitt. A multiple shooting algorithm for direct solutionof optimal control problems. pages 243–247. Pergamon Press, 1984.

[2] J. Englsberger, C. Ott, and A. Albu-Schffer. Three-dimensional bipedalwalking control using divergent component of motion. In IEEE/RSJInternational Conference on Intelligent Robots and Systems (IROS),2013.

[3] T. Erez and E. Todorov. Trajectory optimization for domains with con-tacts using inverse dynamics. In IEEE/RSJ International Conferenceon Intelligent Robots and Systems (IROS), 2012.

[4] R. Featherstone. Rigid Body Dynamics Algorithms. Springer-VerlagNew York, Inc., Secaucus, NJ, USA, 2007.

[5] Y. Guan, K. Yokoi, and K. Tanie. Feasibility: Can humanoid robotsovercome given obstacles. In IEEE/RAS Int. Conf. on Robotics andAutomation (ICRA), 2005.

[6] H. Hirukawa, F. Kanehiro, K. Kaneko, S. Kajita, K. Fujiwara,Y. Kawai, F. Tomita, S. Hirai, K. Tanie, T. Isozumi, K. Akachi,T. Kawasaki, S. Ota, K. Yokoyama, H. Handa, Y. Fukase, J.-I. Maeda,Y. Nakamura, S. Tach, and H. Inoue. Humanoid robotics platformsdeveloped in hrp. Robotics and Autonomous Systems, 48:165–175,2003.

[7] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi,and H. Hirukawa. Biped walking pattern generation by using previewcontrol of zero-moment point. In IEEE/RAS Int. Conf. on Roboticsand Automation (ICRA), 2003.

[8] S. Kajita, T. Nagasaki, K. Kaneko, and H. Hirukawa. Zmp-based bipedrunning control. IEEE Robotics and Automation Magazine, 07:63–73,2007.

[9] S. Kajita, T. Nagasaki, K. Kaneko, K. Yokoi, and K. Tanie. A runningcontroller of humanoid biped hrp-2lr. In IEEE/RAS Int. Conf. onRobotics and Automation (ICRA), 2005.

[10] S. Kajita, K. Yokoi, M. Saigo, and K. Tanie. Balancing a humanoidrobot using backdrive concerned torque control and direct angularmomentum feedback. In IEEE/RAS Int. Conf. on Robotics andAutomation (ICRA), 2001.

[11] K. Kaneko, F. Kanehiro, S. Kajita, H. Hirukawaa, T. Kawasaki,M. Hirata, K. Akachi, and T. Isozumi. Humanoid robot HRP-2. InIEEE/RAS Int. Conf. on Robotics and Automation (ICRA), 2004.

[12] J. Ketchel and P. Larochelle. Collision detection of cylindrical rigidbodies using line geometry. In IDETC/CIE ASME International DesignEngineering Technical Conference & Computers and Information inEngineering Conference, 2005.

[13] D. Leineweber, I. Bauer, H. Bock, and J. Schlder. An efficient multipleshooting based reduced SQP strategy for large-scale dynamic processoptimization - Part I: theoretical aspects. pages 157 – 166. 2003.

[14] P. Michel, J. Chestnutt, J. Kuffner, and T. Kanade. Vision-guidedhumanoid footstep planning for dynamic environments. In Proceedingsof the IEEE-RAS International Conference on Humanoid Robots, 2005.

[15] S. Nakaoka, S. Hattori, F. Kanehiro, S. Kajita, and H. Hirukawa.Constraint-based dynamics simulator for humanoid robots with shockabsorbing mechanisms. In IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS), 2007.

[16] T. Pozzo, Y. Levik, and A. Berthoz. Head and trunk movements in thefrontal plane during complex dynamic equilibrium tasks in humans.Experimental Brain research, 106:327 – 338, 1995.

[17] O. Ramos, L. Saab, S. Hak, and N. Mansard. Dynamic motion captureand edition using a stack of tasks. In Proceedings of the IEEE-RASInternational Conference on Humanoid Robots, 2011.

[18] L. Roussel, C. C. de Wit, and A. Goswami. Generation of energyoptimal complete gait cycles for biped robots. In IEEE/RAS Int. Conf.on Robotics and Automation (ICRA), 1998.

[19] G. Schultz and K. D. Mombaur. Modeling and optimal controlof human-like running. IEEE/ASME Transactions on Mechatronics,15:783–792, 2010.

[20] B. Verrelst, K. Yokoi, O. Stasse, H. Arisumi, and B. Vanderborght.Mobility of humanoid robots: Stepping over large obstacles dy-namically. In IEEE International Conference on Mechatronics andAutomation (ICMA), 2006.

[21] M. Vukobratovic and J. Stephanenko. On the stability of anthromo-morphic systems. Mathematical Biosciences, 15:1–37, 1972.

[22] P.-B. Wieber, F. Billet, L. Boissieux, and R. Pissard-Gibollet. The hu-mans toolbox, a homogenous framework for motion capture, analysisand simulation. In International Symposium on the 3D Analysis ofHuman Movement, 2006.

[23] Y. Xiang, J. Arora, and K. Abdel-Malek. Physics-based modelingand simulation of human walking: a review of optimization-based andother approaches. Medical and Bioengineering Application, 42:1–23,2010.