Hervé Audren

Abderrahmane Kheddar

Pierre Gergondet

Stability polygons reshaping and morphing for smooth multi-contact transitions and force control of humanoid robots

Keywords: Body balancing, Locomotion, Humanoid Dynamics

In this paper we provide hindsight on how to use the information provided by the explicit computation of the stability polygon and build on task-based QP controllers to achieve both stable multi-contact smooth transitions and force control without jeopardizing stability. This entails computing stability polygons with unilateral and bilateral contacts and devising a method to continuously constrain the CoM position throughout multi-contact motion in order to effectively regulate the forces applied on the environment while avoiding hard constraints on the CoM and discontinuity between stances.

I. INTRODUCTION

Multi-contact technology [START_REF] Bouyarmane | Humanoid robot locomotion and manipulation step planning[END_REF], [START_REF] Vaillant | Multi-contact vertical ladder climbing with an hrp-2 humanoid[END_REF] can be used in several tasks such as climbing stairs using handrails or egress/ingress a car, or more generally access confined spaces. While several works have proposed efficient regulation control strategies in multi-contact configurations [START_REF] Ott | Posture and balance control for biped robots based on contact force optimization[END_REF]- [START_REF] Herzog | Balancing experiments on a torque-controlled humanoid with hierarchical inverse dynamics[END_REF], transitions between contact stances is still to be researched 1 .

One can enforce stability by controlling the Center of Pressure (CoP) at each contact to remain within the convex hull of its area [START_REF] Righetti | Optimal distribution of contact forces with inverse-dynamics control[END_REF]- [START_REF] Feng | Optimization Based Full Body Control for the Atlas Robot[END_REF]. This criterion can be added to the QP controller as a constraint. Yet, in these implementations, the CoM was given as a high-level task, and the CoP or contact forces were regulated often in the null-space or in conjunction with the CoM task. Moreover, CoP are illconditioned upon contact removal because they only exist when the normal contact force is non-zero. All papers using CoP treat this issue with an ad-hoc counter-measure.

In order to smoothly transition from contacts, we want to specify only a desired force profile on a given contact without endangering stability. Whole body controllers typically solve an optimization problem that aims at finding compatible joint accelerations, q and contact forces λ minimizing a set of task errors under constraints. Stability objectives typically only depend on the current joint state (position, velocity and acceleration) q, q, q. Thus, adding a task to minimize the 2norm of λ will not result in modification of angular values: the best local solution is redistributing the internal torques to fall as little as possible.

One usual way to circumvent the local-view issue is to use a preview controller to enhance the motion with a flavor of dynamics [START_REF] Audren | Model preview control in multi-contact motion-application to a humanoid robot[END_REF], [START_REF] Dai | Whole-body motion planning with centroidal dynamics and full kinematics[END_REF], but slow multi-contact motion can be achieved without preview [START_REF] Vaillant | Multi-contact vertical ladder climbing with an hrp-2 humanoid[END_REF], using only a closed-loop local task-based controller.

To ensure that the desired force is indefinitely sustainable, we want the controller to move the CoM to a stable region. However, this approach is not directly applicable as the CoM position only depends on q: a task error υ(q, λ) cannot be turned into a quadratic objective T that depends only on q and λ. Indeed, to do so one must differentiate υ with respect to time, but λ is our optimization variable.

Forcefully setting the CoM to a precise location will require to set gains that will interfere with almost all the other tasks, such as end-effectors trajectories. This is generally a very conservative measure. Thus, we may want the CoM objective to be among the tasks to be achieved at best when this is needed, but it should be allowed to freely move in a known region of stability, that can be a hard constraint.

In this study, we will limit our developments to multicontact with quasi-static motions. Hence, constraining the CoM to remain within the frictional static stability polygon is an acceptable criterion (ZMP assumes infinite friction at contact). The static stability is a polygon and allow us to have a better comprehension before moving to the general 3D case in future work. Yet this shape is neither modified according to desired contact forces nor continuous across contact changes. For the latter point, including it directly as a constraint in our QP controller would lead to its immediate violation upon contact transition.

To deal with the latter drawbacks, our main contribution is to propose a novel continuation method based on optimal matching of points to interpolate between convex polygons (those resulting from multi-contact stability region computation). This allows us to smoothly reshape the region of stability during contact removal and addition and hence make QP controllers robust to contact changes.

By formulating a pure CoM based constraint, we effectively decouple the CoM placement from the force regulation problem which allows us to perform force control in multicontact. Indeed, we can deform the static stability polygon obtained from the geometrical and frictional information of a stance to take into account the desired contact forces. Then, using a simple impedance control scheme, we regulate the applied force to the objective one without endangering stability. This is another novel contribution, as we do not move the CoM to a predetermined location, but inside a formally defined polygon that guarantees stability with respect to a certain force limitation. We note that our work applies to torque and position controlled robots, but it has more impact and value for the latter ones as redistributing internal torques has no effect on them.

We recall in section II the static stability polygon computation, then how we included it as a constraint our controller in section IV. We introduce our idea to smoothly constrain the CoM in section III before applying it to the stairs case in section V. We present a way to efficiently combine tasks with this constraint in section VI and present results obtained in another multi-contact scenario in section VII.

II. COMPUTATION OF THE STABILITY POLYGON

We use the algorithm in [START_REF] Bretl | Testing static equilibrium for legged robots[END_REF] to compute the stability polygon for static postures provided by the multi-contact planner. The exact stability polygon lies between an inner and an outer approximation, that are built iteratively from solving a series of second-order cone programs of the form: max.

com d T com s.t. A 1 x + A 2 com = t (1) ||Bx|| ≤ u T x
The solution com * of this problem is an extremal stable CoM position in the direction d i.e. there exists a set of forces x realizing t = mg 0 T under the non-linearized friction cones constraints defined by B and u. This wrench is composed of the total wrench generated by a set of forces x, at the origin, computed using the transformation matrix A 1 and the gravity wrench computed with A 2 given a CoM position com. For each of these problems, com * is added to the inner approximation, while the half-plane defined by

{ com ∈ R 2 |d T com > d T com
* } is added to the outer approximation. At each step, the difference between these two approximations is a set of triangles. The search direction for the next step is perpendicular to the edge of the inner approximation forming the triangle of maximum area. The algorithm stops when the difference between the outer and inner approximations area is less than a given precision σ. The authors prove that there is a strict upper bound on the number of iterations needed to reach σ.

In its original expression, this method is not applicable to the case of mixed unilateral-and-bilateral contacts, that we are using in our experiments [START_REF] Vaillant | Multi-contact vertical ladder climbing with an hrp-2 humanoid[END_REF]. The support region may be unlimited in some directions. Thus, we need additional constraints limiting the search region. As our model of bilateral contacts is simply a set of points with non-aligned friction cones, we still can use Equation 1 providing additional constraints on the acceptable CoM positions. Accordingly, we can either use a constraint | com| < r that will constrain the CoM to a rectangular (polygonal) region or a conic constraint of the form || com|| < r that will constraint the CoM to a circle. A circular constraint is difficult to approximate by a polygon. However, it may be closer to the real geometric constraint: using a maximum limb length per contact, we can write a series of constraints || com -l|| < r that ensure that the CoM never goes further than d from the contact at position l.

The stability region is unlimited is some directions because we can apply arbitrary forces on opposite contact points, allowing us to compensate for any momentum. In fact, the robot cannot apply infinite torques. Translating the torque constraint into a precise force limit of the form |x| < f m requires setting f m from the robot posture, whereas we reason on the CoM model to compute the stability polygon. Instead, we use a reduced torque constraint, |T x| < τ m where T represents the cross product between the contact points and the contacting link. In this case, τ m can be obtained from the characteristics of the link that is in contact (e.g. gripper actuators or feet/ankle actuators). This constraint is linear because we consider non-sliding contacts. This method is quite fast but not enough for real-time. It takes few seconds for over a hundred iterations. To solve the second-order cone programs, Equation 1, we use the convex optimisation package CVXOPT [START_REF] Andersen | CVXOPT: Python Software for Convex Optimization[END_REF] that natively supports conic constraints. Our implementation is sub-optimal as we recompute the area spanned by every edge at every iteration but the cost for solving each problem is by far the most consuming task. This means that even if we curb the time spent finding the next best edge, the overall runtime of the algorithm will not be significantly improved.

III. MORPHING STABILITY POLYGONS

