Stability of Surface Contacts for Humanoid Robots: Closed-Form Formulae of the Contact Wrench Cone for Rectangular Support Areas

0.81 MB·English

by  Stéphane Caron

Stability of Surface Contacts for Humanoid Robots:
Closed-Form Formulae of the Contact Wrench Cone
for Rectangular Support Areas
5
1
0 Ste´phaneCaron1,Quang-CuongPham2,andYoshihikoNakamura1
2

n 1DepartmentofMechano-Informatics,TheUniversityofTokyo,Japan
a
J 2SchoolofMechanicalandAerospaceEngineering,Nanyang

0 TechnologicalUniversity,Singapore
2

] January21,2015
O
R
.
s Abstract
c
Humanoid robots locomote by making and breaking contacts with their
[
environment. A crucial problem is therefore to find precise criteria for a

1
given contact to remain stable or to break. For rigid surface contacts, the
v
most general criterion is the Contact Wrench Condition (CWC). To check
9
1 whetheramotionsatisfiestheCWC,existingapproachestakeintoaccounta
7 largenumberofindividualcontactforces(forinstance,oneateachvertexof
4 the support polygon), which is computationally costly and prevents the use
0
ofefficientinverse-dynamicsmethods. HerewearguethattheCWCcanbe
.
1 explicitlycomputedwithoutreferencetoindividualcontactforces,andgive
0 closed-formformulaeinthecaseofrectangularsurfaces–whichisofpracti-
5
calimportance. Itturnsoutthattheseformulaesimplyandnaturallyexpress
1
three conditions: (i) Coulomb friction on the resultant force, (ii) ZMP in-
:
v sidethesupportarea,and(iii)boundsontheyawtorque. Conditions(i)and
i
X (ii) are already known, but condition (iii) is, to the best of our knowledge,
novel. Itisalsoofparticularinterestforbipedlocomotion,whereundesired
r
a footyawrotationsareaknownissue. Wealsoshowthatourformulaeyield
simplerandfastercomputationsthanexistingapproachesforhumanoidmo-
tions in single support, and demonstrate their consistency in the OpenHRP
simulator.
1 Introduction
Fromtheviewpointoftherobot,establishingcontactwiththeenvironmentamounts
toconstrainingacertainnumberofDegreesOfFreedom(DOF)ofanend-effector
1
link. Forinstance,aslidingplanarcontactimposesthreeequalityconstraints(two
ontheorientationofthelinkandoneforthelink-to-surfacedistance)whileafixed
contact constraints all six DOFs of the end-effector link. A stability criterion can
be seen as a set of inequality constraints describing the conditions under which
theseequalitiesarepreserved.
ThevenerableZero-MomentPoint(ZMP)[1]criterionmaybethebest-known
example of stability criterion for rigid surface contact. It is known [2] to be nec-
essary but not sufficient for contact stability: in particular, it says nothing about
possibleslidinginthehorizontalplaneandrotationaroundtheverticalaxis(yaw).
Yet, humanoid robots are often subject to significant yaw moments during single
support phases, resulting in undesired foot rotations. Attesting the importance of
thisproblem,recentworkshavebeenusingupper-bodymotionstocompensatefor
theseyawswhilecontinuingtouseZMP[3,4].
Amoreprincipled(andgeneral)waytoaddressthisproblemistoconsiderin-
dividual contact forces distributed on the contact surface, as can be found e.g., in
bipedal balance control [5] and motion planning [6, 7]. This approach yields a
stronger stability criterion than ZMP, and accounts for both the sliding and yaw
rotation. It is however hampered by redundancy: the vector of contact forces has
manymorecomponents(threetimesthenumberofcontactpoints)thanthedegree
of the contact constraint (six). This redundancy makes the resolution of the equa-
tionsofmotionfundamentallyharder,asillustratedbythefactthatstate-of-the-art
Inverse Dynamics based on QR-decomposition [8] only apply to non-redundant
variables1. InTime-OptimalPathParameterization(TOPP),redundancyprompted
the use of further contact approximations [9] or expensive polytope projection al-
gorithms [7]. In the present paper, we argue that such workarounds are no longer
necessaryifoneusesthecorrectcontactrepresentation(forinstance, both[9]and
[7]boildowntoasinglematrixinversionforabipedinsingle-support,aswewill
seeinSection5).
The key insight here is that the condition that individual contact forces lie in
their respective friction cones can be replaced by the condition that the contact
wrenchbelongstoacertainwrenchcone[10]. Thecontactwrenchnaturallysolves
theredundancyissue,asitsdimensionisminimal(six). Itwasadvocatedasagen-
eralization of ZMP in [2], along with a stability theorem, and applied to walking
pattern generation on rough terrains [11, 12]. However, this theorem makes the
same“sufficientfriction”assumptionasZMP,whichmeanstheresultingcriterion
does not account for sliding and yaw rotations. Besides, the contact wrench is
1 When this is not the case, the authors advocate the use of Singular Value Decomposition to
computenewindependentvariables;however,itisunclearhowtocomputetheinequalityconstraints
applyingtothesenewvariables.
2
computedfromindividualcontactforces,whichweargueyieldsunnecessarilyex-
tensive calculations: following [10], the wrench cone can be computed explicitly
fromthesolegeometryofthecontactsurface.
Inthispaper,wederivetheclosed-formformulaeofthewrenchconeinthecase
ofrectangularsupportareas,whichisofpracticalimportancesincemosthumanoid
robotfeetcanbeadequatelyapproximatedbyrectangles. Thisresulthelpssimplify
dynamics computations, as we will see for humanoid motions. It also provides
an analytical description of “yaw friction”, from which we derive a simple and
principledcontrollawtopreventundesirableyawrotations.
The rest of the paper is organized as follows. In Section 2, we recall the defi-
nitions related to contact stability. In Section 3, we discuss the physics of surface
contactandgiveatheoreticaljustificationforthepracticeofconsideringindividual
contactforcesattheverticesofthecontactpolygon. Then,inSection4,wederive
aclosed-formexpressionofthewrenchconeinthecaseofrectangularcontactar-
eas. WeapplytheresultingsolutioninahumanoidexperimentinSection5before
concludinginSection6.
2 Background
2.1 ContactForcesandContactWrench
Consider a robot with n degrees of freedom making N point contacts with the
environment, at points C ,...,C in the laboratory reference frame (RF). The
1 N
equationsofmotionoftherobotare:
N
(cid:88)
M(q)q¨ +h(q,q˙) = S(cid:62)τ + J(C )(cid:62)f , (1)
a i i
i=1
whereq,q˙,q¨arethen-dimensionalvectorsofDOFvalues,velocitiesandaccelera-
tions,Misthen×ninertiamatrix,h(q,q˙)then-dimensionalvectorofgravityand
Coriolisforces. Incasetherobothasn actuatedjoints,τ isthen -dimensional
a a a
vector of torques at the actuated joints and S is a n × n joint selection matrix.
a
Finally,foreachi ∈ [1,N],f isa3-dimensionalvectorofcontactforceandJ(C )
i i
isthe3×ntranslationJacobiancalculatedatpointC .
i
Weassumethatboththeenvironmentandthecontactinglinkarerigidbodies.
Thus, interactions between them can be represented by a single contact wrench
3
W = (f,τ),whichwecancomputefromcontactforcesas:
def (cid:88)
f = f , (2)
i
i
def (cid:88)−−→
τ = OC ∧f , (3)
i i
i
whereO istheoriginofthelinkRF.ThecontactjacobianJ isthe6×nmatrix
wr
obtainedbystackingverticallyJ(O),thetranslationJacobiancomputedatO,and
J , the rotation Jacobian of the link, both taken with respect to the absolute RF.
rot
Withthesedefinitions,wehavethefollowingproperty:
N
(cid:88)
J(cid:62) W = J(C )(cid:62)f . (4)
wr i i
i=1
(SeeAppendixAforaproof.) Then,theequationsofmotioncanberewrittenas:
M(q)q¨ +h(q,q˙) = S(cid:62)τ +J(cid:62) W. (5)
a wr
2.2 ContactStability
Assume now that the robot is in a given state (q,q˙). The accelerations q¨ and
generalized forces exerted on the robot (actuated torques or contact forces) are
bound by a complementarity conditions [13]. For the sake of the explanation, let
usconsiderfirstthesimplecaseofasingletranslationcoordinatex,asdepictedin
Figure1.
x
f
c
f
a
mg
Figure1: BlockonahorizontalsurfacewithoneDOF.
UnderCoulomb’sfrictionmodel,eitherofthefollowingsituationsoccurs:
• Fixedcontact: x¨ = 0andthecontactforceobeys|ft| ≤ µfn,whereft and
c c c
fn arerespectivelythehorizontalandverticalcomponentsoff andµisthe
c c
staticfrictioncoefficient;
4
• Slidingcontact: x¨ > 0andthecontactforceobeysft = −µ fn,whereµ
c k c k
isthekineticfrictioncoefficient.
Theaccelerationx¨andcontactforcef areinacomplementaryrelationship: when
c
one is equality-constrained, the other is inequality-constrained. Similarly, for the
generalcaseofa6-DOFend-effectorincontact,thetranslationalandrotationalac-
celerations of the link are in a complementary relationship with some generalized
contact forces γ (here, γ = W or (f ,...,f )). The contact mode describes, for
1 N
eachvariableinacomplementaryrelationship,whetheritisequality-orinequality-
constrained. Incontactstability,weareinterestedinthefixedcontactmodewhere
thepositionandorientationoftheend-effectorareequality-constrainedtoarefer-
encevalue.
Definition1(WeakContactStability) Acontactisweaklystablewhenthereex-
ists a solution (q¨,τ ,γ) of the equations of motion satisfying the fixed contact
a
modeforallcontactinglinks.
Thatistosay,
• foreachcontact(i),therelativevelocityandaccelerationatcontactarezero:
J(i)q˙ = 0andJ(i)q¨ = −J˙(i)q˙,
wr wr wr
• actuatedtorquesτ arewithintorquelimits,
a
• complementary forces γ satisfy their inequality constraints (friction cones
orwrenchcone).
This formulation has been widely used in the literature. In approaches based on
inverse dynamics, the conditions on (q˙,q¨) are first enforced kinematically, then
torquesandcomplementaryforcesaresolved[7,8].
The“weakness”inthedefinitionabovereferstothenotionsofstrongandweak
stability,asstatedby[13]. Strongstabilityhappenswhenallsolutionstotheequa-
tionsofmotionsatisfythefixedcontactmode. Notethatchoosingbetweencontact
forcesandthecontactwrenchchangestheequationsofmotion(Equations(1)and
(5),respectively),buttheunderlyingcontactstabilityisthesamebyEquation(4).
Intherestofthepaper,wewillalwaysrefertocontactstabilityintheweaksense.
3 Surface Contact
Suppose we take contact forces f ,...,f as complementary variables to the po-
1 N
sition and orientation of the contacting link. Let fn and ft denote the normal and
i i
5
the tangential components of the contact force f . Coulomb friction provides the
i
complementaryinequalities:
• Unilaterality: fn > 0, (6)
i
• Non-slippage: (cid:107)ft(cid:107) ≤ µfn, (7)
i i
whereµisthestaticcoefficientoffriction.
However, when the contact is done through a surface and not through a set
of points, the reality of contact is that of continuum mechanics. In this case, the
action of the environment at the contact surface S is described by two quantities:
a scalar field p(x,y) corresponding to normal pressure, and a two-dimensional
vectorfieldσ(x,y)fortangentialmechanicalstress. Figure2-(A)illustratesthese
two fields for a rectangular contact area. For convenience, we also will denote by
def
ν = σ(x,y)+p(x,y)n,wherenistheunitvectornormaltothecontactsurface.
Theequationsofmotionbecome
(cid:90)
Mq¨ +h = S(cid:62)τ + J(C )(cid:62)ν(x,y)dxdy (8)
a xy
S
whereJ(C )isthe3×ntranslationJacobiancalculatedatthepointofcoordinate
xy
C onthesurface(takeninthelaboratoryRF).Thewrenchresultingfromν(x,y)
xy
is
(cid:90)
def
f = ν(x,y)dxdy, (9)
S
(cid:90) −−−→
def
τ = OC ∧ν(x,y)dxdy, (10)
xy
S
where O is the origin of the link RF. Under Coulomb friction, the inequality con-
straintsforν(x,y)are:
• Unilaterality: p(x,y) > 0, (11)
(cid:107)σ(x,y)(cid:107) ≤ µp(x,y). (12)
• Non-slippage:
Note that taking a constant µ in Equation (12) is an approximation: in reality, the
frictioncoefficientµ(x,y)varieswiththepositiononthesurface.
Inthepresentliterature,surfacecontactisoftenmodeledusingsetsofcontact
points: sometimesmorethanrequiredbypositionalconstraints(e.g.,nearly30per
contacting link in [7]) or one at each vertex of the convex hull (e.g., in [5]). The
propositionbelowgivesatheoreticaljustificationforthelatterpractice.
6
Proposition1 Assume that the contact surface S is a convex polygon with ver-
ticesC ,...,C . Ifthereexistsν(x,y) satisfyingcomplementaryinequal-
1 N (x,y)∈S
ityconstraints(11)-(12),thentherewillexistcontactforcesappliedatC ,...,C
1 N
andsumminguptothesamecontactwrenchthatsatisfycomplementaryinequality
constraints(6)-(7).
Y
C C
1 2
X
x
O
y
z
C C
4 3
(A) (B)
Figure2: Contactinthesurfaceplane. (A)Exampleofstress/pressurefields. Red
discsindicatethemagnitudeofpressurebytheirsize. Bluearrowsshowtangential
stress. (B)NotationsusedinSection4.
Proof:considerpressureandstressfieldssumminguptoW. Byconvexity,one
(cid:80)
canfindstrictlypositivefunctionsα (x,y),...,α (x,y)suchthat α (x,y) =
1 k i i
(cid:80)
1 and each point C ∈ S can be written C = α (x,y)C . Then, define for
xy xy i i i
eachvertexC aforce
i
(cid:90)
f := α (x,y)ν(x,y)dxdy,
i i
S
Bypositivityoftheα ’s,itisstraightforwardtocheckthatallfn > 0and(cid:107)ft(cid:107) ≤
i i i
µfn. Inaddition,thisexpressionoff ensuresthattheresultingwrenchesareequal,
i i
i.e.,(2)=(9)and(3)=(10). (cid:3)
We furthermore argue that, when the friction coefficient µ is assumed to be
constant as in Equation (12), the two conditions are equivalent. The complete
proof of this equivalence, which involves reconstructing pressure and stress fields
givenlocalandboundaryconditions,ishoweveroutofthescopeofthispaper. The
bottom line of this argument is that using contact forces at vertices of the convex
hull completely describes the dynamics of surface contact. The wrench cone that
wederiveinthenextsectionwillsharethesamecharacteristic.
7
4 Wrench Cone for Rectangular Surfaces
Consider a rectangular area (C C C C ) as depicted in Figure 2 (B). We calcu-
1 2 3 4
late the contact wrench W at the center O in the link’s RF.2 Let us denote by
(fx,fy,fz)thethreecomponentsofthecontactforceatpointCi. Underthecom-
i i i
monlinearapproximationoffrictioncones,Coulombinequalitiesbecome
|fx|,|fy| ≤ µfz (13)
i i i
fz > 0 (14)
i
Thewrenchconeisthengivenbythefollowingproposition.
Proposition2 Thereexistsasolution(f ,...,f )satisfyinginequalities(13)-(14)
1 4
ifandonlyifthereexistsawrenchW = (fx,fy,fz,τx,τy,τz)suchthat:
|fx| ≤ µfz (15)
|fy| ≤ µfz (16)
fz > 0 (17)
|τx| ≤ Yfz (18)
|τy| ≤ Xfz (19)
τ ≤ τz ≤ τ (20)
min max
where
τ d=ef −µ(X+Y)fz+|Yfx−µτx|+|Xfy−µτy|,
min
τ d=ef +µ(X+Y)fz−|Yfx+µτx|−|Xfy+µτy|.
max
The proof of this proposition is given in Appendix A.1. The validity of this
expressionwasalsotestedempiricallywithascriptavailableat[14].
Letusnowdetaileachlineofthewrenchcone. Thefirsttwoinequalities(15)-
(16)correspondtotheusualCoulombfriction. Inequalities(17),(18)and(19)are
equivalenttotheZMPcondition. Thelastinequality(20)providesaboundonthe
admissible yaw torque that was implicitely encoded in the contact-force model.
Note how this relation is more complex than a mere “no rotation occurs while τz
issmallenough”,asitiscoupledwithallothercomponentsofthecontactwrench.
Notably,the“safest”valueisnotzerobut:
def
τ = (τ +τ )/2
safe min max
= sgn(−fxτx)min(Y|fx|,µ|τx|)
+ sgn(−fyτy)min(X|fy|,µ|τy|),
2multiplybythelink’srotationmatrixforawrenchintheabsoluteRF
8
0.8 1.6 2.4 3.2 4.0 4.8 5.6 6.4 7.2
Figure3: Snapshotsoftheretimedmotion. Themotionlasts7.285s. Timestamps
are shown below each frame. This motion stresses all components of the wrench
cone. The first part stresses the pitch by moving the COM forward. The second
segmentextendsthearms,thenkeepstheleftarmstillwhilemovingtherightarm,
thusstressingtheroll. Atthesametime,thechestpitchisactuatedbackandforth,
which stresses the yaw. Finally, the waist performs an elliptic motion (back and
forth,upanddown)throughoutthewholemotion,thusstressingthetranslationof
thecontactfoot.
wheresgnisthesignfunction. From(20),τz maydeviatefromτ byatmost
safe
µ(X +Y)fz −max(Y|fx|,µ|τx|)−max(X|fy|,µ|τy|).
We see that higher tangential forces or roll-pitch torques reduce the range of ad-
missibleyawtorques. Inparticular,whentheseotherconstraintsaresaturated(for
instance when the ZMP reaches a corner of the support polygon), τ is the only
safe
solutionthatpreventsthecontactfrombreaking. Therefore, τz = τ appearsas
safe
asensiblecontrollawtopreventundesiredyawrotations.
5 Experiment
We test the validity of the Contact Wrench Condition (CWC) in the integrated
simulatorOpenHRPwithamodeloftheHRP4robot. NotethatOpenHRPhasits
own contact model where forces are distributed along the edges (not corners) of
thecontactsurface.
We implemented the CWC condition within the Time-Optimal Path Parame-
terization framework (TOPP), a well-known projection of system dynamics along
a pre-defined path that has been used for motion planning of humanoid robots
[7,15,9]. Weconsideredthecaseofasinglecontactattheleftfootanddesigned
amotionthatwouldchallengeallsixcontactDOFs. Insinglecontact, thecontact
wrenchisfullydeterminedbytheunactuatedrowsoftheequationofmotion,i.e.,
W = (PJ )−1P[M(q)q¨ +h(q,q˙)]
wr
9
1.6 2.2 3.0 3.6 4.2 4.8 5.2 5.8
Figure4: ZoomonthecontactforcescomputedbyOpenHRPattheleftfoot. The
first row corresponds to the retimed motion (7.29 s), the second to the 10% uni-
formaccelerationoftheretimedmotion(6.55s)andthethirdtothe15%uniform
acceleration(6.19s). Timestampsareshownbeloweachcolumn. Theretimedmo-
tionmaintainssurfacecontactwithcontactforcesdistributedallaroundthecontact
area,exceptbetween4.2and4.8swheretheyaremostlyonthelefthalfofthefoot.
The10%-acceleratedversiongoestopointcontact(3.0s)andlineedgecontact(3.6
to4.8s)butdoesnotfall. At15%acceleration,theCWCviolationovercomesthe
stabilizer’sabilityandtherobotfalls.
whereP = I−Sistheunactuatedlineselectionmatrix. Notehowcomputingthe
constraintprojectorisstraightforwardwhenusingthecontactwrench: itisnotthe
casewhencontactforcesareused,asweobservedinpreviousresearch[9](where
weresolvedtheforceredundancywithaforce-bindingmodel).
For this experiment, we designed by hand a set of eleven key postures. The
geometricpathwasobtainedbyinterpolatingBeziercurvesbetweenthesepostures
andapplyingakinematicfiltertofixthepositionandorientationofthesupportfoot
ontheground. Togetafeasibletrajectoryfromthispath(i.e.,tocomputethetiming
information)weusedtheopen-sourceTOPPsolver[16]. Thesolvertakesasinput
thepathandavectorrepresentationofthesystemdynamicsalongit,whichiseasy
to compute once one knows the projectors mentioned above (see [9] for details).
Figure 3 shows a timelapse of the final retimed motion. Videos are also available
onlineat[14].
AninterestingthingtonoteaboutTOPPisthat,becauseoftimeoptimality,the
retimedmotionalwayssaturatesatleastoneofthecontactconstraints. Inanideal
setting with perfect system dynamics, the obtained trajectory should therefore be
at the limit of contact: it would execute correctly, but the contact would break as
soonasonetriestoperformthemotionfasteratanytimeinstant. Weobservedthis
phenomenonintheexperiment,asdepictedinFigure4.
Thereare,however,afewpointstodiscuss. Inordertoensurepropertracking
10