Fast Algorithms to Test Robust Static Equilibrium for Legged Robots

—Maintaining equilibrium is of primary importance for legged systems. It is not surprising then that static equilibrium is at the core of most control/planning algorithms for legged robots. Being able to check whether a system is in static equilibrium is thus important, and doing it efﬁciently is crucial. While this is straightforward for a system in contact with a ﬂat ground only, it is not the case for arbitrary contact geometries. In this paper we propose two new techniques to test static equilibrium and we show that they are computationally faster than all other existing methods. Moreover, we address the issue of robustness to errors in the contact-force tracking, which could lead to slippage or rotation at the contacts. We extend all the discussed techniques to test for robust static equilibrium, that is the ability to maintain equilibrium while avoiding to lose contacts despite bounded force-tracking errors. Accounting for robustness does not affect the computation time of the equilibrium tests, while it qualitatively improves the contact postures generated by our reachability-based multi-contact planner.


I. INTRODUCTION
The problem of equilibrium is central for planning, control and analysis of legged robots [1].Control algorithms for biped and quadruped robots exploit static equilibrium criteria to avoid falls [2], [3].Motion planning algorithms make large use of static equilibrium tests to verify that the system is able to maintain balance at each generated configuration [4], [5].This quasi-static approach to motion planning can be the first step for the generation of dynamic movements through time-scaling techniques [6], [7].
For robots making contact on a flat ground only, static equilibrium can be achieved if and only if the vertical projection of the center of mass (CoM) lies inside the convex hull of the contact points [8] (neglecting the joint-torque limits).However, for arbitrary contact geometries this condition is neither sufficient nor necessary and more complex and computationally-expensive techniques are needed to check static equilibrium [9], [7].Regardless of the application, the computational efficiency of the equilibrium tests is critical, as computation time is often the bottleneck of planning and control algorithms.
Depending on the number of tests that need to be performed at each contact configuration, different test techniques have been identified as being the most efficient [10].In particular, control algorithms typically need to perform thousands of tests since robots usually maintain the same contact configuration for several seconds, and the controller {adelpret,stonneau,nmansard}@laas.frtests equilibrium 100-1000 times per second.At the other end of the scale we have motion-before-contact planners, which compute first the trajectory of the robot trunk and then try to find the contacts that stabilize that trajectory [11].Because of their working principle these algorithms typically perform very few tests (< 5) for each contact configuration.In between we have contact-before-motion planners, which first find the contact points and then generate the motion of the robot along these contacts [5].These planners typically perform a higher number of equilibrium tests for each contact configuration, in the order of 10-100.
For application on real systems the problem of equilibrium needs to be coupled with the problem of robustness: the robot needs to maintain equilibrium despite some bounded uncertainties.Two robustness measures have been proposed: the capacity of the system to accelerate its CoM [7] or to generate wrench at its CoM [12], [13].While being interesting, these robustness measures do not seem easy to apply on a real system: how much CoM acceleration/wrench must the system be able to generate in order not to fall?
This paper starts by recalling in Section II the problem of equilibrium 1 and the classic methods to test it.We then propose three contributions.In Section III, we propose a more efficient version of the algorithm proposed by Bretl and Lall [10] (up to 3 times faster).Then in Section IV we show that the classic "support-polygon" technique to test equilibrium on flat ground can also be applied to the more general class of quasi-flat contacts.Finally, in Section V we extend all the discussed techniques for testing equilibrium to account for a user-specified degree of robustness to contactforce tracking errors.Contrary to previous works [7], [12], [13], our robustness measure is easy to relate to the tracking capability of the force controller of the robot.Section VI collects all the simulation results, which validate the three contributions through three different tests.To demonstrate the importance of robustness we show in the accompanying video the qualitative difference between robust and nonrobust motions generated with a reachability-based planner [11].

II. STATIC EQUILIBRIUM
This section recalls the fundamental equations to model the equilibrium of a floating system in contact and the main algorithms to test it.This long summary is necessary as it introduces the notations and the methods that we will test in the experimental section.

A. Centroidal Dynamics
Considering a robot in contact with the environment at k contact points, its Newton-Euler equations are: where m is the robot mass, c ∈ R 3 is the CoM position, f i ∈ R 3 is the i-th contact force, g = 0 0 −9.81 is the gravity acceleration, L ∈ R 3 is the angular momentum (expressed at the CoM) and p i ∈ R 3 is the i-th contact point.All quantities are expressed in an arbitrary inertial frame having z aligned with the gravity.Each contact force is constrained to lie inside a friction cone: where µ i is the friction coefficient, t i1 , t i2 ∈ R 3 are the tangent directions and n i ∈ R 3 is the normal direction at the i-th contact.In the following we will denote this condition as f ∈ K.By replacing c × k i=1 f i with mc × (c − g) in (1b) we can reformulate (1) in matrix form: where w ∈ R 6 is the so-called gravito-inertial wrench (GIW) [7] and p ∈ R 3×3 is the cross-product matrix related to p.

