Quang-Cuong Pham

Olivier Stasse

Time-Optimal Path Parameterization for Redundantly-Actuated Robots (Numerical Integration Approach)

Time-Optimal Path Parameterization (TOPP) under actuation bounds plays a fundamental role in many robotic theories and applications. This algorithm was first developed and perfected for classical serial robotic manipulators whose actuation is non-redundant. Yet, redundantly-actuated systems, such as parallel manipulators or humanoid robots in multi-contact tasks, are increasingly common in all fields of robotics. Here we extend the classical algorithm of TOPP (a.k.a. numerical integration approach) to the case of redundantlyactuated systems. As illustration, we present an application to multicontact trajectory planning for a humanoid robot.

I. INTRODUCTION

Given a robot and a smooth path in its configuration space, the problem of finding the Time-Optimal Parameterization of that Path (TOPP) under the robot actuation constraints (e.g. bounds on the joint torques) plays a fundamental role in many robotics theories and applications. For serial, normally-actuated manipulators (i.e. every joint is actuated), an efficient algorithm to solve this problem was first proposed in the 1980's [START_REF] Bobrow | Time-optimal control of robotic manipulators along specified paths[END_REF], [START_REF] Shin | Selection of near-minimum time geometric paths for robotic manipulators[END_REF] and has been continuously perfected since then, see [START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF] for a historical review.

TOPP for different degrees of robot actuation

Normally-actuated systems constitute however only a subclass of existing robot systems : under-and redundantly-actuated systems are increasingly common in all fields of robotics. Cases of underactuation arise when some of the degrees of freedom of the robot are not actuated : consider for instance a serial manipulator in which some of the joints are not actuated, or a flying robot whose non-actuated degrees of freedom correspond to the position and orientation of the base-link. In such cases, the difficulty for TOPP consists in that a given path in the robot configuration space may not be trackable at all, let alone time-parameterized. This case was essentially solved in [START_REF] Lynch | Collision-free trajectory planning for a 3-dof robot with a passive joint[END_REF], [START_REF] Bullo | Kinematic controllability for decoupled trajectory planning in underactuated mechanical systems[END_REF], where the authors give a characterization of the paths that can be time-parameterized. For a given time-parameterizable path, TOPP is no different from the normal actuation case.

Redundantly-actuated (or over-actuated) systems have more actuated joints than degrees of freedom. Actuation redundancy can be by design, such as in parallel (or closed-chain) manipulators, or can arise spontaneously when an otherwise non-redundantly-actuated robot interacts with the environment (consider e.g. a humanoid robot having two flat feet on the ground, or when it manipulates an object with both hands). In the former case, actuation redundancy is a means to eliminate singularities, to provide more homogeneous output forces, or to minimize the internal loading of the actuators [START_REF] Cheng | Dynamics and control of redundantly actuated parallel manipulators[END_REF]. As far as TOPP is concerned, actuation redundancy implies that any path in the robot configuration space can be time-parameterized (subject to actuation bounds), but contrary to the normal-and under-actuation cases, a given time-parameterization of the path may correspond to infinitely many possible torque profiles [START_REF] Nakamura | Advanced Robotics: Redundancy and Optimization[END_REF], [START_REF] Nakamura | Dynamics computation of structurevarying kinematic chains and its application to human figures[END_REF], [START_REF] Cheng | Dynamics and control of redundantly actuated parallel manipulators[END_REF], [START_REF] Righetti | Optimal distribution of contact forces with inverse-dynamics control[END_REF]. One possible way to deal with that redundancy is to fix a predefined load distribution [START_REF] Nakamura | Advanced Robotics: Redundancy and Optimization[END_REF]. However, it is clear that such a solution is suboptimal and would give rise to slower trajectories than what can be obtained using the full actuation available to the system. Our purpose here is to extend TOPP so that it can optimally handle actuation redundancy.

Quang-Cuong Pham is with School of Mechanical and Aerospace Engineering, NTU, Singapore. Olivier Stasse is with LAAS-CNRS, France.

Main approaches to TOPP

