Computing stable equilibrium

stances of a legged robot in

frictional environments
Yizhar Or
Dept. of ME, Technion – Israel Institute of Technology
Ph.D. Advisor: Prof. Elon Rimon

g
x3

x2
x1
 Outline
2D: • Computation of frictional equilibrium stances
• Robustness w.r.t disturbance forces and torques
• Dynamics – contact modes and strong stability

3D:
• The support polygon principle for flat terrains
• Geometric Parametrization of equilibrium forces in 3D
• Exact Computation of frictional equilibrium stances
• Polyhedral approximation of equilibrium stances
 Problem Statement
Setup: mechanism modeled as a variable c.o.m. body
Given a 2D (3D) multi-limbed mechanism standing on a terrain
with k frictional contacts, where should the center-of-mass be for:

• Static equilibrium?

• Robustness w.r.t disturbance forces?

• Dynamic stability? g
 Applications
• Quasistatic legged locomotion on rough terrain
(spider robots, snake robots, climbing, search-and-rescue
robots)
• Graspless manipulation (part feeding, assemblies)

• Motion planning for Hybrid wheeled-legged robots

• Semi-dynamic locomotion
Application - Three-Legged Locomotion

2-3-2 gait pattern:

• Select 2-contact postures that share a common contact point
• 3-contact stage connecting two consequent 2-contact postures
• At the 3-contact stage: straight line motion of center-of-mass
 Related Work – 2D
• Mason, Rimon, Burdick (1995):
Frictionless postures under gravity
• Mason (1991): Graphical methods for frictional equilibrium in 2D
• Erdmann (1998): Two-palm manipulation with friction in 2D

• Lotstedt (1982); Erdmann (1984); Mason & Wang (1988);

Rajan, Burridge and Schwartz (1987); Dupont (1992):
Contact modes and frictional dynamic ambiguity
• Trinkle & Pang (1998):
Strong stability, LCP formulation of frictional dynamics
• Greenfield, Choset and Rizzi (2005):
planning quasistatic climbing via bracing
 Related Work – 3D
• McGhee and Frank, 1968:
The support polygon principle for legged locomotion

• Mason, Rimon and Burdick , 1997:

Computing stable equilibrium frictionless stances in 3D

• Han, Trinkle and Li, 2000:

Feasibility test of frictional postures in 3D as LMI problem

• Bretl and Lall, 2006:

Adaptive polyhedral approximation of 3D equilibrium stances
• Bretl, Latombe (2003): PRM-based motion planning
algorithm for climbing on vertical walls with discrete supports
 Statics in 2D - LP Formulation
• Center of mass: x2
• External wrench: w=(fext,ext) 2
• Contact forces: f 2k
G f   f ext 
• Equilibrium condition: G  f    
 τ  τ( x ) 

where (x)= x Jfext+ext

• Friction Cones Bounds: Bf ≥ 0

Statics - LP Formulation (cont’d)
Theorem:
The feasible k-contact equilibrium region:
R(w) = {x: min  xJfext + ext max}
where
min = min{-Gf} max = min{-Gf}
s.t. s.t.
Gff=-fext Gff=-fext
Bf ≥ 0 Bf ≥ 0

Infinite strip parallel to fext

Two Contacts Graphical Example
R(wo) wo=(fg,0)

 = 0.3
g

x2
x1
Two Contacts Graphical Example (cont’d)

R(wo)

g
 = 2.0

x2
x1
2 Contacts - Graphical Characterization

++ +- S++ = Strip (C1+,C2+)

S S
S+ - = Strip (C1+,C2-)
S-- = Strip (C1-,C2-)
S -+ = Strip (C1-,C2+)
x2
 = Strip (x1, x2)

Theorem:
R(wo)
R(wo) = [(S++  S-- )  ]_
x1
 [(S+-  S -+ )   ]
 k-Contacts - Graphical Characterization