Now, we are able to compute the stability region for each contact configuration. It is a polygon that can be reshaped in a more conservative form when needed. But more importantly, the static stability can be integrated in our controller as a hard constraint through 'CoM in Polygon' equations. When the robot changes contact configuration, the stability polygon shape for the following configuration is different from the current one. We do not want to reset the problem or to design a specific CoM task. Instead, we would like the constraints to continuously deform from a stability shape to the other while subscribing the CoM all along the deformation. In this case, the transition from a contact configuration to another is made smoothly while keeping hard constraints on the stability.

If we do not enforce the CoM to be maintained within a moving/morphing polygon, the QP controller might often start from an infeasible constraint set and fails. Controller failure is solved by our new Algorithm 1 to smoothly morph the stability polygons between stances and this is done within the QP controller. It is not necessary to use state-of-the-art 2D or 3D shape morphing found in the computer graphics and animation community, see e.g. [START_REF] Sederberg | A physically based approach to 2-D shape blending[END_REF], [START_REF] Alexa | As-rigid-as-possible shape interpolation[END_REF] because in this study we only morph 2D convex polygons. Furthermore, as we are interested in adding and removing contacts one at a time, successive stability polygons have a non-empty intersection. In particular, when adding a contact, the previous stability polygon is entirely included in the new one. Thus, our successive polygons will be both convex and have non empty intersections, which allows us to use a simpler, faster, morphing algorithm. Morphing polygons is typically done in three steps:

• Adding points to the shapes;

• Finding a correspondence between the points from the start and target shape;

• Generating intermediate shapes by interpolating each of these couples. The main difficulty when morphing polygons is that if corresponding points are badly chosen, the resulting polygon will self-intersect during interpolation.

To solve the problem of self-intersection during morphing, we use the Munkres (Hungarian) algorithm [START_REF] Munkres | Algorithms for the Assignment and Transportation Problems[END_REF]. This algorithm solves the assignment problem, i.e. finds a minimum weight matching in a weighted bipartite graph. In our case, we want to match every point from the target polygon to one from the current polygon while minimizing the sum of distances between each couple of matched points. In order to warrant that the current polygon has more points than the target polygon, we first extend the current polygon with the midpoint of each edge until it contains more points than the target. The result of the Munkres algorithm is the minimal, in terms of distance travelled, set of points a cur from the current polygon matching all points a tar from the target polygon. The remaining b points of the current polygon are mapped to their orthogonal projections onto the target polygon. The intermediate shape is generated by interpolating each couple of points by a percentage κ. Figure 2 presents the results obtained on simple shapes.

The Munkres algorithm is in O(n 3 ), yet pre-computations are possible. The start and target polygons only depend on the geometry and friction properties of each stance. Then, although the algorithm presented here directly gives back the interpolation, it is much more interesting to save the result, that is the set of correspondences a cur → a tar and then only compute the interpolation between each couple of points for a given morphing percentage κ at each time-step. Moreover, as we usually use a few tens of points, running the Munkres algorithm is not very costly. To map a 25-sided polygon onto a 50-sided one, it takes about 0.67 s. Solving the assignment problem takes about 95 % of the time. IV. TASK-BASED CONTROLLER Now we discuss how this morphing is integrated in the controller. We control our humanoid robot using the multicontact QP task controller described in [START_REF] Vaillant | Multi-contact vertical ladder climbing with an hrp-2 humanoid[END_REF], which compact form is:

min. y 1 2 y T Qy + c T y def = i β i T i (y) s.t. Ay ≤ B ; Cy = D
where y is the decision variable representing both the joint accelerations q, and the intensity of the forces λ applied at each contact point. The problem is written as a sum of quadratic or linear objectives weighted by β i (task gains). Encoded constraints include joint position, velocity and torque limits, non-sliding contacts, and non-desired collisions avoidance. Typical tasks are written either directly or using the set-point objective formulation and comprise a wide variety of objectives [START_REF] Vaillant | Multi-contact vertical ladder climbing with an hrp-2 humanoid[END_REF]. Here, we focus on the CoM task.

To subscribe the CoM within a moving polygon, we use a collision constraint as in [START_REF] Vaillant | Multi-contact vertical ladder climbing with an hrp-2 humanoid[END_REF], [START_REF] Kanehiro | Integrating geometric constraints into reactive leg motion generation[END_REF]. However, in our case the CoM interacts with a changing shape (see next section), which means that we have to take into account the speed and acceleration of the edges w.r.t the CoM, that is:

δ = n T • (J q -ṗ) (2) δ = ṅT • (J q -ṗ) + n T • J q + J q -p (3)
where p, n are respectively the position and normal of the CoM projection on one such edge, δ the resulting distance, and J the CoM Jacobian. We formulate a constraint:

u = J q -ṗ e = J q - p -dt n T J q ≤ ξ δ -δ s δ i -δ s + δ + dt [ ṅT u + n T e] (4)
with ξ a damping coefficient, δ i and δ s the interaction and safety distances respectively. Activating the above constraint whenever δ < δ i will ensure that δ is never smaller than δ s .

In a number of cases, the quantities ṗ, p, n, ṅ can be explicitly computed from the mapping obtained by applying Algorithm 1. Indeed for every ordered vertex v k of our current (convex) polygon that is the interpolate between a cur k and a tar k at κ(t), t being the time:

v k (κ) = a cur k (1 -κ) + a tar k κ ∂ n v k ∂t n = (a tar k -a cur k ) ∂ n κ ∂t n ∀n ≥ 1
Thus, for every point p k of the segment [v k , v k+1 ] at a distance α k from the v k and the associated normal n k :

∂ n p k ∂t n = n i=0 n i ∂ i α k ∂t i ∂ n-i v k ∂t n-i + ∂ i (1 -α k ) ∂t i ∂ n-i v k+1 ∂t n-i (5
)

n k = 0 -1 1 0 (v k+1 -v k ) def = N (v k+1 -v k ) (6) = N a cur k+1 (1 -κ) + a tar k+1 κ -(a cur k (1 -κ) + a tar k κ) = (1 -κ)N (a cur k+1 -a cur k ) + κN (a tar k+1 -a tar k ) = (1 -κ)n a cur k + κn a tar k (7) ∂ n n k ∂t n = (n a cur k -n a tar k ) ∂ n κ ∂t n (8)
Whenever two points of the current polygon map to the same point of the destination polygon, i.e. n a tar k = 0 0 , the direction of n k is constant but its norm is strictly decreasing. To avoid numerical issues whenever ||n k || is small, we consider n k constant with respect to κ in this case.

Thus, knowing the n th time derivative of κ and α, one can compute the n th time derivative of the segments' position and their normals. However, when κ or α depend implicitly on the time, these quantities have to be evaluated through numerical differentiation. We can make the following approximations to neglect high-order terms: -As the CoM only interacts with edges closer than δ i , it usually interacts with few edges at a time. Because all points are interpolated uniformly and because the constraint pushes the CoM perpendicularly to the edge, variations of α k are small during interaction: we consider that ∂ n α k ∂t n = 0 ∀n > 0.