There are two main approaches to TOPP : numerical integration and convex optimization. Numerical integration is based on the Pontryagin Maximum Principle : the optimal velocity profile in the s -ṡ plane [s(t) denotes the position on the path at a given time instant t] is known to be "bang-bang" and can thus be found by integrating successively the maximum and minimum accelerations s, see [START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF] and also Section II-A for details. Convex optimization discretizes the s-axis into N segments and subsequently converts the original problem into a convex optimization problem in O(N ) variables and O(N ) equality and inequality constraints [START_REF] Verscheure | Time-optimal path tracking for robots: A convex optimization approach[END_REF]. This approach has the advantage of being versatile (it can for instance trade off time duration with other objectives such as energy or torque rate) and can rely on existing efficient convex optimization packages. However, the sheer size of the convex optimization problem makes this approach slower than its numerical integration counterpart, which exploits the bangbang structure of the TOPP problem in order to compute directly the optimal accelerations without any search process. Experimental comparisons in the case of non-redundantly-actuated robots supported this analysis [START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF].

Recently, the convex optimization approach has been extended to the case of humanoid robots in multi-contact tasks [START_REF] Hauser | Fast interpolation and time-optimization with contact[END_REF], which is a particular case of actuation redundancy [START_REF] Righetti | Optimal distribution of contact forces with inverse-dynamics control[END_REF]. The key of this extension lies in an ingenious polytope projection technique [START_REF] Bretl | Testing static equilibrium for legged robots[END_REF], which converts actuation constraints into "polygon" constraints in the s -ṡ2 plane.