B. Conditions of Static Equilibrium
From (2), the GIW needed for equilibrium is: The wrench w 0 is independent of c z because of the cross product matrix ĝ inside D, so we can rewrite (3) as: In this paper we are interested in characterizing the region of R 3 where the CoM can stand while allowing for equilibrium, that is: Since equilibrium is independent of c z , S is a generalized cylinder whose axis is parallel to gravity.We call its section the support section and denote it by Y (it is often named support polygon, while in general it is a rounded and possiblyunbounded shape).To test equilibrium, it is sufficient to check whether c xy ∈ Y.

C. SOCP and Linear Approximation
In the general case testing equilibrium requires the resolution of a Second-Order Cone Program [9].A very common and computationally-faster alternative-which we adopt as well-is to approximate the friction cones with polytopes [14], [13], [7], [10], [12].For safety reasons inner approximations are often preferred.We can express the linearized friction-cone constraints as a set of linear inequalities:

D. Linear Programming (LP)
The simplest way to test equilibrium for a given CoM position c is to look for contact forces that are inside the (linearized) friction cones and compensate for gravity.This amounts to solving this feasibility Linear Program (LP): The CoM position c allows for equilibrium if and only if (5) has a solution.

E. Polytope Projection (PP)
Another approach to test equilibrium is to compute S and check whether the interested CoM positions belong to it.Considering linear approximations of the friction cones, S becomes a polytope, which we can describe by linear inequalities on c.This solution is introduced in [13] and further described in [7].The proposed algorithm relies on polytope-projection techniques [15] to compute the cone of feasible GIW, expressed as a matrix H such that: The method is built on the observation that a linear cone P ⊆ R n can be equally represented as: Where the matrices P f and P s are respectively named the face and the span descriptions of P. Conversion between face and span form is possible using dedicated numerical algorithms [15] with a significant computational cost.The standard approach for computing H is to [7], [13]: • convert the force friction cones from face form to span form: B → V f ; • map the force generators to the GIW space: V w = AV f ; • convert the GIW cone from span form to face form V w → H.Note that the first step-which was performed in previous works [7], [13]-might be avoided by expressing directly the friction cone under span form V f instead of face form B, as the columns of V f are simply the generators of the linearized friction cones.Once we have computed H, which is computationally expensive, testing equilibrium boils down to checking a set of linear inequalities, which has negligible computational cost:

F. Incremental Projection (IP)
The key idea of the Incremental Projection (IP) algorithm [10] is to maintain inner and outer polyhedral approximations (Y in and Y out ) of the support section Y defined by (6).Y out is stored as a list of inequalities, while Y in as a list of vertices.To test equilibrium for a CoM position c the algorithm iteratively refines both approximations, until either c ∈ Y in (positive answer) or c / ∈ Y out (negative answer).At each iteration the algorithm shrinks Y out and grows Y in by solving an LP of this form: where a ∈ R 2 is the normal to a line separating c and Y in .
The solution (c * , f * ) of ( 7) bounds Y in the direction a, so we can shrink Y out by adding the following inequalities: Moreover, given that c * is inside the support polygon (actually it is on its borders), we can grow Y in by adding c * to its list of vertices.Fig. 1 shows an example of one iteration of the algorithm.A detailed explanation with a proof of the speed of convergence can be found in the original paper [10].

G. Conclusions
In conclusion, we have recalled three algorithms to test equilibrium of a CoM position given a set of contacts.LP does not require any preparatory computation, but each new test of a configuration has a significant cost.PP requires a costly preparatory computation, but each new test is nearly cost free.IP is a trade off, as it requires some few iterations for the first tests, but its cost decreases after each new test.
In the following sections we propose two original algorithms to test equilibrium, both more efficient than LP, PP and IP in most situations.The first algorithm relies on the same hypothesis, but it is up to 3 times faster; the second one uses more restrictive hypothesis, but its cost is nearly negligible with respect to the others.We then propose a generic solution to extend all these algorithms to test for robust equilibrium.