R(w) =
R(wo) =conv{Rij (w) }
g
R13 R56
Algorithm:
δ max
O(k  log )
δ min
x1
x4 x6
x2 x3 x5
 External Wrench Neighborhood
• Wrench magnitude scales static response
• Parametrize wext=(fx,fy,ext):
g
fx
p  tan β
fy 
fext
τ ext
q  dx
fy c.o.m. dx
• External wrench neighborhood:
N = {(p,q): -≤p≤ , -≤q≤}
• Robust Equilibrium Region: R(N) = R(w)
wN
 Robust Equilibrium Region – Example

R(N) = R(w)
wN
fgfextfext

Recipe:
If N = conv {wi}
R(N)
Then R(N) =i
R(wi)

x2
x1
 Dynamic Contact Modes Theory
3 equations ma = fext + f1 + f2 + …+ fk
3+2k unknowns
Ic = ext + (x1-x)×f1 +… (xk-x) ×fk
Contact modes (F, R, U, W)

(or S)

• Contact modes add 2k equations

 a unique dynamic solution as a function of x

• Contact Mode’s inequalities  Feasibility Region of x

 Example of Dynamic Ambiguity
Contact mode UF:

g
 The Strong Stability Criterion
Strong Stability (Trinkle and Pang, 1998):
S (w) = RSS(w) - RFF (w)  RUF (w)  ...  RWW(w)

• Eliminates ambiguity – only static solution is feasible

• Any roll/slide/break motion cannot evolve (at zero velocity)

• Yet, not formally related to classical dynamic stability

(bounded response to bounded position/velocity perturbations)

• Does not always imply bounds on c.o.m. height

 Must be augmented with robustness

 Robust Stability - Definitions
Strong Stability:
S (w) = RSS(w) - RFF (w)  RUF (w)  ...  RWW(w)

Robust Stability: S(N) = S(w)

wN

Define:
Robust Equilibrium Region: RSS(N) = RSS(w)
wN

Non-Static Modes’ N-Feasible Region: RXY(N) = 

wN
R
XY (w)

Robust Stability Region:

S(N) = RSS(N) - RFF(N)  RUF(N) ...  RWW(N)
 Non-Static Modes N-Feasible Region

• Definition: RXY(N) =  RXY(w)

wN
• Express RXY as an intersection of halfspaces
in a four-dimensional space: Fi(x,y,p,q) ≥ 0

• RXY(N) is the projection of RXY onto xy plane

The Silhouette Theorem:

The Silhouette curves of the projection are critical
values of the projection function, on which the
generalized normal of RXY is parallel to xy plane.
 N-Feasible Region of UF Mode

• Critical curves fi(x,y,p,q) are

linear in p,q and quadratic in x,y
• Critical curves generate
cell arrangement in xy planeN

• Line-Sweep Algorithm:
identifies the cells and generates sample points
• Checking cell membership: LP problem in p,q
 Example - Robust Stability Region
S(N) = RSS(N) - RFF(N)  RUF(N) ...  RWW(N)

=0.25 =0.1 =0.05

 I 
 ρ  c 
 m

S(N)
N

N
S(N) N N

S(N) S(N)
 Strong Stability and Dynamic stability

Force Closure  asymp. stability under keep-contact perturbations

Here: no force closure, passive contacts, arbitrary perturbations

Two contacts - neutral stability under keep-contact perturbations

Strong Stability  non-static mode decays until collision

• How to model collisions? treat sequence of collisions?

• Does strong stability really leads to dynamic stability?
• How to design stabilizing joints’ control laws
for a legged robot?
 Frictional Equilibrium Stances in 3D

• Analyze 3D equilibrium stances of legged

mechanisms in frictional environments

• Support Polygon criterion does not apply for

non-flat terrains g

• Exact formulation of equilibrium region

• Efficient conservative approximation by

projection of convex polytopes
 Problem Statement
• Characterize feasible equilibrium postures of a multi-limbed
mechanism supported against frictional environment in 3D.

• Given k frictional contacts, find the feasible region R