Our contribution in the present paper is twofold. First, we show how the numerical integration approach can also handle "polygon" constraints, and implement the algorithm to do so as a new module in the TOPP library (https://github.com/quangounet/TOPP). This algorithm, combined with the polytope projection technique [START_REF] Bretl | Testing static equilibrium for legged robots[END_REF], [START_REF] Hauser | Fast interpolation and time-optimization with contact[END_REF], enables the numerical integration approach to address the case of redundant actuation, which in turn yields a significant improvement in computation time as compared to the convex optimization approach. Second, we show that the torque constraints for closed-chain manipulators can be reduced to the polygon form, which enables a unified, efficient, treatment of this class of robots together with humanoid robots in multi-contact tasks.

The remainder of this paper is organized as follows. In Section II, we show how to address "polygon" constraints within the numerical integration approach to TOPP and discuss how to reduce the actuation constraints of some general redundantly-actuated systems into these "polygon" constraints. In Section III, we present an application in a multi-contact task where a humanoid robot steps down a 30 cm-high podium. We show in particular that our approach enables a significant speed-up in computation time as compared to the convex optimization approach. Finally, Section IV briefly discusses the obtained results and future research directions.

II. TIME-OPTIMAL PATH PARAMETERIZATION CONSIDERING REDUNDANT ACTUATION

A. Background I : TOPP for non-redundantly-actuated systems

For completeness, we briefly recall below the classical TOPP algorithm for non-redundantly-actuated systems [START_REF] Bobrow | Time-optimal control of robotic manipulators along specified paths[END_REF], [START_REF] Shin | Selection of near-minimum time geometric paths for robotic manipulators[END_REF]. For details, the reader is referred to [START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF].

Consider an n-dof serial manipulator with dynamics M(q)q + q C(q) q + g(q

) = τ , ( 1
)
where q is the n × 1 vector of joint values, M the n × n manipulator inertia matrix, C the n × n × n Coriolis tensor, g the n × 1 vector of gravity forces and τ the n × 1 vector of actuator torques.

Assume that the manipulator is subject to lower and upper bounds on the joint torques, that is, for every joint i and time instant t,

τ min i ≤ τi(t) ≤ τ max i .
(2)

Consider now a path P -represented as the underlying path of a trajectory q(s) s∈[0,s end ] -in the configuration space. Assume that q(s) s∈[0,s end ] is C 1 -and piecewise C 2 -continuous. We are interested in time-parameterizations of P -or time-reparameterizations of q(s) s∈[0,s end ] -which are increasing scalar functions s : [0, T ] → [0, s end ].

To check whether a reparameterized trajectory satisfies the actuation bounds, one may differentiate q(s(t)) with respect to t q = qs ṡ, q = qss + qss ṡ2 ,

where dots denote differentiations with respect to the time parameter t and qs = dq ds and qss = d 2 q ds 2 . Substituting (3) into ( 1) and ( 2) and rearranging the terms then lead to

sa(s) + ṡ2 b(s) + c(s) ≤ 0, (4)
where a, b and c are vectors of dimension 2n. Each row i of ( 4) is of the form

ai(s)s + bi(s) ṡ2 + ci(s) ≤ 0. (5)
Next,

• if ai(s) > 0, then one has s ≤ -c i (s)-b i (s) ṡ2

a i (s)
. Define the

acceleration upper bound βi def = -c i (s)-b i (s) ṡ2 a i (s) ; • if ai(s) < 0, then one has s ≥ -c i (s)-b i (s) ṡ2 a i (s)
. Define the

acceleration lower bound αi def = -c i (s)-b i (s) ṡ2 a i (s)
. One can then define for each (s, ṡ)

α(s, ṡ) def = max i αi(s, ṡ), β(s, ṡ) def = min i βi(s, ṡ). (6)
From the above transformations, one can conclude that q(s(t)) t∈[0,T ] satisfies the constraints (4) 1 if and only if

∀t ∈ [0, T ] α(s(t), ṡ(t)) ≤ s(t) ≤ β(s(t), ṡ(t)). (7)
Next, observe that if α(s, ṡ) > β(s, ṡ) then, from [START_REF] Nakamura | Advanced Robotics: Redundancy and Optimization[END_REF], there is no possible value for s. Thus, to be valid, every velocity profile must stay below the Maximum Velocity Curve (MVC) defined by

MVC(s) def = min{ ṡ ≥ 0 : α(s, ṡ) = β(s, ṡ)} if α(s, 0) ≤ β(s, 0), 0 if α(s, 0) > β(s, 0). (8)
It was shown (see e.g. [START_REF] Shiller | Computation of path constrained time optimal motions with dynamic singularities[END_REF]) that the time-minimal velocity profile is obtained by a bang-bang-type control, i.e., whereby the optimal profile follows alternatively the β and α fields while always staying below the MVC. More precisely, the algorithm to find the timeoptimal parameterization of P starting and ending with the desired linear velocities v beg and v end is (see Fig. 1 for illustration) :

1. In the s -ṡ plane, start from (s = 0, ṡ = v beg / qs(0) ) and integrate forward following β until hitting either (i) the MVC, in this case go to step 2;

(ii) the horizontal line ṡ = 0, in this case the path is not dynamically traversable; (iii) the vertical line s = s end , in this case go to step 3. 2. Search forward along the MVC for the next candidate α → β switch point (cf. [START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF]). From such a switch point: (a) integrate backward following α, until intersecting a forward βprofile (from step 1 or recursively from the current step 2). The intersection point constitutes a β → α switch point;

1 Joint velocity constraints can also be taken into account, see [START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF].

1. Numerical approach to TOPP. The optimal velocity profile (blue) is obtained by integrating alternatively the maximum and minimum acceleration fields.

(b) integrate forward following β. Then continue as in step 1.

The resulting forward profile will be the concatenation of the intersected forward β-profile, the backward α-profile obtained in (a), and the forward β-profile obtained in (b). 3. Start from (s = s end , ṡ = v end / qs(s end ) ) and integrate backward following α, until intersecting a forward profile obtained in steps 1 or 2. The intersection point constitutes a β → α switch point. The final profile will be the concatenation of the intersected forward profile and the backward α-profile just computed.

B. Background II : polytope projection technique

For clarity, consider, in this section, a fixed position s0 on the path. Inequalities (4) can be seen as constraining the vector sa(s0) + ṡ2 b(s0) + c(s0) to stay in the negative orthant of R 2n . As we shall see in Section II-D, actuation bounds in redundantly-actuated systems imply the following more general condition

sa(s0) + ṡ2 b(s0) + c(s0) ∈ C , ( 9
)
where C is a (possibly unbounded) convex polytope of R 2n . The polytope projection technique [START_REF] Bretl | Testing static equilibrium for legged robots[END_REF], [START_REF] Hauser | Fast interpolation and time-optimization with contact[END_REF] enables one to compute, given the vectors a(s0), b(s0), c(s0) and the set of linear constraints defining the convex polytope C , the convex polygon P in the s -ṡ2 plane such that

(s, ṡ2 ) ∈ P if and only if sa(s0) + ṡ2 b(s0) + c(s0) ∈ C .
For completeness, we briefly recall below the polytope projection algorithm [START_REF] Bretl | Testing static equilibrium for legged robots[END_REF], [START_REF] Hauser | Fast interpolation and time-optimization with contact[END_REF]. Let first

C def = {(u, v, y) : v ≥ 0; ua(s0) + vb(s0) + c(s0) = y; y ∈ C }.
Since the constraints defining C are linear, C is a convex polytope. Next, given a vector d in the plane, one can find the extreme point of P (or "extremize") in the direction of d by solving the following Linear Program (LP)

max (u,v) d (u, v) such that (u, v, y) ∈ C . ( 10
)
The projection algorithms starts by finding extreme points of P in three directions of the plane, for instance (1, 0), (cos(2π/3), sin(2π/3)), (cos(4π/3), sin(4π/3)). This gives an initial polygon -which is actually a triangle. The algorithm then iteratively attempts to expand this polygon by "extremizing" in the outward normal direction of unexpanded edges, see Fig. 2A. The algorithm stops when all edges have been expanded (in this case, the final polygon is P), or when one has reached the maximum number of iterations (in this case, the final polygon is a subset of P).

In [START_REF] Hauser | Fast interpolation and time-optimization with contact[END_REF], the author then converts the obtained polygon back into M half-plane inequalities similar to [START_REF] Bullo | Kinematic controllability for decoupled trajectory planning in underactuated mechanical systems[END_REF] and feed these inequalities into a large convex optimization problem over the entire path.

C. TOPP-Polygon : numerical integration approach to redundantlyactuated systems

The fact that torque constraints -even in the non-redundant case -define a polygon in the s -ṡ2 plane has been remarked as early as in [START_REF] Pfeiffer | A concept for manipulator trajectory planning[END_REF], [START_REF] Shiller | Computation of path constrained time optimal motions with dynamic singularities[END_REF]. However, in the non-redundant case, it is more efficient to deal directly with the analytic expressions ( 6) and ( 8) rather than to construct an explicit polygon and use this polygon in subsequent computations.

By contrast, as shown in Section II-B, one is given the polygon in the redundant case. One possible approach might consist in converting the polygon into M half-plane inequalities similar to [START_REF] Bullo | Kinematic controllability for decoupled trajectory planning in underactuated mechanical systems[END_REF] and then try to apply the development of Section II-A. However, besides the computational cost required by the conversion, doing so would result in an O(M2 ) algorithm [to compute in particular the MVC by ( 8)], while proceeding directly with the polygon representation yields a simpler and faster O(M ) algorithm, as shown below.

To follow the numerical integration approach, one needs to compute the acceleration bounds α and β as well as the MVC. For a given ṡ0, one can compute α(s0, ṡ0) and β(s0, ṡ0) by simply finding the intersection of the horizontal line ṡ2 = ṡ2 0 with the edges of the polygon, see Fig. 2B. This can be done in time O(M ) where M is the number of edges of the polygon. To compute MVC(s0), it suffices to determine the highest vertex of the polygon, which can also be done in time O(M ).

A B

Fig. 2. A : Expanding an edge. At a given step of the algorithm, one chooses an edge that has not yet been expanded (red segment). One then computes the outward normal direction d to this edge and solves the LP defined in [START_REF] Verscheure | Time-optimal path tracking for robots: A convex optimization approach[END_REF]. The solution of this LP gives a new vertex (magenta disk), and one can finally replace the old edge (red segment) by two new edges (green segments). B :

The constraint polygon at a given s 0 in the s -ṡ2 plane.

The numerical integration approach requires furthermore determining the switch points on the MVC (cf. [START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF]). Determining the "discontinuous" and "tangent" switch points can be done as in [START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF] by stepping along the MVC. Regarding the "singular" switch points, it is not possible here to consider "zero-inertia" points [START_REF] Pfeiffer | A concept for manipulator trajectory planning[END_REF], [START_REF] Shiller | Computation of path constrained time optimal motions with dynamic singularities[END_REF], [START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF] -which are the points where ai = 0 in (5) -since the ai are not defined 2 . Thus, one has to step along the MVC and determine numerically the points where the MVC is continuous but not differentiable. Finally, to compute the optimal accelerations at the singular points, one cannot use the analytical value suggested in [START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF], also because the ai, bi and ci are not defined. Instead, we suggest here to search numerically for an approximate optimal acceleration as follows :

• Discretize the space of possible accelerations (-∞, +∞) into a large number of values (100 in our implementation); • For each discretized acceleration value β0, integrate forward a small number of steps using β0, evaluate the acceleration β1 at the end of the integration, and record the absolute difference |β0 -β1|; • Choose the acceleration value β0 that minimizes the absolute difference |β0 -β1|. Doing so ensures that the obtained profile is reasonably smooth around the singular switch point (see Fig. 3B).

We implemented the methods described above as a new TOPP-Polygon module in the TOPP library, see https://github.com/ quangounet/TOPP.

As a sanity check, we compared TOPP-Polygon with the original algorithm in [START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF], on a model of a serial, normally-actuated 7-dof manipulator (since the original TOPP cannot handle redundantlyactuated systems). Fig. 3 shows that the two versions produced nearly identical results. The profile computed by TOPP-Polygon was slightly less smooth near the dynamic singularity (around s = 0.6) and more false-positive singularities were detected (green dots). However, TOPP-Polygon was 50% faster in terms of computation time. This shows in particular that it would be suboptimal to convert the polygon constraints back into half-plane inequalities and apply the original TOPP on these inequalities.

A detailed comparison of TOPP-Polygon with the convex optimization approach, in a redundant actuation setting, is reported in Section III. on a model of a serial 7-dof manipulator. The two methods produced nearly identical results (MVC in magenta and velocity profiles in black), but TOPP-Polygon was 50% faster. Note that the singular switch point (green disk near s = 0.6 could be appropriately addressed using the method described in the main text.

D. Reductions of some redundantly-actuated systems to TOPP-Polygon form

We now discuss how to reduce some general classes of redundantly-actuated robots to the form of equation [START_REF] Righetti | Optimal distribution of contact forces with inverse-dynamics control[END_REF].

1) Closed-chain manipulators: Consider a closed-chain (or parallel) manipulator as in Fig. 4. Let q denote the joint values of the whole chain and qA the joint values of the nA actuated joints. Assume that the torques of the actuated joints are subject to lower and upper bounds, that is, for all i ∈ [1, nA],

τ min Ai ≤ τAi ≤ τ max Ai . ( 11
)
Following [START_REF] Nakamura | Dynamics computation of structurevarying kinematic chains and its application to human figures[END_REF], let qO denote the joint values of the tree structure obtained by cutting the chain at some specific joints. Consider a motion (q, q, q) of the chain. Assume that the tree structure Fig. 4. Left : A closed chain manipulator. If all joints are actuated, then q A = (q 1 , q 2 , q 3 , q 4 , q 5 , q 6 ). Right : Tree structure obtained by cutting the chain at joint 4. Here the joints of the tree structures are q O = (q 1 , q 2 , q 3 , q 5 , q 6 ).

makes the same motion as the closed chain. One can calculate the joint torques τ O of the tree structure by usual inverse dynamics algorithms [START_REF] Walker | Efficient dynamic computer simulation of robotic mechanisms[END_REF] τ O = M(qO)qO + q O C(qO) qO + g(qO), [START_REF] Bretl | Testing static equilibrium for legged robots[END_REF] where (qO, qO, qO) are simply extracted from (q, q, q). Next, it was shown that the torques of the actuated joints τ A satisfy

S τ A = W τ O , ( 13
)
where S and W are sensitivity matrices of appropriate dimensions computed from the kinematics of the chain and the choice of the actuated joints, see [START_REF] Nakamura | Dynamics computation of structurevarying kinematic chains and its application to human figures[END_REF] for details on the computations of these matrices.

As in Section II-A, introducing the path parameter s, differentiating ( 12) with respect to t, and rearranging the terms lead to (

τ O = sa(s) + ṡ2 b(s) + c(s), (14)
) 15
If there is no actuation redundancy, then S is invertible, and one can compute τ A by

τ A = S -sa * (s) + ṡ2 b * (s) + c * (s) .
Thus, the torque bounds on the actuated joints can be rewritten as :

for all i ∈ [1, nA], τ min Ai ≤ sa • i (s) + ṡ2 b • i (s) + c • i (s) ≤ τ max Ai ,
where

a • i (s), b • i (s), c • i (s) are the i-th component of the vectors S -a * (s), S -b * (s), S -c * (s).
Thus, in this case, one has reverted to the classical form of (4).

In case of actuation redundancy, S is non-square, hence noninvertible. Remark that the torque bounds of ( 11) can be alternatively written as

τ A ∈ C , where C = [τ min A1 , τ max A1 ] × • • • × [τ min An A , τ max An A ]. Let next C * = S C .
Since S is linear and C is a parallelotope, C * is clearly a convex polytope. We have thus shown that the motion (q, q, q) satisfies the torque constraints if and only if

sa * (s) + ṡ2 b * (s) + c * (s) ∈ C * ,
which has the form of ( 9) in Section II-C.

After obtaining the optimal parameterization s(t) t∈[0,T ] from TOPP, one can compute at each s, the generalized torques

τ G def = sa * (s) + ṡ2 b * (s) + c * (s).
To obtain the torques at each actuated joint, one may solve for instance the quadratic program

min τ A ∈C τ A 2 such that S τ A = τ G.
In the above optimization, other objectives rather than the instantaneous square torque are possible. For instance, one may seek to exploit redundancy to minimize torque changes between two consecutive time steps. In any case, the key point here is that such optimizations are guaranteed to find at least one solution since we have taken care to ensure that τ G ∈ S C .

2) Humanoid robots in multi-contact tasks: Consider the equation of motion of a humanoid robot with n joints M(q)q + q C(q) q + g(q) = τ