-In most of our use-cases, we interact when the interpolation is almost done (the CoM is generally nearby the barycenter of the polygons. At this moment, p is either zero (when we interpolate at constant speed) or negative (using quadratic interpolation, the polygon is contracting and slowing down): we set p = 0. This puts a harder bound on the constraint. Overall rotation speeds are usually small compared to the homothetic part of the transformation, but we can not neglect ṅ. Depending on the shape of the support polygons, local rotations can be important, such as in the top corner of the polygon in Figure 2.

V. LIMITING THE COM REGION DURING MOTION

For the sake of clarity and without loss of generality, we illustrate our approach using the stair climbing with handrail task by our humanoid robot (HRP-2Kai). In our experiment trials the robot applied too much forces on the handrail. Although part of the excess of force is due to positioning error and would be solved by applying lowerlevel compliance, part of it comes from the fact that our QP controller does not explicitly reduce the force applied on the gripper. Adding a task of the form min ||S gripper λ|| 2 did minimize the force and torque computed, but kept the posture unchanged meaning that the force applied by the real robot would remain high (this was also observed for a torque controlled robot in [START_REF] Righetti | Optimal distribution of contact forces with inverse-dynamics control[END_REF], p. 288, last sentence). Thus, we want to modify the trajectory such that the force applied on the gripper dimishes without endangering stability.

A. Linking force limitation and interpolation

From intuition, maintaining the CoM in a shape that is similar to the static stability polygon of a restricted set of contacts should reduce the force applied on the others. We can compute the real static stability polygon with an additional force constraint: for a set of real numbers γ ∈ [0, 1] we compute the static stability polygon as described in section II with an additional constraint on the forces applied on each contact point of the grippers:

||f i || ≤ γmg (9)
Then we compute two stability polygons at each step of the climbing, i.e. every time the active set of contacts is changed: one using all contacts, the other using all contacts but those between grippers and rail, and their interpolation. We then find by dichotomy which κ yields the the interpolation of the "full" and "restricted" polygon with the closest area to the real constrained static stability polygon. Captions of this process are shown in Figure 3 while a numerical comparison of values of γ and κ are shown in Figure 4.

This shows us that interpolation is a good approximation of the force constraint with restrictions:

• Because we limit the search region, the interpolation retains the shape of this limit contrarily to the constrained static stability polygon.

• κ is a non-linear function of γ. Indeed, as a consequence of the above point, there is a "dead-zone" when the force constraint results in a shape modification smaller than the distance constraint. However, this function does not have a general expression. Indeed, even when linearizing the friction constraints, the two-dimensional stability polygon is the result of a series of nonlinear transformations. As our problem is defined as a series of inequalities describing contact geometry and friction and equalities describing stability conditions, the first step is to enumerate the vertices of the convex polyhedron hence described in the manifold of forces and CoM positions. This operation is called double description [START_REF] Fukuda | Double Description Method Revisited[END_REF]. Then, adding a linear force limitation constraint corresponds to capping this n-dimensional polyhedron, which may modify any number of edges, and will present discontinuities depending on which edges intersect with the constraint. Note that the two above operations can be computed in a single pass of double description, but capping would allow for efficient testing of various force limitations. Finally, the polygon is the convex hull of the projections of those vertices in the plane. Another non-linear, complex operation. It is thus impossible to derive a general relationship between γ and κ that is independent of the contact configuration. Moreover, as double description is an NPhard problem [START_REF] Khachiyan | Generating all vertices of a polyhedron is hard[END_REF], enumerating all vertices in a highdimensional space may be much more computationally costly than repeatedly computing only the projection, as the worst-case complexity is doubly exponential. Similarly, capping in high-dimensional spaces is much more expensive than in two or three dimensional space [START_REF] Günther | A dual approach to detect polyhedral intersections in arbitrary dimensions[END_REF].

-2 -1 0 -1 0 γ = 0.8 -2 -1 0 -1 0 γ = 0.6 -2 -1 0 -1 0 y (m) γ = 0.4 -2 -1 0 -1 0 x (m) γ = 0.2
• Interpolation is more accurate for extreme values of γ which is a direct consequence of using interpolation. Thus, maintaining the CoM inside a polygonal region using the constraint presented in section IV corresponds to limiting the force applied on the gripper. This polygonal region is smoothly interpolated from the current static stability polygon, to an intermediate one. We can still maintain a task of the CoM in the cost function with low gains and limit the CoM motion in the constraint part of the QP. Note that we can use a static stability polygon because the accelerations and speeds generated by our controller are small and because the safety distance δ s acts as a stability margin.

B. Results of stairs climbing

To limit the stability region, we use the constraints of section II in the stability polygon computation. We strictly limit the torque applied on the gripper, and loosely the forces and CoM position using the following values:

|f i | = |S i x| ≤ 5mg; |τ i | = |T i S i x| ≤ 5 N m; || com|| ≤ 1 m
With S i a selection matrix of forces at the i th contact. The interpolation between the current and target polygon takes place over the course of one second. To avoid strong discontinuities when the constraint gets activated, we use quadratic interpolation, i.e. a trapezoidal speed profile. We also use relatively high damping, ξ = 0.1, to further avoid discontinuity when activating the constraint. To make sure the QP does not fail, we allow a small violation of the constraint. The intermediate polygon is the interpolate at 90 %: we need to choose a high κ because we want to strongly limit the forces applied on the gripper. This κ corresponds to an upper force of about 34 N, which means that in the worst case the torque applied on the gripper is lower than our torque limit.

We first designed the motion, and checked that introducing the polygonal constraint in our controller does indeed modify the posture as shown in Figure 5. Moreover, introducing this constraint allowed us to check that our controller can indeed generate a trajectory without applying any force on the gripper.

We then modified the CoM objectives in each phase to ensure that the robot's flexibilities would not be too excited To make sure that the robot was able to climb the stairs in the real world, it was very important to ensure that the contacts were properly established before moving on to the next phase. Thus, every time a contact has to be added, we use a 6D position task as a form of impedance control to regulate the contact force. Its objective X(t) is updated as follows:

δf = f mes -f obj (10)
X(t + dt) = X(t) + K p δf + K d δ f (11)
With K p , K d positive diagonal gain matrices, f mes the measured 6 dimensional wrench, f obj an objective wrench. This task allows us to align the contacting bodies with the contacting surfaces by specifying a pure normal force objective as shown in the attached video.

The resulting trajectory is shown in Figure 6: while the constraint was correctly added to our QP controller, it is rarely activated. This confirms that the manual tuning concords with the stability information. Note that during this testing phase, being sure that the CoM would never exit the polygon was very useful as it allowed us to tinker without worrying about changes in other tasks disturbing the CoM. In that sense, this technique is a complement to posture generation as it adds explicit boundaries in between the known to be static stances.

However, we can see in Figure 8 that the effective force applied during the motion is often superior to the designated force. Although part of this error stems from the fact that the polygons are not recomputed according to the actual contacts taken and more generally modeling errors, most of it is due to the fact that a low-force solution only exists, but it is not the one we get when doing raw position control.

VI. COMBINING TASKS AND CONSTRAINTS FOR FORCE

CONTROL

In this section we present how to combine the CoM constraint and an impedance force control task to drive smooth removal of contacts. This is in fact a particular case of potentially more complex robot force control tasks. We also show that it is possible to recompute on the fly, but not in real-time the important polygons.

In a n contact scenario, we want to force control the i th contact, c i . We can then compute two geometry stability polygons:

P s = ({c k } k∈[0•••n] ) (12)
P d = ({c k } k∈[0•••n] k =i ) (13)
We can then interpolate between P s and P d at a rate κ(t). We choose κ(t) to have a trapezoidal speed profile of total duration 30 s. Denoting f k the force at the k th contact, we can derive an objective force at that contact, fk that is a function of κ. Knowing that we only control one contact, we choose:

fk = mg(1 -κ) (14)
We can then use an impedance force control task such as the one presented in section V to regulate the actual f k around fk by setting f obj to fk at each timestep. In reality, we only modify the impedance task objective whenever f k > fk to only maintain f k under the maximum desired force.

This linear objective in κ is in general sub-optimal as lower forces could be attained, but without an explicit relationship between κ and γ, we have to remain conservative.

VII. APPLICATION TO A MULTI-CONTACT SETTING

In this experiment, the controller establishes three contacts with the environment. Each of the two contacts that are added use the force control task to ensure paralellism of the contacting body and surface. When done, the robot has established three unilateral contacts with its environment: one between the right foot and the rear flat platform, one with its left foot and an inclined ramp and the last one is with its hand and an elevated flat block.

After establishing the contacts, we reposition the CoM before starting the experiment. During those few seconds, we asynchronously compute the corresponding polygons: P s and P d for every contact as well as every interpolator.

While maintaining those contacts, we interpolate P s towards P d for every contact: first for removing the right foot, then for removing the left foot and finally for removing the right hand. Another shorter experiment presented in Figure 9 and the attached video consists in doing only one interpolation, but then entirely removing the right foot.

In order to check that this technique would be applicable on the robot, we first tested it in the Choreonoid dynamics simulator [START_REF] Nakaoka | Choreonoid: Extensible virtual robot environment built on an integrated GUI framework[END_REF], which is very reliable (it embeds flexibilities Raw Filtered Fig. 8: Force (magnitude) applied on left hand during stairs climbing. Data was low-pass filetred by an order 2, Butterworth filter with a critical frequency of 10Hz. For phases, a,m,r stand for "add", "move" and "remove" respectively while R,L,F,H stand for "Right", "Left", "Foot", "Hand". at the ankle and noise) to ensure that the trajectory is indeed feasible and that the robot does avoid both environment and self-collisions.

In this experiment, we do not use a CoM task, only the constraint with wide safety margin. This ensures smoothness and stability. We also add a very low weight posture task to avoid having uncontrolled joints. The last task is the impedance control task that is only inserted when interpolating for the first contact. This will allow us to confirm its importance and role. Note that without an explicit force regulation task, as the Hessian of the problem is positive definite, the controller will try to minimize ||λ|| 2 i.e. an equal distribution that does not reflect the real forces being applied.

Whenever we finish the interpolation (i.e. κ = 1), the CoM is inside the reduced support polygon of the supporting contacts: there exists a zero-force solution -at the contact to be removed-to support the robot in this configuration. Of course, we do not know a priori if this solution fulfills the torque limits of the robot, but as we started from a feasible configuration and smoothly drove the robot to the desired one while continuously reducing the force applied on the Fig. 9: Snapshots of the force control experiment with HRP-2 to smoothly remove the right foot. contact, it is to be expected.

We show in Figure 10 how the force did decrease continuously, both in simulation and in reality albeit with slightly different profiles. We reach a zero-force configuration whenever the interpolation ends for the first contact. Note that for successive contacts, this is no longer true. When we do not explicitly aim for the desired force distribution, the robot establishes itself in a configuration that balances the contact forces with the joint torques generated by the lower level PD controller. Indeed, the worst case is shown in the third part, when interpolating to remove the hand: as the CoM is already almost inside the reduced polygon, the force applied on the left hand is only slightly reduced. However, it could safely be controlled to smoothly decrease towards zero as the interpolation index decreases as was done for the foot.

VIII. CONCLUSION

In this paper, we show that it is possible to enforce stability as a constraint in low dynamics multi-contact transitions. We devised a continuous morphing of the stability polygon as part of the constraint in QP controllers. We use this technique to limit the force applied on a given contact, whether by directly restraining the accessible region of the CoM throughout the movement or by specifying the force to be applied on a contact and displacing the CoM accordingly. Once the CoM is correctly placed, we can be sure that impedance or admittance control is usable to select the appropriate force distribution. Although we still use postures generated from planning, it is an interesting addition to it, as it allows us to modify the generated motion according to our needs without any expensive re-computation. This is even more interesting as it allows us to specify that the CoM has to remain in a deformable stability region instead of explicitly forcing the CoM to remain at the planned position.

As future work, we are working on the following improvement: extending this work to dynamic motion, and closedloop control. For the former, we need to extend the theoretical part to take into account accelerations explicitly instead of relying on safety margins (this is almost done). For the latter, we need to curb the computation time of the morphing (as it will depend on online measured/estimated forces) and that of the computation of the geometric stability polygon to run with the controller on the robot embedded computer. Similarly, to account for differences between actual surface shapes and planned ones we need to efficiently compute the actual shape from the off-line planned one, possibly by using the current robot state to narrow down the search area.

Fig. 1 :

 1 Fig. 1: Pie chart showing as angular sections the repartition of computation time for 150 iterations: total time 1.87 s

Fig. 2 :

 2 Fig. 2: Stability 2D polygon morphing: P s in blue, P d in red, the green and black arrows show the motion and the normals directions of the edges respectively.

Fig. 3 :Fig. 4 :

 34 Fig. 3: Comparison between direct computation of the constrained static stability polygon and interpolation.

Fig. 5 :Fig. 6 :

 56 Fig. 5: Climbing the stairs with HRP-2 Kai: with neither the constraint nor gripper torque task activated (left), with only the task (middle) and with both (right)

Fig. 7 :

 7 Fig. 7: Frames of HRP2 climbing the stairs using the handrail

Fig. 10 :

 10 Fig. 10: Results of multi-contact experiment and simulation. Top: normal force applied on all contacts during motion. Bottom: interpolation index.

  Algorithm 1 Interpolation of two polygons : f (P s , P d , κ) Input: P s , P d start and destination polygons, κ percentage a cur , done ← {}, {} while |P s | < |P d | do P s ← P s ∪ midpoints(P s ) end while M ← zeros(|P s | , |P d |) for a tar ∈ P d do for b ∈ P s do M ij = ||a tar -b|| 2 end for end for a cur , done ← Munkres(M ) for b ∈ P sdone do a tar ← P ⊥ (b, P d ) end for conv(interpolate(a cur , a tar , κ))

CNRS-AIST Joint Robotics Laboratory UMI3218/RL, Tsukuba, Japan

CNRS-UM LIRMM, Interactive Digital Human, Montpellier, France 1 Among the conclusions of the panelists of IEEE-RAS Humanoids 2015's workshop on Whole-Body Multi-Task Multi-Contact Humanoid Control

This work was supported by the H2020 European Project COMANOID

NOMENCLATURE α

Distance along the segment, see equation [START_REF] Audren | Model preview control in multi-contact motion-application to a humanoid robot[END_REF]


Vincent De Sapio

Oussama Khatib
email: khatib@robotics.stanford.edu

Scott Delp
email: delp@stanford.edu

Least action principles and their application to constrained and task-level problems in robotics and biomechanics

Keywords: Least action, Constraints, Task-level control, Musculoskeletal system, Optimization

ou non, émanant des établissements d'enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.

Introduction

Variational principles have shared a rich history in mathematics and physics. Their efficacy has been particularly apparent with regard to problems in mechanics. Of special importance among variational principles are so-called least action principles [START_REF] Bloch | Nonholonomic Mechanics and Control[END_REF][START_REF] Goldstein | Classical Mechanics[END_REF][START_REF] Marsden | Introduction to Mechanics and Symmetry[END_REF]. The fundamental theme associated with these principles is that the evolution of a dynamical system can be revealed by examining the stationary condition for an appropriately defined action integral.

In the field of robotics, there has been more interest in the multibody equations of motion that result from such principles than in the principles themselves. However, significant insight can be derived by using a least action principle as a point of departure for framing problems in robotics. In this paper, we will apply this approach to holonomically constrained systems and task-level control problems involving rheonomic servo constraints. We will also address task-level optimization problems where a task consistent path is sought which extremizes some criterion.

We begin with the application of least action to constrained systems, which yields the constrained Euler-Lagrange equations. A special case of this is investigated in the context of least curvature, or straightest path, on a constrained motion manifold. An example is presented which addresses the problem of computing geodesics for a surface, using a mechanical analog.

Subsequently, least action is applied to task space by interpreting the operational space approach [START_REF] Khatib | Inertial properties in robotic manipulation: an object level framework[END_REF] with regard to least action for rheonomically constrained systems. Task/posture decomposition is then applied to the problem of performing a task while minimizing some instantaneous criterion. Finally, by defining action in terms of a performance criterion, task-level optimization is formulated with respect to an extremization objective. Examples of these approaches are presented for model problems involving robotics and the biomechanics of movement.

Constrained least action

While least action can refer to a general family of variational principles, perhaps the most significant among these is Hamilton's principle of least action. This principle states that the path, q(t), of a system in configuration space over an interval, [t o , t f ], is such that the action is stationary under all path variations that vanish at the endpoints, q(t o ) and q(t f ). It is noted that this does not strictly imply a minimization of the action, as the name of the principle suggests, but rather an extremization of the action. For schleronomic systems (no explicit time dependence) the action is defined as,

I t f to L(q, q) dt ( 1
)
where q is a set of n generalized coordinates and L = T -V is the Lagrangian of the system. For conciseness, we will often refrain from explicitly denoting the functional dependence of L on q and q. This practice will also be employed with other quantities throughout the paper.

In the absence of constraints, least action requires that

δI = 0 ∀δ|δq(t o ) = δq(t f ) = 0 (2)
where δ represents the first variation. Least action can also be applied to multibody systems with auxiliary holonomic constraint equations. We introduce a set of m holonomic (and schleronomic) constraint equations, φ(q) = 0. The first variation of the constraint equations is δφ = Φδq = 0, where the matrix, Φ(q) = ∂φ/∂q ∈ R m C ×n , is the constraint Jacobian. In this case, the principle of least action can be stated as,

δI = 0 ∀δ|δq(t o ) = δq(t f ) = 0 and Φδq = 0. (3)
Thus, least action seeks the path, q(t), in configuration space that results in a stationary value of action, I , under all path variations, δq, that vanish at the endpoints and satisfy the constraints.

For forced systems, the principle of least action is modified such that the variation in the action is given by

δI = δ t f to L dt + t f to τ • δq dt. ( 4
)
Calculus of variations and integration by parts can be applied to this problem [START_REF] Goldstein | Classical Mechanics[END_REF]. The derivation is straightforward and leads to

δI = t f to ∂L ∂q - d dt ∂L ∂ q + τ • δq dt. ( 5
)
This makes use of the condition that the path variations vanish at the endpoints. The additional condition,

δI = 0 ∀δq|Φδq = 0 (6)
applied to (5) implies the following orthogonality relation at any instant,

d dt ∂L ∂ q - ∂L ∂q -τ • δq = 0 ∀δq ∈ ker(Φ) (7)
where ker() represents the kernel or null space operator. Thus,

d dt ∂L ∂ q - ∂L ∂q -τ ∈ ker(Φ) ⊥ = im Φ T ( 8
)
where im() represents the image or range operator. This implies the familiar constrained Euler-Lagrange equations,

d dt ∂L ∂ q - ∂L ∂q = τ + Φ T λ ( 9
)
where λ is a set of m unknown Lagrange multipliers. Identical equations could have been obtained by embedding the constraints directly in the Lagrangian. In this case, the Lagrangian in (4) would be replaced by the following augmented Lagrangian with the constraints adjoined, L aug (q, q, λ) L(q, q) + λ T φ(q) [START_REF] De Sapio | Simulating the task-level control of human motion: a methodology and framework for implementation[END_REF] and the stationary value of I would be sought for all variations that vanish at the endpoints. We note that,

δL aug = δL + λ T δφ = δL + λ T Φδq. ( 11
)
Using indicial notion with summation over repeated indices, we can express (9) as, [START_REF] De Sapio | Predicting reaching postures using a kinematically constrained shoulder model[END_REF] or in standard matrix form, M(q) q + b(q, q) + g(q) = τ + Φ T λ [START_REF] Gauss | Über ein neues allgemeines Grundgesetz der Mechanik (On a new fundamental law of mechanics)[END_REF] subject to φ(q) = 0. For the stationary value of I to correspond to a minimum requires that the second variation, δ 2 I , be greater than or equal to zero. In [START_REF] Vujanovic | An Introduction to Modern Variational Techniques in Mechanics and Engineering[END_REF], this condition is shown to be satisfied, for sufficiently small time intervals, [t o , t f ], if the following is satisfied,

∂ 2 T ∂ qi ∂ qj M ij qj + ∂ 2 T ∂ qi ∂q j qj - ∂T ∂q i b i + ∂V ∂q i g i = τ i + Φ ji λ j
t f to δ qT Mδ q dt 0 ( 14
)
which corresponds to M being positive definite over the actual path. For classical Lagrangian systems, this condition is met. Since (13) forms a set of second order differential equations, it is appropriate to complement it with the second derivative of the constraint equations,

φ = Φ q + Φ q = 0. ( 15
)
In practice, the integration of the forward dynamics would also require constraint stabilization to mitigate drift in the constraints. Baumgarte stabilization [START_REF] Baumgarte | Stabilization of constraints and integrals of motion in dynamical systems[END_REF] involves replacing our original acceleration constraint equations with a linear combination of acceleration, velocity and position constraint terms,

φ + β φ + αφ = Φ q + Φ q + βΦ q + αφ = 0. (16)
3 Least curvature or straightest path

In the previous section, we reviewed the application of the principle of least action to a general class of holonomically constrained multibody systems. In this section, we will restrict the domain of our examination to investigate alternate statements of least action.

The special case of a particle moving in n dimensions, r(t) = q(t) ∈ R n , under holonomic constraints but no applied force can be described by,

M r = Φ T λ subject to φ(r) = 0. (17)
This system can be equivalently represented using Gauss' principle of least constraint [START_REF] De Sapio | Operational space control of multibody systems with explicit holonomic constraints[END_REF][START_REF] De Sapio | Task-level approaches for the control of constrained multibody systems[END_REF][START_REF] Gauss | Über ein neues allgemeines Grundgesetz der Mechanik (On a new fundamental law of mechanics)[END_REF] which states that the Gauss function, G, is stationary under all acceleration variations that satisfy the constraints. That is,

δG = 0 ∀δ|δ q ∈ ker(Φ). ( 18
)
Since the Gauss function is a positive definite quadratic form, Gauss' principle is a strict minimization principle. For this system, the Gauss function is defined as

G 1 2 M( r -r ) T ( r -r ) ( 19
)
where r is the unconstrained acceleration of the system. That is, r is the acceleration that the system would exhibit in the absence of constraints. In this case, where no applied forces are present, r = 0.

So G = 1 2 M rT r = 1 2 M r, r . ( 20
)
The The implication that curvature is minimized reflects Hertz' principle of least curvature [START_REF] Hertz | The Principles of Mechanics Presented in a New Form[END_REF][START_REF] Lanczos | The Variational Principles of Mechanics, 4th edn[END_REF][START_REF] Papastavridis | Analytical Mechanics: A Comprehensive Treatise on the Dynamics of Constrained Systems for Engineers, Physicists, and Mathematicians[END_REF] which states that under force-free constrained motion, a system will follow the path of least extrinsic curvature, k, on the constrained motion manifold, Q p . Further, this constrained minimization implies ∇k 2 ⊥T r (Q p ), where T r (Q p ) denotes the tangent space of Q p at the point r. We note

∇k 2 = ∂ ∂r r 2 = 2r = 2 d 2 r ds 2 (24)
so

d 2 r ds 2 ⊥T r Q p . ( 25
)
This implies that the covariant derivative, D/ds, of the path tangent vanishes,

D ds dr ds = proj T d ds dr ds = proj T d 2 r ds 2 = 0 (26)
where proj T ( ) denotes the projection of a vector onto the tangent space. The intrinsic geodesic curvature of the path,

k g = D ds dr ds ( 27
)
is thus zero. This implies that under force-free constrained motion a system will follow geodesics (paths for which k g = 0) on the constrained motion manifold. This is illustrated in Fig. 1. Since a geodesic path minimizes arc length, the condition of zero geodesic curvature is equivalent to finding a path which minimizes the action defined in terms of arc length. That is

δI = δ ds = 0 ( 28
)
Fig. 1 Geodesic force-free paths on a constrained motion manifold. Force-free motion minimizes the extrinsic path curvature, k, subject to the constraints, yielding zero geodesic (intrinsic) curvature, k g subject to the constraints. Equivalently, for a system with no external forces, this fact can be concluded from Jacobi's form of least action [START_REF] Goldstein | Classical Mechanics[END_REF] which states,

δI = δ t f to T dt = 0 (29)
subject to the constraints. In this case, the Lagrangian has been replaced by the kinetic energy alone (no potential energy). Since

dt = M/2T ds ( 30
)
we have

δI = δ MT /2 ds = 0. ( 31
)
Because T is constant for this system (31) implies that arc length is minimized on the constrained motion manifold.

Computing geodesics using a mechanical analog

One application of the least curvature principle involves computing the geodesics of a surface by solving an analogous mechanical system. By the principle of least curvature, solutions of this system will trace out geodesics on the constrained motion surface Q p ⊂ R 3 .

Given the system

M r = Φ T λ subject to φ(r) = 0 (32)
where φ(r) = 0 is an implicit representation of the surface represented parametrically by r(u, v). Using the acceleration form of the constraint equation, the system of (32) can be solved to yield the differential equations,

r = -Φ T ΦΦ T -1 Φ ṙ (33)
or, incorporating constraint stabilization

r = -Φ T ΦΦ T -1 ( Φ ṙ + βΦ ṙ + αφ). ( 34
)
Thus, (34) represents a mechanically derived approach for computing geodesics. Equation (34) is not limited to R 3 and can be used to compute geodesics in R n .

Example

We can apply (34) to the problem of computing geodesics for the surface,

φ(x, y, z) = 4 + x 2 + y 2 -z - 1 3 cos 3x cos 3y 2 = 0. ( 35
)
The constraint Jacobian is computed directly from φ as

Φ = ∂φ ∂x ∂φ ∂y ∂φ ∂z (36)
and (34) yields a system of 3 second order nonlinear differential equations in x, y and z,

( ẍ ÿ z) T = -Φ T ΦΦ T -1 ( Φ ṙ + βΦ ṙ + αφ). ( 37
)
Specifying the point (x o , y o , z o ) = (.25, -.25, -1.85) as one initial condition, we can solve (37) using different departure directions, ( ẋo , ẏo , żo ). It is noted that the choice of the initial condition and departure directions must be consistent with the kinematics of the system. That is, φ(x o , y o , z o ) = 0, and Φ T ( ẋo ẏo żo ) = 0. The resulting geodesics are shown in Fig. 2.

It is noted that the specific time parameterization used does not affect the shape of the paths, only the speed at which they are traversed. Therefore, only the direction (not the magnitude) of the initial velocity dictates the shape of the path.

Least action in task space

The task space description of robotic systems, as formalized by Khatib with the operational space approach [START_REF] Khatib | A unified approach to motion and force control of robot manipulators: the operational space formulation[END_REF], presents an avenue of investigation with regard to the principle of least action. This is fundamentally due to the observation that tasks can be interpreted as rheonomic constraints enforced by a control law. The task space control problem can thus be interpreted as a rheonomically constrained least action problem.

We begin by expressing the dynamical equations for an unconstrained and fully actuated system in the form:

τ = J T f + N T τ p = M q + b + g (38)
where the control torque has been decomposed into a task component, J T f , and a null space or posture component, N T τ p . The term J (q) = ∂x/∂q ∈ R m T ×n is the Jacobian associated with task vector, x(q) ∈ R m T . The term f ∈ R m T is the operational space force, N (q) T = 1 -J T J T ∈ R n×n is the null space projection matrix, and J T ∈ R m T ×n is the dynamically consistent inverse [START_REF] Khatib | Inertial properties in robotic manipulation: an object level framework[END_REF] of the task Jacobian. The term τ p ∈ R n is an arbitrary generalized force vector that is projected into the null space, N T . Projecting (38) into task space yields the operational space equations [START_REF] Khatib | Inertial properties in robotic manipulation: an object level framework[END_REF],

f = Λ(q)ẍ + μ(q, q) + p(q) ( 39
)
where Λ(q) ∈ R m T ×m T is the operational space mass matrix, μ(q, q) ∈ R m T is the operational space centrifugal and Coriolis force vector, and p(q) ∈ R m T is the operational space gravity vector [START_REF] Khatib | Inertial properties in robotic manipulation: an object level framework[END_REF],

Λ = J M -1 J T -1 , ( 40
)
μ = ΛJ M -1 b -Λ J q, ( 41
)
p = ΛJ M -1 g. ( 42
)
As suggested earlier, tasks can be viewed as rheonomic servo (control) constraints [START_REF] Bajodah | Inverse dynamics of servo-constraints based on the generalized inverse[END_REF][START_REF] Blajer | A geometric approach to solving problems of control constraints: theory and a DAE framework[END_REF][START_REF] De Sapio | Task-level approaches for the control of constrained multibody systems[END_REF] which enforce some motion control objective. In particular, we can express a task in the form of a set of rheonomic trajectory constraints, x(q) = x d (t), where x d is a desired task space trajectory.

Noting that δx d = 0, since time is fixed under all virtual variations, we have δx = J δq = 0. Application of the principle of least action then yields:

δI = 0 ∀δ|δq(t o ) = δq(t f ) = 0 and J δq = 0. (43)
Thus,

δI = t f to ∂L ∂q - d dt ∂L ∂ q • δq dt = 0 ∀δq|J δq = 0. ( 44
)
This implies the following Euler-Lagrange equations,

d dt ∂L ∂ q - ∂L ∂q = J T f . ( 45
)
As in Sect. 2, identical equations could have been obtained by embedding the rheonomic constraints directly in the Lagrangian. Adjoining the constraints yields

L aug (q, q, λ) L(q, q) + f T x(q) -x d (t) (46)
and the stationary value of I would be sought for all variations that vanish at the endpoints.

Recalling that δx d = 0,

δL aug = δL + f T δx = δL + f T J δq. ( 47
)
In either case, (45) can be expressed as

J T f = M q + b + g (48)
subject to ẍ(q) = ẍd (t). This is similar to (38) except that no null space term is present. That is, τ = J T f (absent any null space torque) achieves the control objective consistent with least action. Projecting (45) into task space yields the operational space equations noted earlier,

f = Λẍ + μ + p. ( 49
)
Since the trajectory constraints are being imposed by a servo, in practice they cannot be perfectly achieved in open loop and drift would occur. Consequently the trajectory constraint equations need to be augmented with feedback terms. For example, the expression

ẍ = f = ẍd (t) + k v ẋd (t) -ẋ + k p x d (t) -x ( 50
)
represents a linear servo control law. This is analogous to the numerical constraint stabilization that was employed in [START_REF] Holzbaur | A model of the upper extremity for simulating musculoskeletal surgery and analyzing neuromuscular control[END_REF]. The equations of motion for our servo enabled system are

f = Λf + μ + p (51)
where the . represents estimates of the dynamic properties since the servo is assumed not to have perfect knowledge of the system. Equation (51) provides feedback linearization by compensating for the dynamics of the system.

Least action of cost criteria

We now pose the problem of minimizing a cost criterion subject to a motion control task. This can be performed for an instantaneous potential based criterion, U(q), by using a gradient descent method in conjunction with the task/posture decomposition of (38). Given the Lagrangian form of the system equations of motion

τ = d dt ∂L ∂ q - ∂L ∂q (52)
our overall control torque is

τ = J T f + N T τ p . ( 53
)
The posture term, τ p , can be chosen to correspond to the gradient descent, -∂U/∂q, of our cost criterion. In this case, the equations of motion are

J T f = d dt ∂L ∂ q - ∂L ∂q + N T ∂U ∂q ( 54
)
or in matrix form

J T f = M q + b + g + N T g U (55)
subject to the task ẍ(q) = ẍd (t). The term g U is the gradient of the potential, U , analogous to the gravity vector, g, in the Lagrangian system (gradient of the potential energy, V ). We complement (55) with the task space control law given by ( 50) and (51). Gradient descent seeks to reduce an instantaneous criterion rather than extremize a criterion over an integration interval. To address this latter case, we define the action integral associated with a cost criterion as

I t f to L(q, q) dt ( 56
)
where L(q, q) is an instantaneous criterion. If no task trajectory constraints are specified we have

δI = 0 ∀δ|δq(t o ) = δq(t f ) = 0. ( 57
)
It is noted that (56) and ( 57) represent an optimization problem where a path, q(t), is sought [START_REF] Biess | Simulating discrete and rhythmic multi-joint human arm movements by optimization of nonlinear performance indices[END_REF] rather than an optimal control problem where a control history, u(t), is sought. The optimal control problem will not be discussed here but the reader is referred to [START_REF] Bryson | Dynamic Optimization[END_REF][START_REF] Stengel | Optimal Control and Estimation[END_REF]. Equations ( 56) and (57) result in the Euler-Lagrange equations,

d dt ∂L ∂ q - ∂L ∂q = 0. (58)
Using indicial notion, we can express (58) as

∂ 2 L ∂ qi ∂ qj M L ij qj + ∂ 2 L ∂ qi ∂q j qj - ∂L ∂q i b L i +g L i = 0 (59)
or, in matrix form, M L (q, q) q + b L (q, q) + g L (q) = 0.

As in Sect. 2, if M L is positive definite over the actual path the stationary value of I corresponds to a minimum (given a sufficiently small time interval). Thus far, the sets of second order differential equations that have been derived from least action principles have been assumed to form initial value problems (IVP). That is, the initial conditions q(t o ) and q(t o ) were assumed to be specified, rather than the boundary conditions, q(t o ) and q(t f ), that appear in the action integral. It may, however, be more appropriate to pose the system of (58) as a two point boundary value problem (TPBVP) where q(t o ) and q(t f ) are specified. In this case, various shooting methods can be employed to solve the TPBVP. These solution methods will not be addressed here but the reader is referred to [START_REF] Bryson | Dynamic Optimization[END_REF][START_REF] Roberts | Two-Point Boundary Value Problems: Shooting Methods[END_REF].

Imposing rheonomic task trajectory constraints, x(q) = x d (t), implies δI = 0 ∀δ|δq(t o ) = δq(t f ) = 0 and J δq = 0 (61) which yields the system

d dt ∂L ∂ q - ∂L ∂q = J T λ (62)
or

J T λ = M L q + b L + g L (63)
subject to ẍ(q) = ẍd (t). Projecting (63) into task space yields the operational space equations for this system: λ = Λ L (q, q)ẍ + μ L (q, q) + p L (q) (64)

where Λ L , μ L , and p L are analogous to Λ, μ, and p. That is, they have the same form as the expressions in (40) through ( 42), but with M, b, and g replaced by M L , b L , and g L . Applying constraint stabilization, the trajectory constraints can be expressed as

ẍ = λ = ẍd (t) + β ẋd (t) -ẋ + α x d (t) -x (65)
and the constraint stabilized system is

λ = Λ L λ + μ L + p L .
(66)

Robotic cost criterion

The norm of the torque change in a robotic system serves as one example of a mechanical performance criterion. Using this criterion, we may seek a motion which extremizes the integral of the torque change over the movement interval (see Sect. 5.2). If we apply this optimization to the actuator torque associated with overcoming gravity, we have the following action integral.

I = t f to L(q, q) dt t f to ġ • ġ dt.
(67)

Example

We wish to solve the optimization problem of (56) and (61) for the n = 4 degree-of-freedom robot manipulator shown in Fig. 3. Specifically, a trajectory, q(t), is sought which moves the Fig. 3 (Left) A redundant torque actuated robot. Initial and final configurations, q(t o ) and q(t f ), associated with movement to a target, x f , along a prescribed path, x d (t), are shown. (Right) Time history of the robot between configurations q(t o ) and q(t f ). Motion corresponds to extremization of the gravity torque change action integral, subject to the rheonomic constraint Fig. 4 (Left) Time history of end effector trajectory for the robot manipulator showing compliance with the (Right) history of change, L(q, q) = ġ • ġ, associated with extremizing the action integral (67), subject to the rheonomic constraint robot end effector to a target location, x f , along a prescribed path, x d (t), while extremizing the action integral of (67) associated with gravity torque change. The manipulator is kinematically redundant with respect to the m T = 2 degree-of-freedom motion task specified.

It is noted that for the specific definition of L used in this example the expression,

M L ij = ∂ 2 L ∂ qi ∂ qj (68)
cannot generally be shown to be positive definite over any path that yields stationary action for arbitrary endpoint boundary conditions and task trajectory constraints. So, while least action will always yield an extremal solution it will not necessarily yield a minimal one in all cases. Under task constraints, the system which extremizes the action is given by ( 63), (65), and (66). Figures 3 and4 display time histories of joint motion, endpoint motion, and torque change for a simulation run. The solution yields the path between configurations q(t o ) and q(t f ) which extremizes gravity torque change, given rheonomic constraints on the end effector of the robot, x = x d (t).

Biomechanical cost criterion

We begin with a general consideration of biomechanical effort measures. An instantaneous effort measure can be used to seek a trajectory, consistent with task constraints, that minimizes the integral of that measure over the time interval of motion.

As a prelude to the muscle effort measure used here, we consider two effort measures based on skeletal dynamics. Mechanical power, P , can be used as an instantaneous effort measure. Power integrated over the movement interval yields

W t f to P dt = t f to τ • q dt. ( 69
)
The minimum work model [START_REF] Soechting | Moving effortlessly in three dimensions: does donders law apply to arm movement?[END_REF] thus seeks a trajectory, consistent with task constraints, that minimizes the work function, W . Rather than minimizing mechanical work another objective which may characterize human movement is the minimization of torque change.

The norm measure of instantaneous torque change is τ 2 = τ • τ . Integrating this measure over the movement interval yields

I t f to τ • τ dt. ( 70
)
The minimum torque change model [START_REF] Uno | Formation and control of optimal trajectory in human multijoint arm movement[END_REF] thus seeks a trajectory, consistent with task constraints that minimizes this integral.

The minimum work and minimum torque change models utilize quantities that are derivable purely from skeletal kinematics and that are not specific to muscle actuation. It is thus useful to consider an analogous measure which encodes information about the musculoskeletal system. Activation, which represents the normalized exertion of muscles, provides a natural starting point for constructing such a measure of neuromuscular action. Specifically, the magnitude of the muscle activation vector, a, has been used as an optimization criterion in both static and dynamic optimizations [START_REF] Anderson | Static and dynamic optimization solutions for gait are practically equivalent[END_REF][START_REF] Crowninshield | A physiologically based criterion of muscle force prediction in locomotion[END_REF].

We begin by defining an instantaneous muscle effort measure, L = a 2 . To express this measure in joint space, we first represent the joint torques, τ , in terms of muscle activation. To do this we employ a Hill-type active state model [START_REF] Zajac | Muscle and tendon: properties, models, scaling, and application to biomechanics and motor control[END_REF] with a stiff tendon assumption [START_REF] De Sapio | Simulating the task-level control of human motion: a methodology and framework for implementation[END_REF][START_REF] De Sapio | Predicting reaching postures using a kinematically constrained shoulder model[END_REF]. In a Hill-type active state model, activation dynamics is modeled as a first order differential equation, relating muscle activation to muscle excitation. We will not consider activation dynamics in the subsequent analysis since the dynamics of our musculoskeletal system will be assumed to be much slower than the activation dynamics. Nevertheless, it may be desirable to consider it for application of the subsequent methodology to fast dynamic motions.

Given a stiff tendon assumption, the following relationship can be established,

τ = K τ (q, q)a (71)
where K τ (q, q) is the muscle torque-activation gain matrix. That is, for a system of r muscles and n generalized coordinates it maps muscle activation, a, to joint torque, τ . The muscle torque-activation gain matrix is computed as

K τ = - ∂l ∂q T diag(f S ) = -L T K f ( 72
)
where l(q) is the vector of total muscle-tendon path lengths and f S (l M , lM ) is the vector of muscle forces at full activation (saturation), as a function of the muscle lengths and muscle contraction velocities. For a given muscle-tendon pair, the muscle length and contraction velocity can be computed from the total muscle-tendon path length/velocity as

l M = l -l T = l To and lM = l. (73)
The term l T is the tendon length and it is constant in the stiff tendon model. That is, l T = l To , where l To is a constant tendon slack length. The term L(q) = ∂l/∂q in (72) is the muscle Jacobian and K f (q, q) = diag(f S ) maps muscle activation, a, to muscle force, f . The saturation muscle forces, f S , used to construct K f are computed from a constitutive model [START_REF] De Sapio | Simulating the task-level control of human motion: a methodology and framework for implementation[END_REF]. A typical muscle force-length-velocity surface at full activation is shown in Fig. 5. The normalization constants f o , l Mo , and v Mo represent the maximum isometric force, optimal fiber length, and maximum contraction velocity, respectively. These are parameters specific to an individual muscle.

There are typically more muscles spanning a set of joints than the number of generalized coordinates used to describe those joints. Thus, (71) will have an infinite set of solutions for a. Choosing the solution, a o , which has the smallest magnitude yields,

a o = K + τ τ = K T τ K τ K T τ -1 τ (74)
where K + τ is the pseudo-inverse of K τ . Our instantaneous muscle effort measure can then be expressed as

L = a o 2 = τ T K τ K T τ -1 τ . ( 75
)
Expressing this effort measure in constituent terms and dissecting the structure, we have

L = τ T muscular capacity L T kinematics K f K T f kinetics L kinematics -1 τ . ( 76
)
This allows us to gain some physical insight into what is being measured. The terms inside the brackets represent a measure of the net capacity of the muscles. This is a combination of the force generating kinetics of the muscles as well as the mechanical advantage of the muscles, as determined by the muscle path geometry. The terms outside of the brackets represent the kinetic torque requirements of the task/posture. It is noted that the solution of (71) expressed in (74) corresponds to a constrained minimization of a 2 ; however, this solution does not enforce the constraint that muscle activation must be positive (muscles can only produce tensile forces). Imposing inequality constraints, a i 0, on the activations requires a quadratic programming approach to performing the constrained minimization. In this case, the solution of (71) which minimizes a 2 and satisfies a i 0 can be represented in shorthand as where a o is given by (77). Despite the preferred use of quadratic programming for computational purposes, (76) provides valuable insights at a conceptual level.

a o = qp K τ , τ ,

Example

In this problem, we consider a simplified n = 3 degree-of-freedom model of the human arm actuated by r = 14 muscles. The model is depicted in Fig. 6. The system is kinematically redundant with respect to the m T = 2 degree-of-freedom task of positioning the hand. The muscle attachment and force-length data were taken from the study of [START_REF] Holzbaur | A model of the upper extremity for simulating musculoskeletal surgery and analyzing neuromuscular control[END_REF]. The maximum isometric forces, f o , optimal fiber lengths, l Mo , and tendon slack lengths, l To , are shown in Table 1.

We wish to control the hand to move to a target, x f , location while minimizing an instantaneous muscle effort criterion defined as

U(q) g T K τ K T τ -1 g (78)
where K τ ∈ R 6×12 is computed from (72) and each muscle force is modeled as

f S (q) = f o e -5( l(q) l Mo -1) 2 . ( 79
)
In ( 78), the general torque term, τ , has been replaced by the gravity torque, g, since gravity terms dominate in this example where the motion is relatively slow. No task trajectory, x d (t), will be specified, just the final target location, x f . The system given by ( 50), ( 51) and (55) can then be solved. We use a simplified form of (50) and ( 51) such that,

f = k p (x f -x) -k v ẋ, (80)
f = f + p. (81)
In this case, no model of the dynamic properties (except for the gravity term, p) is included in (81). Also, the terms ẍd (t) and ẋd (t) have been omitted in (80) and x d (t) has been replaced by the final target location, x f , since the goal is to move to a target location without specifying a trajectory.

To the posture space portion of (55), we add a dissipative term, k d q, and a gain, k e , on the gradient descent term, g u . Finally, the gravity vector, g, is perfectly compensated for in the overall control torque. Thus, we have

J T f = M q + b + N T (k e g u + k d q). (82)
Figures 7 and8 display time histories of joint motion, hand motion and muscle effort for a simulation run. We can see that the controller achieves the final target objective while the null space control simultaneously seeks to reduce the instantaneous muscle effort (consistent with the task requirement). It is recalled that no compensation for the dynamics (except for gravity) was included in (81). Thus, there is no feedback linearization present in the control. Normally, perfect feedback linearization without explicit trajectory tracking would produce straight line motion to the goal. In the absence of feedback linearization, nonstraight line motion results.

We now wish to solve the optimization problem of (56) and (61). Specifically, a trajectory is sought which moves the hand to a target location (see Fig. 9), while extremizing muscle action,

I t f to L(q, q) dt. ( 83
)
In this case, we will define the instantaneous criterion as

L(q, q) = r i=1 l M i -l Mo i l Mo i 2 + r i=1 lM i v Mo i 2 + q2 3 . ( 84
)
(q) = g T (K τ K T τ ) - 1
g, associated with gradient descent. The null space control seeks to reduce the muscle effort but is also constrained by the task requirement It is noted that this is a modification of (76). Nevertheless, the criterion of (84) is just as suitable as (76) to demonstrate the relevant methods, and, it is computationally less demanding to generate and solve the Euler-Lagrange equations for (84) than for (76). The term q2

3 is included to mitigate excessive flexion-extension of the wrist.

As in Sect. 5.1.1, for the specific definition of L used in this example, the expression cannot generally be shown to be positive definite over any path that yields stationary action for arbitrary endpoint boundary conditions and task trajectory constraints. Under task constraints the system which extremizes the muscle action is given by equations (63) and (66), where the servo law,

M L ij = ∂ 2 L ∂ qi ∂ qj (85)
ẍ = λ = α(x f -x) -β ẋ (86)
is used. The solution yields the muscle action extremizing path between configurations q(t o ) and q(t f ), given the hand target constraint. Figures 9 and 10 display time histories of joint motion, hand motion and muscle effort for a simulation run. The straight line motion of the hand results from the feedback linearization employed.

Conclusion

In this paper, least action principles have been used as a template for investigating problems involving constraints and task-level control. With regard to constraints the principle of least action and the principle of least curvature were addressed for holonomically constrained multibody systems. In particular, the principle of least curvature was demonstrated as an insightful means of computing geodesics, using a mechanical analog.

Least action also provides a perspective for interpreting task-level control problems in terms of rheonomic servo constraints. Control in task space, as governed by the operational space equations of motion, was shown to involve the rheonomically constrained minimization of action. This fact provides a basis for investigating the constrained extremization of cost criteria. Replacing the Lagrangian, L, with a cost criterion, L, least action can be used to formulate an extremization problem. From the corresponding Euler-Lagrange equations, we derived the task space description of the system, given a set of rheonomic trajectory constraints.

Examples of action extremization were presented for a problem involving a torque actuated robot manipulator and for problems involving muscle-based actuation of a human arm model. The action extremization problem was solved for the example of a four degree-offreedom robot using a torque change criterion. Subsequently, a Hill-type model was used to establish a muscle-based cost criterion. An example was presented which demonstrated the gradient descent minimization of a muscle-based potential for a three degree-of-freedom human arm model. Another example was presented which demonstrated the extremization of muscle-based action integral for the same arm model. These examples serve to illustrate the efficacy of our approaches and provide a practical motivation for addressing more complex problems of this type in robotics and biomechanics.

Fig. 2

 2 Fig.2Solving the system of (34) geodesic force-free paths were computed for the surface φ = 4 + x 2 + y 2 -(z -1 3 cos 3x cos 3y) 2 = 0. All paths were chosen to emanate from the point (0.25, -0.25, -1.85)

Fig. 6 A

 6 Fig.6A simplified model of the human arm actuated by r = 14 muscles. The muscle attachment and force-length data were taken from the study of[START_REF] Holzbaur | A model of the upper extremity for simulating musculoskeletal surgery and analyzing neuromuscular control[END_REF]

Fig. 7 (Fig. 8 (

 78 Fig. 7 (Left) A redundant muscle-actuated model of the human arm. Initial and final configurations, q(t o ) and q(t f ), associated with gradient descent movement to a target, x f , are shown. (Right) Time history of the arm motion to the target. Motion corresponds to gradient descent of the muscle effort, subject to the task requirement

Fig. 9 (Fig. 10 (

 910 Fig.9(Left) A redundant muscle-actuated model of the human arm. Initial and final configurations, q(t o ) and q(t f ), associated with movement to a target, x f , are shown. (Right) Time history of the arm motion between configurations q(t o ) and q(t f ). Motion corresponds to extremization of the muscle effort action integral

  vector r can be parameterized in terms of arc length, s, in which case we have the following relationships, The term ds/dt is the particle speed, determined from the system state, and d 2 s/dt 2 is the tangential acceleration. Both k 2 and (d 2 s/dt 2 ) 2 are positive numbers. Additionally, they can be selected (and minimized) independently of each other. Therefore, minimizing G subject to the constraints implies that k 2 (and k) is minimized subject to the constraints and that d 2 s/dt 2 = 0.

				ṙ =	dr dt	=	dr ds	ds dt	(21)
	and
	r =	d dt	dr dt	=	d 2 r ds 2		ds dt		2	+	dr ds	d 2 s dt 2 .	(22)
	Substituting (22) into (20), and noting that dr/ds 2 = 1 and d ds dr/ds 2 = 0, we have
	G =	1 2	M k 2 ds dt	4	+	d 2 s dt 2	2	(23)
	subject to the constraints, where the term k = d 2 r/ds 2 = r is the extrinsic path curva-
	ture.

Table 1

 1 Maximum isometric forces, f o , optimal fiber lengths, l M o , and tendon slack lengths, l T o , for the 14 muscles used in the model of Fig.6. The muscle attachment and force-length data were taken from the study of[START_REF] Holzbaur | A model of the upper extremity for simulating musculoskeletal surgery and analyzing neuromuscular control[END_REF]

	Muscle	Span joint	f o (N)	l M o (cm)	l T o (cm)
	DELT1	shoulder	1142.60	9.76	9.30
	DELT3	shoulder	259.88	13.67	3.80
	TMAJ	shoulder	425.39	16.24	2.0
	TMIN	shoulder	354.25	7.41	7.13
	PECM1	shoulder	364.41	14.42	0.28
	PECM3	shoulder	390.55	13.85	13.20
	BRA	elbow	987.26	8.58	5.35
	TLAT	elbow	624.30	11.38	9.80
	TMED	elbow	624.30	11.38	9.08
	PT	elbow	566.22	4.92	9.80
	FDSM	wrist	91.03	7.49	31.80
	FDPM	wrist	81.65	8.35	29.30
	EPL	wrist	39.46	5.40	22.05
	EIP	wrist	21.70	5.89	20.50

Acknowledgements Support for this work was provided by National Institute of Health U54GM072970, HD33929 and HD046814. The authors would like to thank Jaeheung Park, James Warren and Clay Anderson for their helpful comments on this work. Vincent De Sapio would also like to thank Sandia National Laboratories for supporting this work.