of center-of-mass locations achieving frictional equilibrium.
• Assumption: point contacts, uniform friction coefficient .

• Friction Cones in 3D:

Ci = {fi : (fi⋅ni)≥0 and (fi⋅si)2 + (fi⋅ti)2 ≤ 2(fi⋅ni)2}
• Feasible equilibrium region in 3D:
  f1   fk   f g  
  x :           , f i   i 
  x1  f1   xk  f k   x  f g  
 Basic Properties of R
• R is a convex and connected set.
~
• R is a vertical prism with horizontal cross-section  .

• The dimension of R is generically min{k,3}.


~
Focus on computing the boundary of  for 3-contact stances.

Assumption: upward pointing contacts: fie > 0 for all fi 

Ci ,
where e is the upward direction
 Motivational Example

 = 0.5

g
R
z

x
y

The Support Polygon Principle:

x must lie in the vertical prism spanned by the contacts:
 Motivational Example (cont’d)
top view
X3

 = 0.2
g
z

x3
y
~
 ??? x2
y
X1 X2
x1
x x
Support Polygon Principle is unsafe!!!
 Parametrizing Equilibrium Forces
• Horizontal and vertical components:
~
   E T xi , xiz  e  xi , ~  E T 
   ET fi , fi z  e  fi , ~
1 0  0 
where E  0 1 , e  0
   
0 0 1

• fi must intersect a common vertical line lr

~ 
  must intersect at a common point r in horizontal plane
~
   σ λi (  )(   ~
xi ) where λi (  )  (   ~
xi 1 )  J ( ~
xi  2  ~
xi 1 )
0  1
σR ,J   
 1 0 
 Permissible Polygonal Region of r
~ ~
• Projected frictional constraints: fi   i

• r must lie in the polygonal region P = P+ P- ,where

 Graphical Example of P
top view

~
3

~
1 P+ ~
2

P-
x
 Complete Graphical Parametrization
• Action line of fi intersects the common vertical line lr at pi

• Define i – height of pi about xi i = e∙(pi – xi)

p2

• Parametrize contact forcespby (r,)  2 3, where =(3):

3

p1

where
 Permissible Region in (r,) space
P Q = Q  Q  Q , where
• The permissible region: (r,)  1 2 3

and
Q = Q1  Q2  Q3
Qi
where Ci
• for fi lying on the boundary of Ci , i=i*(r)

P
 ~
Computing the Boundary of 

• Torque balance implies a map from (r,) to :


~
Horizontal cross section  is the image of Q under

• Formulate the restriction of to all possible manifolds of Q

• Compute critical curves of on each manifold of Q
~
• Candidate boundary curves of  are -image of critical curves
 ~
Graphical Example of 

type-2 boundary
fiC , f Cj,
r=r* i j

type-3 boundary
fi Ci, i=1..3
~

type-1 boundary
fi,fj≠0 ; fk=0
x
Conservative Polyhedral Approximation
• Replacing exact friction cones with inscribed pyramids.

• Reduces to projection of a convex polytope onto a plane

• Approximate outer bound by taking circumscribing pyramids

• Graphical example – with 6-sided pyramids

Polyhedral Approximation of R - Example
top view
x3

~
y  ''
~
'
x1 x2

x
 Future Research
• Physical geometric intuition of boundary curves, effect of 
Already done
• Relation to line geometry and parallel robots’ singularities

• Generalization to multiple contact points

In progress
• Robustness with respect to disturbance forces and torques

• Elimination of non-static contact modes

(Complementarity formulation, Pang and Trinkle, 2000)

• Application to legged locomotion on rough terrain in 3D

 Computing stable equilibrium
stances of a legged robot in
frictional environments
Yizhar Or
Dept. of ME, Technion – Israel Institute of Technology
Ph.D. Advisor: Prof. Elon Rimon
izi@technion.ac.il robots.technion.ac.il/yizhar

g
x3

x2
x1
Thank You
‫תודה רבה‬