+ K k=1 J k f k , ( 16
)
where q is a vector of dimension (n + 6) representing the joint angles and the 6 degrees of freedom of the free-flying base-link, τ is the vector of torques -whose 6 coordinates corresponding to the free-flying base-link are set to 0, K is the number of contact points, J k and f k are the contact Jacobian and the contact force at contact point k.

For the robot to respect the torque constraints and be dynamically balanced, one must ensure that, for all i ∈ [1, n]

τ min i ≤ τi ≤ τ max i , and for all k ∈ [1, K], f k ∈ F k ,
where F k is the friction pyramid at the contact point. For computational efficiency, we consider friction pyramids instead of friction cones.

As previously, introducing the path parameter s, differentiating ( 16) with respect to t, and rearranging the terms lead to

sa(s) + ṡ2 b(s) + c(s) = τ + K k=1 J k f k , (17)
where a, b, c are vectors of appropriate dimensions. Note that, if the robot is free-flying or has one non-flat contact with the environment, then the system is under-actuated. If it has exactly one flat contact with the environment, then the system is normallyactuated and ( 17) can be inverted and put back into the classical form of (4). If the robot has more than one flat contact with the environment -e.g. when both feet lie flat on the ground or one foot lies flat on the ground and one hand lies flat on a table top, see Fig. 5 -then the system is redundantly-actuated [START_REF] Righetti | Optimal distribution of contact forces with inverse-dynamics control[END_REF]. In this case, denoting by C the convex polytope [START_REF] Hauser | Fast interpolation and time-optimization with contact[END_REF]