III. INCREMENTAL PROJECTION REVISITED (IPR)
We propose a new IP algorithm that at each step solves an LP with less variables than (7).Being the main computational effort in IP the resolution of the LPs (7) in 3k+2 dimensions, our new algorithm results to be much faster than the original one, especially for large values of k.We replace the original LP with another LP of constant dimension 4 that gives us the same solution a c * , so that we can build the same Y out .The 4d LP does not give us c * , so we can not build Y in as in the original algorithm.However, we show that a different Y in can be directly deduced from Y out , at almost no cost.

A. Computing the Outer Approximation
We start by trying to bound the GIW cone in an arbitrary direction h ∈ R 6 using the following LP: where m is the number of generators per contact, and the columns of V w = AV f are the GIW generators (using the same notation of Section II-E).By eliminating the equality constraint w = V w β, the LP (8) becomes: It is easy to see that the solution of this LP is zero if V w h ≤ 0 and infinity otherwise.If the solution is zero we can then bound the GIW cone in direction h: This observation allows us to avoid the explicit computation of the value of β in (8) (or similarly of f in ( 7)).To get a tight bound on Y in direction a ∈ R 2 we now must simply solve this 6d LP (which does not contain the contact forces): Finally, given the simple structure of the matrix D xy , it is straightforward to eliminate the equality constraints from (9) to convert it into a 4d LP.Solving this 4d LP gives us the same Y out of the original IP algorithm, that is a collection of inequalities of the form: Another way to derive ( 9) is to replace f with V f β in (7) and then formulate the dual problem [16].When ( 9) is unbounded, ( 7) is unfeasible, meaning that the support section is empty.When ( 9) is unfeasible, ( 7) is unbounded, that is the support section is unbounded in direction a.

B. Computing the Inner Approximation
The dual formulation (9) does not give us the optimal primal variable c * , so we can not build the same inner approximation Y in of IP.A cheap way to build an inner approximation starting from a tight outer approximation is: • to maintain the ordered list of vertices of Y out : v out • for each vertex i compute the inequality passing through v out i and v out i+2 (with sign such that v out i+1 / ∈ Y in ) This simple procedure gives us a halfspace representation of Y in .Fig. 2 shows one iteration of the algorithm, while a complete example is illustrated in the accompanying video.Note that for Y in not to be empty we need Y out to have at least five vertices-some of which may be coincident.Indeed every time Y out has three coincident vertices, the same vertex belongs to Y in as well, which is how Y in can converge to Y out .
Our inner approximation is rougher than the one of the original algorithm [10].This is the payback for having a much cheaper computation of Y out .Consequently, we can not hope to have the same convergence speed.However, in practice our algorithm turns out to be much faster.

IV. QUASI-FLAT CONDITIONS FOR STATIC EQUILIBRIUM (QF) A. Classical Case
It is well-known that when the robot is in contact with a horizontal ground a necessary and sufficient condition for equilibrium is [17]: where Hull({p xy i }) is the convex hull of the contact points on the ground, that is a convex 2d polygon.This condition still holds for flat sloped grounds (as long as friction is large enough to avoid slippage).We show now that this condition remains interesting for a large class of contacts, even if only as an approximation.

B. Quasi-Flat Contacts
Consider an arbitrary set of contact points p i and their associated friction cones denoted by K i .We say that this contact configuration is quasi-flat if the direction opposite to gravity is in every cone K i .In that case, the condition (10) is sufficient for equilibrium.Proof: We rewrite (1) for c = L = 0: (11) leaves us with: Since by assumption f z i ≥ 0 is a valid force for all contacts, these equations always admit a solution if ( 10) is satisfied.
The sufficiency of (10) implies that any CoM whose projection is inside the convex hull allows for equilibrium.However, other CoM positions that allow for equilibrium may exist.Looking at (11b) we can easily see that the larger the height difference of the contact points, the larger the error we commit using Hull({p xy i }) as an approximation of Y.We will show that this condition-which we call the quasiflat test (QF)-is accurate in a large majority of interesting situations.
This sufficient condition had already been observed for the case of three contact points [17].Here, we generalized it to an arbitrary number of contacts and our tests quantify the computational gains of using QF rather than the general PP technique.

A. Existing Criteria
Real systems are affected by countless uncertainties and disturbances, hence conditions (6) may not be enough to ensure equilibrium.Different robustness measures have already been proposed.Caron et al. [7] proposed to check the capacity of the system to generate x-y CoM accelerations inside a polytope (while maintaining L = 0).This measure would typically corresponds to situations in which the orientation of the robot is not properly estimated.The robustness measure proposed by Barthelemy [12], [13] is the radius of the largest hypersphere centered at w 0 that is fully contained inside the GIW cone.This measure is more abstract and it is hard to relate it to any particular uncertainty.

