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 x1x1 x2x2 x3x3

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

g Problem Statement 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? Setup: mechanism modeled as a variable c.o.m. body Dynamic stability?

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 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 gait pattern:

Related Work – 2D Greenfield, Choset and Rizzi (2005): planning quasistatic climbing via bracing 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 Trinkle & Pang (1998): Strong stability, LCP formulation of frictional dynamics Lotstedt (1982); Erdmann (1984); Mason & Wang (1988); Rajan, Burridge and Schwartz (1987); Dupont (1992): Contact modes and frictional dynamic ambiguity

Related Work – 3D 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 McGhee and Frank, 1968: The support polygon principle for legged locomotion 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 =( f ext,  ext )   2  Contact forces:f   2k Friction Cones Bounds: Bf ≥ 0 Equilibrium condition: where  (x)= x  Jf ext +  ext x

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

Two Contacts Graphical Example g x 1 x 2 R(wo)R(wo) w o =(f g,0)  = 0.3

R(wo)R(wo) Two Contacts Graphical Example (cont’d) x 1 x 2 g  = 2.0

R(wo)R(wo) x 1 x 2  S ++ S +- 2 Contacts - Graphical Characterization  = Strip (x 1, x 2 ) S ++ = Strip ( C 1 +, C 2 + ) S + - = Strip ( C 1 +, C 2 - ) Theorem: R (w o ) = [(S ++  S -- )   ]  [(S + -  S - + )   ] _ S -- = Strip ( C 1 -, C 2 - ) S - + = Strip ( C 1 -, C 2 + )

R(wo)R(wo) k -Contacts - Graphical Characterization x 2 x 1 x 3 x 4 x 5 x 6 R 13 R 56 R (w) = =conv{ R ij (w) } g Algorithm:

External wrench neighborhood: N = {(p,q):-  ≤p ≤ , - ≤ q ≤ } External Wrench Neighborhood Wrench magnitude scales static response Parametrize w ext =(f x,f y,  ext ):  dxdx f ext c.o.m. g Robust Equilibrium Region: R ( N ) =  R (w) wNwN

fgfg f ext Robust Equilibrium Region – Example R ( N ) =  R (w) wNwN R(N)R(N) Recipe: If N = conv {w i } x 1 x 2 Then R ( N ) =  R (w i ) i

Dynamic Analysis Equations of motion: G(q) + w ext = Assuming a contact mode Contact velocities constraints: differentiate solve for Contact velocities inequalities: Contact forces inequalities : At zero velocity, q direction is determined by q Evaluating at q o

Dynamic Contact Modes Theory ma = f ext + f 1 + f 2 + …+ f k I c  =  ext + (x 1 -x)×f 1 +… (x k -x) ×f k 3 equations 3+2k unknowns Contact modes add 2k equations  a unique dynamic solution as a function of x (or S) Contact modes (F, R, U, W) Contact Mode’s inequalities  Feasibility Region of x

Examples of Dynamic Ambiguity g Contact mode FF Contact mode UF

g Example of Dynamic Ambiguity Contact mode UF:

The Strong Stability Criterion Strong Stability (Trinkle and Pang, 1998): S (w) = R SS (w) - R FF (w)  R UF (w) ...  R WW (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

g (b) (a) f ext

Robust Stability: S ( N ) =  S (w) wNwN Robust Stability - Definitions Strong Stability: S (w) = R SS (w) - R FF (w)  R UF (w) ...  R WW (w) R SS ( N ) =  R SS (w) wNwN Define: Robust Equilibrium Region: Non-Static Modes’ N -Feasible Region: R XY ( N ) =  R XY (w) wNwN S ( N ) = R SS ( N ) - R FF ( N )  R UF ( N ) ...  R WW ( N )  Robust Stability Region:

Non-Static Modes N -Feasible Region Definition: R XY ( N ) =  R XY (w) wNwN Express R XY as an intersection of halfspaces in a four-dimensional space: F i (x,y,p,q) ≥ 0 R XY ( N ) is the projection of R XY 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  R XY is parallel to xy plane.

x y z projection onto xy plane Silhouette Theorem - 2D example ??? The Silhouette Theorem: The Silhouette curves of the projection are critical values of the projection function, on which the generalized normal of  R XY is parallel to xy plane.

Non-Static Modes’ N-Feasible Region FF Contact Mode N N N

N-Feasible Region of UF Mode Line-Sweep Algorithm: identifies the cells and generates sample points Critical curves f i (x,y,p,q) are linear in p,q and quadratic in x,y Critical curves generate cell arrangement in xy plane Checking cell membership: LP problem in p,q N

S ( N ) = R SS ( N ) - R FF ( N )  R UF ( N ) ...  R WW ( N )  =0.1  =0.05 Example - Robust Stability Region S(N)S(N) N N N N  =0.25 S(N)S(N) S(N)S(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 Exact formulation of equilibrium region Efficient conservative approximation by projection of convex polytopes g

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: C i = { f i : (f i ⋅ n i )≥0 and (f i ⋅ s i ) 2 + (f i ⋅ t i ) 2 ≤  2 (f i ⋅ n i ) 2 } Feasible equilibrium region in 3D:

Basic Properties of R R is a convex and connected set. Focus on computing the boundary of for 3 -contact stances. R is a vertical prism with horizontal cross-section. The dimension of R is generically min{k,3}.  Assumption: upward pointing contacts: f i  e > 0 for all f i  C i, where e is the upward direction

The Support Polygon Principle The vertical prism spanned by the contacts: P = {x :  h, x+he  conv(x 1,…,x k )} Support Polygon Principle is unsafe!!! On nearly-flat terrains with sufficient friction, we get P  R. (Under certain conditions, even P = R ). On general terrains with small friction, we get R  P

x y z g R  = 0.5 Motivational Example The Support Polygon Principle: x must lie in the vertical prism spanned by the contacts:

x y z g  = 0.2 x3x3 x1x1 x2x2 X1X1 x y X3X3 X2X2 ??? Support Polygon Principle is unsafe!!! Motivational Example (cont’d) top view

Parametrizing Equilibrium Forces Horizontal and vertical components: f i must intersect a common vertical line l r 

Permissible Polygonal Region of r Projected frictional constraints: r must lie in the polygonal region P = P +  P -,where

Graphical Example of P P+P+ P-P- y x top view

p3p3 p2p2 p1p1 Complete Graphical Parametrization Action line of f i intersects the common vertical line l r at p i Define  i – height of p i about x i  i = e∙(p i – x i ) Parametrize contact forces by (r,  )   2   3, where  =(      3 ): where

Permissible Region in ( r,  ) space The permissible region: ( r,  )  Q = Q 1  Q 2  Q 3, where and where for f i lying on the boundary of C i,  i =  i * (r) QiQi CiCi P P Q = Q 1  Q 2  Q 3

 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 Torque balance implies a map from (r,  ) to : Computing the Boundary of

Formulating Boundary Curves of Type-1 critical curves: r lies on segment, f k =0 Corresponding boundary curves: edges of support polygon Type-2 critical curves: r=r *,  i =  i * (r),  j =  j * (r),  k is free r * are isolated points, solving a polynomial system in r. Corresponding boundary curves: line segments Type-3A critical curves: r   P,  i =  i * (r) for i=1,2,3 Corresponding boundary curves: parametric curves Type-3B critical curves: r  int(P),  i =  i * (r) for i=1,2,3 r is the solution curve of a polynomial equation in r. Corresponding boundary curves: parametric curves

x y Graphical Example of type-1 boundary f i,f j ≠0 ; f k =0 type-2 boundary f i  C i, f j  C j, r=r * type-3 boundary f i  C i, i=1..3

Polyhedral Approximation of R Equilibrium condition in matrix form: Approximate friction cones by inscribed polyhedra: General solution: The feasible region in space is a convex polytope: The cross section is the projection of S’ onto -plane Efficient algorithm for projecting convex polytopes: Ponce et.al, 1997 where f=(f 1,f 2,f 3 )   9, G   6  9, T   6  2, u o   6, where   3

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

x 1 x 2 x 3 x y Polyhedral Approximation of R - Example top view

Future Research Physical geometric intuition of boundary curves, effect of  Generalization to multiple contact points Elimination of non-static contact modes (Complementarity formulation, Pang and Trinkle, 2000) Robustness with respect to disturbance forces and torques Application to legged locomotion on rough terrain in 3D Relation to line geometry and parallel robots’ singularities Already done In progress

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 x1x1 x2x2 x3x3

Thank You תודה רבה