[τ min 1 , τ max 1 ] × • • • × [τ min n , τ max n ] n actuated coordinates × {0} × • • • × {0} 6 unactuated coordinates + K k=1 J k F k ,
enables one to transform equation ( 17) into the form of [START_REF] Righetti | Optimal distribution of contact forces with inverse-dynamics control[END_REF].

After obtaining the optimal parameterization s(t) t∈[0,T ] from TOPP, one may recover the actuated torques and contact forces by solving a quadratic program as in Section II-D1. It is also possible to perform more sophisticated optimizations as in [START_REF] Righetti | Optimal distribution of contact forces with inverse-dynamics control[END_REF]; the key point here is that the existence of at least one solution is guaranteed by the TOPP procedure.

III. EXPERIMENTAL RESULTS

We now present an example of optimal time parameterization for a 30-dof humanoid robot (HRP2-14, Kawada Industries) in a multicontact task consisting in stepping down a 30 cm-high podium 3 , see Fig. 5. During the whole task, the left foot of the robot should lie flat on the podium, while its right hand should lie flat on the handrail. As discussed in Section II-D2, this double flat contact creates a situation of actuation redundancy.

We first generated the initial configuration and the final configuration (first and last snapshots in the third row of Fig. 5). The flat contact constraints for the left foot and the right hand were enforced by inverse kinematics. We then generated, using a simple algorithm presented in the Appendix, a smooth trajectory connecting the two configurations while keeping fixed the positions of the left foot and the right hand.