B. Robustness to Contact-Force Errors
In our previous work [18] we studied the control of legged robots focusing on the robustness to torque-tracking errors, a major source of uncertainty that in turns affect the contactforce tracking.We thus propose to account for robustness to errors in the contact-force tracking: we would like the forces necessary to maintain equilibrium not to be too close to the friction cone boundaries.Our motivation is that we do not want small errors in the contact-force tracking to cause slippage or rotation at the contacts.This uncertainty model also indirectly covers other errors such as in the orientation of the contacting bodies or friction coefficients.
Denoting the i-th contact-force tracking error e i ∈ R 3 , we consider a worst-case approach by ensuring robustness to any force error within given bounds e max : where R i = t i1 t i2 n i .A force vector f is then considered robust if: Given the simple structure of B we can show that this is equivalent to a vertical shift of the origin of the friction cones: where

C. Testing Robust Static Equilibrium
We can easily adapt the three methods for testing equilibrium presented in the previous sections to account for robustness.In the LP test ( 5) and in the LP (7) used by IP we just need to replace the constraints Bf ≤ 0 with the new constraints Bf ≤ Bl.In the PP approach the shift of the origin of the force friction cones causes a shift of the origin of the GIW cone:

VI. TESTS
We present three tests to validate the three different contributions of this paper.Test 1 focuses on the quasiflat contacts (Section IV), showing the quality of the QF approximation while quantifying the evident computational gains with respect to PP.It also shows that, on average, QF results in less false negatives than PP with 4-side linearized friction cones.Test 2 compares the performances of four methods for testing equilibrium: LP, PP, IP and IPR.In almost all scenarios our algorithm outperforms the others in terms of computation time.Test 3 shows how using a robust equilibrium criteria (Section V) results in planning qualitatively better contact postures [11].
In all our tests, we have used a friction coefficient µ = 0.3 for all contacts.We have solved the Linear Programs with qpOases [19], an efficient C++ active-set solver for Quadratic Programs, which can also solve Linear Programs.For the polytope projection we have used the C++ cdd library [15].We wrote all our code in Python, using bindings of the aforementioned libraries.

A. Test 1 -Quasi-Flat Contacts
In this test we compare three methods to compute a conservative approximation of Y in case of quasi-flat contacts (i.e.all the friction cones contain the negative gravity direction).The first method (QF) is to use the 2d convex hull of the vertical projection of the contact points (see Section IV).The second method (PP4) is to approximate the friction cones with 4-side pyramids and to use the polytope-projection technique discussed in Section II-E.The third method (PP8) is the same as the second one, but using 8-side pyramids, so as to be more precise.Each method results in a different polygon, each polygon being included in the real support section Y.For each contact configuration we evaluate the support polygon computed by the three methods on a 2d grid of CoM positions with step 1 cm.To avoid computing the real support section we approximate it by the union of      the three computed polygons.We then count the percentage of false negatives with respect to true positives for each test.We compare the three methods on 10 3 random biped contact configurations (each foot contact being defined by four contact points).We sampled the feet positions in a box of size 1 × 1 × 0.5m (x, y, z) and the feet orientations such that all friction cones contained the negative gravity direction.Table I reports the average/max percentage of false negatives and the mean computation time for each method.As evidenced by Fig. 3, for each method there exist contact configurations resulting in a large percentage of false negatives.This percentage can go as high as to 100% for PP4 and PP8 (i.e. the approximation says that no CoM position allows for equilibrium).This happens when the linearized cones do not contain the negative gravity direction, while the real cones do.On average, PP4 performs much worse than the others, while PP8 performs a bit better than QF.However, the better performances of PP8 comes at a high price in terms of computation time, which is more than 100 times higher with respect to QF.

B. Test 2 -Incremental Projection Revisited
In this test we compare the performances of the algorithms LP, PP, IP and IPR.We compared the algorithms for 2 and 4 rectangular contacts with the environment.We also tested the algorithms using either 4 or 8 generators per contact point to measure the percentage of false negatives when using 4 generators.For each random contact configuration we used the four algorithms to test 100 random CoM positions (sampled in a box slightly bigger than the convex hull of the contact points) and we measured the computation times.
Fig. 4 shows the mean computation times for 10 3 random contact configurations.As expected LP is more efficient than PP for a small number of tests-which varies between 5 and 20 depending on the number of contacts and generators.PP is extremely fast for the case of two contacts and 4 generators, but its cost quickly increases with the number of contacts and generators, making it appealing only for very large numbers of tests (> 100).IP performs better than LP and PP in many situations, especially for an average number of tests (5)(6)(7)(8)(9)(10)(11)(12)(13)(14)(15)(16)(17)(18)(19)(20) or for a large number of contacts and generators.IPR is overall the best algorithm because it is always the fastest (except for the case of 2 contacts and 4 generators, which is not very interesting given the large number of false negatives that we measured when using 4 generators).IPR exhibits the best performances in the case of 4 contacts and 8 generators, where it is about 3 times faster than IP.
Fig. 5 shows that the ratio between the mean number of LPs solved by IP and IPR as a function of the number of tests is almost constant (between 1.5 and 1.6).This holds true regardless of the number of contacts and generators.IPR solves then 50-60 % more LPs than IP, but being the LPs of IPR of dimension 4 rather than 3k + 2 it is not surprising that for k ≥ 8 IPR performs better than IP.