Next, based on the developments of Section II, we computed the time-optimal reparameterization of that trajectory respecting (i) joint velocity bounds, (ii) joint torque bounds, (iii) friction pyramid constraints for the contact forces at the left foot and right hand. For safety, we constrained the normal components of the contact forces to be ≥ 1 N instead of > 0.

The optimal time-parameterization of the trajectory had duration 0.65 s, see Fig. 5. We now discuss the time taken by our implementation to compute that optimal parameterization. To calculate the MVC and to perform the numerical integration, we considered the grid size N = 100. The average number of edges per constraint polygon was 11.8. The time-parameterization step (excluding dynamics computations and polytope projection) took 0.005 s on our computer (Intel Core i5 3.2 GHz, 3.8 GB memory, GNU/Linux). On a comparable computer installation, also using C++, and for a humanoid multicontact problem where N = 100 and the average number of edges per constraint polygon was 9.1, the convex optimization approach took 2.46 s (also excluding dynamics computation and polytope projection, cf. page 1247 in [START_REF] Hauser | Fast interpolation and time-optimization with contact[END_REF]), which is about 500 times slower than our implementation. This significant improvement in computation time may come from two main factors. First, the numerical integration approach is inherently more efficient than the convex optimization approach since it exploits the "bang-bang" structure of the time optimization problem [START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF]. Indeed, at each point (s, ṡ) of the phase plane, the optimal acceleration can be analytically computed [by equation [START_REF] Cheng | Dynamics and control of redundantly actuated parallel manipulators[END_REF]], instead of being iteratively searched as part of a large optimization problem. Second, addressing directly the polygon form of the constraints as we did in Section II-C enables a further computational improvement with respect to handling half-plane inequalities as in the original TOPP algorithm.

To validate the results of our algorithm, we executed the reparameterized trajectory in OpenHRP, a highly realistic physical simulation environment. Snapshots of the movement (third row of Fig. 5) show that the robot maintained flat contacts at the left foot and the right hand throughout the whole movement.

The contact forces under the left foot and the right hand are also shown in Fig. 5. One can observe that the contact forces under the left foot saturated the positivity constraint between s = 0 and s = 0.05. Thus, the theory predicts that a higher execution speed will violate this constraint. That was indeed what we observed : when scaling up the velocity profile given by our algorithm by respectively 1.2 times and 2 times, we observed in the OpenHRP simulation that the left foot lifted up from the podium near the beginning of the trajectory, slightly at 1.2× and significantly at 2× (second row of Fig. 5, see also the video at http://youtu.be/4DVE1cUCh4M). Note that the motion is quasi-statically executable : scaling down the velocity profile by arbitrary coefficients will not violate the constraints. Planning motions that are dynamically executable but not quasi-statically executable using TOPP was suggested in [START_REF] Pham | Kinodynamic planning in the configuration space via velocity interval propagation[END_REF]. Extending this approach to humanoid robots in multi-contact based on the results of the present paper is the topic of ongoing research.

Finally, we executed the optimally-reparameterized trajectory on the actual robot. For safety reasons, we restricted the execution speed to 90% of the maximum speed calculated by our algorithm. The actual robot movement indeed respected the constraints (i), (ii) and (iii) mentioned earlier, see fourth row of Fig. 5 and also the video4 .

IV. CONCLUSION

We have presented an extension of the classical Time-Optimal Path Parameterization algorithm to the practically-important class of redundantly-actuated robots, which comprises in particular closedchain (or parallel) manipulators and humanoid robots in multicontact tasks. We implemented this extension within the framework of the TOPP library and tested it in a multi-contact task where a 30-dof humanoid robot steps down from a 30 cm-high podium. We showed that our implementation enabled a significant speedup in computation time as compared to existing implementations. This improvement is particularly crucial for the global trajectory optimization problem (i.e. where the path is not fixed), which requires calling the TOPP routine thousands of times [START_REF] Bobrow | Optimal robot path planning using the minimum-time criterion[END_REF].

Time optimality implies that, at any time instant, at least one of the constraints is saturated. Thus, in presence of uncertainties in the models of the robot or of the environment, it is advisable to consider constraints that are tighter than in reality : for example, we constrained the normal components of the contact forces to be ≥ 1 N instead of > 0 (cf. Section III).

We are currently investigating how to leverage the performance gain to plan through contact changes [START_REF] Hauser | Motion planning for legged robots on varied terrain[END_REF], [START_REF] Escande | Planning contact points for humanoid robots[END_REF]. In particular, integrating the results obtained here with the technique of Admissible Velocity Propagation [START_REF] Pham | Kinodynamic planning in the configuration space via velocity interval propagation[END_REF] may enable one to efficiently plan truly dynamic multi-contact motions for humanoid robots. Another research direction consists in studying how higher-order terms (such as jerk) can be taken into account [START_REF] Tarkiainen | Time optimal motions of manipulators with actuator dynamics[END_REF], in order to gain control authority on the flexibility of the robots.

Fig. 3 .

 3 Fig.3. Comparison of the original TOPP (Left) and TOPP-Polygon (Right) on a model of a serial 7-dof manipulator. The two methods produced nearly identical results (MVC in magenta and velocity profiles in black), but TOPP-Polygon was 50% faster. Note that the singular switch point (green disk near s = 0.6 could be appropriately addressed using the method described in the main text.

  where a, b, c are vectors of appropriate dimensions. Letting a * def = W a, b * def = W b, c * def = W c and left-multiplying equation (14) by W yield sa * (s) + ṡ2 b * (s) + c * (s) = W τ O = S τ A.

Fig. 5 .

 5 Fig.5. Time-optimal path parameterization for a humanoid robot stepping down a 30 cm-high podium. Top row, Left : Maximum Velocity Curves in magenta (plain line : MVC caused by the torque bounds and friction constraints; dashed line : MVC caused by the velocity bounds) and optimal velocity profile (black) calculated by our algorithm. Circles on the MVC indicate possible switch points (red : tangent switch point, green : singular switch points). Right : Normal components of the contact forces under the left foot (red) and the right hand (green) as calculated by our algorithm. The four points where the contact forces were computed were the front-left corner (plain lines), front-right corner (dashed lines), back-left corner (dotted lines) and back-right corner (dashed-dotted lines). Second row : Maximum elevation of the left foot near the beginning of the trajectory at different execution speeds (simulation in OpenHRP). Note that, at 120% of the maximum speed, the foot slightly lifted up from the podium, and at 200% of the maximum speed, the foot largely lifted up from the podium. Third row : Snapshots of the motion executed in OpenHRP at 100% of maximum speed. Fourth row : Snapshots of the motion executed on the actual robot at 90% of maximum speed.

Even if one converts the polygon into half-plane inequalities, there would be, in general, no way to index these inequalities so that the a i , b i and c i are continuous, since polygons computed at different points s 0 on the path may have different numbers of edges. Yet, the continuity of a i is necessary to determine "zero-inertia" points, and the differentiability of a i , b i and c i are required to compute the optimal acceleration near a dynamic singularity, see[START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF].

A minor part of this example application -including, in particular, neither the physical simulation nor the actual robot experiment nor the detailed explanation of the algorithm -was briefly mentioned in[START_REF] Pham | A general, fast, and robust implementation of the time-optimal path parameterization algorithm[END_REF].

In the video, the right foot of the robot makes a rather strong contact with the ground at the end of the movement. This was caused by the intrinsic flexibility of the robot, which makes movements at very high speed difficult to control, and has nothing to do with the contact conditions -on the left foot and the right hand -that are considered in the present work.

Acknowledgments

This paper has benefited from discussions with S. Caron, N. Mansard and L. Righetti. The authors would like to thank M. Naveau for help with the humanoid experiment. The first author is grateful to J.-P. Laumond for his kind invitation to visit LAAS-CNRS in the summer of 2014, which made the present work possible.

APPENDIX

A. Interpolating smooth trajectories respecting closed-chain constraints

Consider a link L contained in a closed chain and a partition of the joints angles q of the chain into qX and qY . The linear and angular velocities of link L is given by JX qX where JX is the Jacobian of the position and orientation of link L with respect to qX . Similarly, these velocities are also given by JY qY . Thus the closed chain constraint is given by JX qX = JY qY .

Assume that link L and the partition were chosen such that JX is square and invertible. For instance, for the closed chain of Fig. 4, this can be achieved by choosing L to be the link comprised between q3 and q4, qX = (q1, q2, q3), qY = (q4, q5, q6). In this case, qX is called the dependent, and qY the independent, joint angles. Next, a given small displacement δqY of the independent angles can be "compensated" by a displacement δqX of the dependent angles computed by

Based on the above development, we propose the following scheme to interpolate smoothly between two configurations q 0 and q 1 of the closed chain :

• interpolate smoothly and freely the independent angles between q 0 Y and q 1 Y (using e.g. third-degree polynomials); • compute qX by integrating δqX given by equation [START_REF] Escande | Planning contact points for humanoid robots[END_REF]. This scheme yields a valid interpolation if (see page 105 of [START_REF] Nakamura | Advanced Robotics: Redundancy and Optimization[END_REF])

• q 0 X and q 1 X are one the same branch of the inverse kinematics; • during the integration process, JX never traverses a singularity. Note that, if the independent trajectory qY is C k -continuous, then the compensating trajectory qX is also C k -continuous, and bounds on the derivatives of qX may be obtained from equation [START_REF] Escande | Planning contact points for humanoid robots[END_REF]. This contrasts with the interpolation method of [START_REF] Hauser | Fast interpolation and time-optimization with contact[END_REF], which cannot offer such guarantees.