C. Test 3 -Planning with Robust Static Equilibrium
This last test compares the postures generated by our contact planner using the standard equilibrium test and the robust one.We only provide a qualitative evaluation, as it seems difficult to quantitatively validate the robustness of a  The contact planner [11] first finds a collision-free trajectory of the base of the robot using a probabilistic planner.It then iteratively computes the contacts that maintain the robot in equilibrium at each key pose.The contacts are obtained by random shooting, rejecting those that do not allow for equilibrium.In the original paper [11] the PP test was used; we replaced it here with a robust PP test and compared the results.
We present two different scenarios.In the first scenario the humanoid robot HRP-2 has to walk over rubble (see Fig. 6a).The ground is composed by three kinds of blocks (green, blue, red) having three different inclinations (0 • , 15 • , 20 • ).The robust planner naturally avoids stepping on the red blocks, because this would not result in a robust equilibrium.In the second scenario HRP-2 has to climb some stairs using a handrail for support.The non-robust planner tends to find postures where the CoM of the robot is close to the borders of its support region, which are clearly non-robust (see for instance Fig. 6b and 6c).The robust planner finds instead qualitatively better poses, such as the one depicted in Fig. 6d.The whole sequence of contact poses can be seen in the accompanying video.

VII. CONCLUSIONS
This paper addressed one of the central problem in control and planning of legged robots: testing static equilibrium.The presented algorithms advance the state of the art in two directions.First, from a computational standpoint, we presented two algorithms to test equilibrium that are faster than previous approaches.The first algorithm is actually a well-known technique, which we proved to be applicable to a much larger set of contact geometries (i.e.quasi-flat contacts).The second algorithm is a modification of the incremental projection technique [10], which resulted to be up to 3 times faster than the original version.Second, we proposed a method to test robust equilibrium that allows the robot not to lose contact in case of bounded forcetracking errors.We incorporated this feature in all the discussed techniques for testing equilibrium; we also showed the qualitative difference between the robust and non-robust posture sequences generated by a contact planner [11].
An interesting future direction is the extension of our IPR algorithm to handle quadratic cones, similarly to [9].
Another useful extension would be dynamic equilibrium.While on flat grounds the capture point [20] is a good indicator of dynamic equilibrium, no such thing exists for arbitrary contacts, but it is clear that the mathematical tools to deal with it are similar to the one discussed in this paper.
(a) The CoM c is inside Yout, but outside Y in : we can not determine whether c ∈ Y.(b) After one iteration c is outside Yout, so we can say that c / ∈ Y.
which in turns allows us to bound Y in direction D xy h: h D xy c xy + h d ≤ 0 (a) The CoM c is inside Yout, but outside Y in : we can not determine whether c ∈ Y.(b) After one iteration c is inside Y in , so we can say that c ∈ Y.

H
(w − Al) ≤ 0 → HD xy c xy + H(d − Al) ≤ 0 Finally, in the LP (9) solved by the IPR algorithm we just have to replace d with (d − Al) in the cost function.These simple modifications allow us to test for robust equilibrium at no additional computational cost.

Fig. 3 :
Fig. 3: Three examples of biped contact configurations in which at least one of the three considered methods presents a large number of false negatives.Only the false negatives are depicted in the figures.

Fig. 4 :
Fig.4: Mean computation times to test the equilibrium of 1-100 CoM positions using LP, PP, IP and IPR with 4 or 8 generator per contact point.Using 4 generators rather than 8 we had 19% / 14% of false negatives with two / four contacts.

Fig. 5 :
Fig. 5: Number of LPs solved to test the equilibrium of 1-100 CoM positions using IP and IPR with 4 or 8 generators per contact point.

Fig. 6 :
Fig. 6: Examples of robust and non-robust contact postures found by a contact planner.

TABLE I :
Results of Test 1.