Time Parameterization of Humanoid Robot Paths

Home , Humanoid robot

SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 1 Time Parameterization of Humanoid Robot Paths Wael Suleiman, Fumio Kanehiro, Member, IEEE, Eiichi Yoshida, Member, IEEE, Jean-Paul Laumond, Fellow Member, IEEE, and Andr´eMonin

Abstract—This paper proposes a unified optimization framework to solve the time parameterization problem of humanoid robot paths. Even though the time parameterization problem is well known in robotics, the application to humanoid robots has not been addressed. This is because of the complexity of the kinematical structure as well as the dynamical motion equation. The main contribution in this paper is to show that the time parameterization of a statically stable path to be transformed into a dynamical stable trajectory within the humanoid robot capacities can be expressed as an optimization problem. Furthermore we propose an efficient method to solve the obtained optimization problem. The proposed method has been successfully validated on the humanoid robot HRP-2 by conducting several experiments. These results have revealed the effectiveness and the robustness (a) Initial configuration (b) Final configuration of the proposed method. Index Terms—Timing; Robot dynamics; Stability criteria; Fig. 1. Dynamically stable motion Optimization methods; Humanoid Robot.

these approaches are based on time-optimal control theory [3], I.INTRODUCTION [4], [5], [6], [7]. The automatic generation of motions for robots which are In the framework of mobile robots, the time parameteriza- collision-free motions and at the same time inside the robots tion problem arises also to transform a feasible path into a capacities is one of the most challenging problems. Many feasible trajectory [8], [9]. The main objective, in this case, is researchers have separated this problem into several smaller to reach the goal position as fast as possible. subproblems. For instance, collision-free path planning, time Even though the conventional methods which are based on parameterization of a specified path, feedback control alonga the optimal-control theory have been successfully applied in specified path using vision path planning, etc. In this paper, practice on manipulators and mobile robots, the application of the problem of finding a time parameterization of a given path time optimal control theory in the case of humanoid robot is for a humanoid robot is investigated. however a difficult task. This is because not only the dynamic The time parameterization problem is an old problem in equation of the humanoid robot motion is very complex, robotic research [1]. In order to better understand the objective but also applying time optimal control theory requires the of time parameterization of a path, let us start by defining a calculation of the derivative of the configuration space vector path and a trajectory. of humanoid robot with respect to the parameterized path. A path denotes the locus of points in the joint space, or in Although such calculation can be evaluated from differential the operational space, the robot has to follow in the execution geometry, it is a very difficult task in the case of systems with of the desired motion, and a trajectory is a path on which a large number of degree of freedoms and branched kinematic time law is specified [2]. chains, which is the case of humanoid robot. For that, we pro- Generally speaking, the time parameterization of a path is pose to solve the time parameterization problem numerically the problem of transforming this path into a trajectory which using a finite difference approach. respects the physical limits of the robot, e.g. velocity limits, The remainder of this paper is organized as follows. Section acceleration limits, torque limits, etc. II gives an overview of the dynamic stability notion and the In the research works on manipulators, the time parameter- mathematical formulation of Zero Moment Point (ZMP). The ization problem has the objective of reducing the execution time parameterization problem is formulated as an optimiza- time of the tasks, thereby increasing the productivity. Most of tion problem under constraints and an efficient method to solve it is explained in Section III. Cases studies and experimental W. Suleiman, F. Kanehiro and E. Yoshida are with CNRS-AIST JRL results using the humanoid robot platform HRP-2 are given in (Joint Robotics Laboratory), UMI3218/CRT, National Institute of Ad- Section IV. vanced Industrial Science and Technology (AIST), Tsukuba Central 2, 1- 1-1 Umezono, Tsukuba, Ibaraki, 305-8568 Japan. {wael.suleiman, f-kanehiro, e.yoshida}@aist.go.jp II. DYNAMIC STABILITY AND ZMP: AN OVERVIEW J.-P. Laumond and André Monin are with LAAS-CNRS, 7 Avenue du Let us start by introducing some definitions: Colonel Roche, 31077 Toulouse, France. {jpl, monin}@laas.fr A part of this work has been done at LAAS-CNRS during the Ph.D. thesis Definition 1: Statically stable trajectory is a trajectory for of W. Suleiman. which the trajectory of the projection of the Center of Mass SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 2

(CoM) of the humanoid robot on the horizontal plane is always where [.]∧ designs the skew operator defined as follows inside of the polygon of support (i.e. the convex hull of all ∧ [.] : ω ∈ R3 → so(3) points of contact between the support foot (feet) and the 0 −ωz ωy ground). ∧ (8) [ω] = ωz 0 −ωx Definition 2: Dynamically stable trajectory is a trajectory   for which the trajectory of Zero Moment Point (ZMP) [10] is −ωy ωx 0 always inside of the polygon of support. where so(3) denotes the Lie algebra of SO (3) which is the group of rotation matrices in the Euclidean space. The generation of a statically stable path deals only with the The inverse operator of skew operator can be defined as kinematic constraints of the humanoid robot. It can be obtained follows ∨ by constraining the projection of the Center of Mass (CoM) [.] : Ω ∈ so(3) → R3 on the horizontal plane to be always inside of the polygon of ∨ 0 −ωz ωy support [11], [12], [13], [14]. ∨ [Ω] , ωz 0 −ωx Theoretically, any statically stable trajectory can be trans-   −ωy ωx 0 (9) formed into a dynamically stable one by slowing down the   humanoid robot’s motion. ωx = ωy Let the ZMP on the horizontal ground be given by the   following vector ωz T   p = px py (1) and the angular velocity can be calculated using the inverse operator as follows To compute p, one can use the following formula i ∨ n × τ o i dR iT p N ω = R (10) = o (2) dt (f |n) where the operator × and (.|.) refer to the cross and scalar Note that the skew operator and its inverse are linear products respectively, and operators. Finally, ω˙ i denotes the angular acceleration of the ith • N is a constant matrix link. 1 0 0 0 N = (3) Note that R and Ic0 are the rotation matrix associated 0 1 0 to the free-flyer joint (pelvis joint) and its inertia matrix . respectively. ω0 and ω˙ 0 are the angular velocity and the • the vector n is the normal vector on the horizontal ground T angular acceleration of the free-flyer joint respectively. n = 0 0 1 . • The vector f o is the result of the gravity and inertia forces III. TIME PARAMETERIZATION PROBLEM FORMULATION n Generally speaking, the time parameterization problem of o ¨ ci f = Mg − mi X (4) a function f (xt), where t denotes time, consists in finding a i=0 real function in such a way f xS verifies some temporal X St ( t ) where g denotes the acceleration of the gravity (g = constraints. −gn), and M is the total mass of the humanoid robot. Mathematically that means c th The quantities mi, X¨ i are the mass of the i link h(St) ≤ f (xSt ) ≤ l(St) (11) and the acceleration of its center of mass ci respectively. c0 Note that m0, X¨ are, respectively, the mass and the In order to obtain a causal and feasible motion, the function dSt acceleration of the free-flyer joint (pelvis joint) of the St should be a strictly increasing function, that means dt > 0. humanoid robot. Therefore we will express St as the integral of a strictly o o • τ denotes the moment of the force f about the origin positive function sh > 0, as follows of the fixed world frame. The expression of τ o is the t following St = sh dh (12) h=0 n Z ci o ci ci Our objective is to transform the statically stable path into τ = mi X × g − X¨ − L˙ (5) a dynamically stable trajectory by minimizing a specified i=0 X criterion. We will be interested in minimizing cost function where Lci is the angular momentum at the point c i of the form L˙ ci i i i i = R Ici ω˙ − Ici ω × ω (6) tf Ri I and ci are the rotation matrix associated to the J (St)= St + L (qs , q˙s , q¨s , τs ,sh) dh th i f h h h h i link and its inertia matrix respectively. ω is the Zh=0 angular velocity of the ith link can be obtained using tf tf = s dh + L (q , q˙ , q¨ , τ ,s ) dh the following formula h sh sh sh sh h Zh=0 Zh=0 i ∧ dR T T1 T2 ωi = Ri (7) dt (13) | {z } | {z } SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 3

• where qsh , q˙sh , q¨sh and τsh design the configuration vector, Constraint (16) guarantees that the ZMP is inside of the the joint velocity, the joint acceleration and the exerted torque polygon of support. vector respectively. The first term of the cost function T1 has • Constraint (17) ensures that the foot is in contact with the purpose of minimizing the final time tf , the second term the ground (the humanoid robot will not jump). T2 captures the desire to minimize an energy related function • Constraint (18) prevents the foot from sliding around the such as the exerted joint torques or the jerk function of some Z-axis. specific joints of the humanoid robot. • Constraint (19) guarantees that the obtained trajectory In order to obtain a motion within the humanoid robot respects the joint velocity limits of the humanoid robot. capacities, we will consider two cases: • Constraint (20) guarantees that the obtained trajectory 1) The physical limits which are taken into account are the respects the joint acceleration limits of the humanoid joint velocity and acceleration limits of the humanoid robot. robot. In this case, the function L in Eq. (13) is de- o o Let us write pst , fst and τst as functions of st fined independently of the exerted torques on humanoid o robot’s joints. n × τst ps = N (21) 2) The physical limits which are taken into account are the t o fst |n joint velocity and torques limits of the humanoid robot, and there is no constraint on the function L. where Even though the first case is included in the second one, we n c will propose an adequate and optimized method to solve each o ci ¨ ci L˙ i τst = mi Xt × g − Xt − t case. i=0 X n (22) o ci f = Mg − mi X¨ A. Case 1: joint velocity and acceleration limits st t i=0 Let us suppose that we have a path which consists of X K points. At first, we transform this path into a trajectory in which c by considering a uniform time distribution function. In other ci X i ∆Xt ∆ t−1 st−1 − st words, we suppose that st = 1 : ∀t in Eq. (12). Let the X¨ ci ∆t ∆t t = 2 sampling period of the desired trajectory be ∆t, we denote st st−1∆t (23) L˙ ci i i i i the time horizon T = K ∆t. t = Rt Ici ω˙ t − Ici ωt × ωt In this case, the time parameterization problem of transform- i ing the initial path into a dynamically stable trajectory within The angular velocity ωt can be obtained as follows the joint velocity and acceleration limits of the humanoid robot i ∨ can be expressed as an optimization problem as follow ∆R T ωi = t Ri t s ∆t t T T t i i ∨ min J(st) = min stdt + L (qst , q˙st , q¨st ,st) dt R − R − T s s 1 t t 1 i t t ( t=0 t=0 ) = Rt (24) Z Z st ∆t (14) ∨ i iT 1 I3 − R − R subject to = t 1 t st " ∆t # st > 0 (15) − + where I ∈ R3×3 is the identity matrix. pst ≤ pst ≤ pst (16) 3 i o The angular acceleration ω˙ t can be calculated as follows fst |n < 0 (17) µ f o |n < τ o |n < −µ f o |n (18) st st st − + i i q˙ ≤ q˙ ≤ q˙ (19) ω − ω − st ω˙ i = t t 1 − + t q¨ ≤ q¨s ≤ q¨ (20) st∆t t ∨ ∨ − i i T − i i T (25) − I3 Rt−1Rt I3 Rt−2Rt−1 + s − − s where pst is the ZMP vector, pst and pst design the t 1 ∆t t ∆t polygon of support for the humanoid robot. f o and τ o denote st st = 2 the applied force on the foot (feet) and the moment of this st st−1∆t force about the origin of the fixed world frame respectively. In similar way, we obtain µ is the coefficient of the static friction. The vector q˙st and q¨st denote the joint velocity and acceleration of the humanoid ∆qt + + q˙st = robot respectively. q¨ and q˙ design the upper limits of st∆t acceleration and velocity respectively. q¨− and q˙− design the (26) lower limits of acceleration and velocity respectively. ∆q ∆qt−1 t s − − s The constraints of the optimization problem (14) can be q ∆t t 1 ∆t t ¨st = 2 analyzed as follows st st−1∆t SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 4

1) Reformulation of the inequality constraints: It is clear As a result, all constraints of the optimization problem that the constraints (16 ∼ 20) are rational functions in the are transformed into polynomial functions with respect to st. time parameterization function (st). In order to accelerate the Recall that the original constraints of the optimization problem convergence rate of the optimization problem and the accuracy before the reformulation are rational function with respect to of the obtained solution, one might think of transforming the st. By using this reformulation the convergence rate of the inequality constraints from rational functions to polynomial optimization problem has been considerably improved. functions. 2) Discretization of The Solution Space: As it is well In order to reformulate the inequality constraints, let us start known, the space of the admissible solutions of the minimiza- by the constraint of ZMP (16): tion problem (14) is in fact very large. In order to transform + this space to a smaller dimensional space, we can use a basis − + pst pst ps ≤ pst ≤ ps ⇔ ≤ − t t −ps −p of shape functions (e.g. cubic B-spline functions). t st −N n × τ o + f o |n p+ Let us consider a basis of shape functions Bt that is defined ⇔ st st st ≤ 0 N n × τ o − f o |n p− as follows st st st (27) 1 2 l T 2 Bt = Bt Bt ··· Bt (35) As D(st) = st st−1∆t > 0 : ∀st, we multiply the two sides of inequality (27) by D(st) and obtain i where Bt denotes the value of shape function number i at the instant t. The dimension of Bt is l which defines the −N n × τõ + fõ |n p+ − + st st st dimension of the basis of shape functions. ps ≤ pst ≤ ps ⇔ ≤ 0 t t  o õ −  The projection of s into the basis of shape functions B N n × τ˜st − fst |n pst t t (28) can be given by the following formula   where n l o ci ˜ ci ˜ ci i i T ¨ L˙ st = sB Bt = sB Bt (36) τ˜st = mi Xt × g˜ − Xt − t i=0 i=1 X n (29) X õ ¨˜ ci fst = Mg˜ − mi Xt Thus, the optimization problem (14) can be rewritten as i=0 follows X in which l T T 2 g˜ = g st st− ∆t k k 1 min s B dt + L (qs , q˙s , q¨s ) dt s B t B B B Xci ∆Xci B ( t=0 t=0 ) ˜ ci ∆ t t−1 k=1 Z Z X¨ = s − − s X t ∆t t 1 ∆t t i s − L˜˙ ci = Ri I ω˜˙ − t 1 I ω˜ i × ω˜ i subject to t t ci t ∆t ci t t ∨ i iT T i I3 − Rt−1Rt sB Bt > 0 ω˜ t = " ∆t # o õ + N n × τ˜sB + fsB |n psB ≤ 0 ∨ ∨ T T I Ri Ri I Ri Ri o o − ˜ i 3 − t−1 t 3 − t−2 t−1 N n × τ˜ − f˜ |n p ≤ 0 ω˙ t = st−1 − st sB sB sB " ∆t # " ∆t # õ (30) fsB |n < 0 It is clear that τõ and fõ are polynomial function with st st õ o õ µ fs |n < τ˜s |n < −µ fs |n respect to st. As a result, the inequality constraint of ZMP B B B trajectory becomes polynomial function with respect to s . ∆q t q˙− sT B ≤ t ≤q˙+ sT B In similar way the constraints (17 - 18) can be transformed B t ∆t B t into the following equivalent ones − ∆qt T ∆qt−1 T + q¨s ≤ sB Bt−1 − sB Bt ≤ q¨s B (∆t)2 (∆t)2 B fõ |n < 0 (31) st (37) õ o õ in which µ fst |n < τ˜st |n < −µ fst |n (32) + T T T + − The velocity and acceleration limits can be reformulated as q¨sB = Bt sB sBBtsBBt 1 q¨ − T T T − (38) follows − q¨sB = Bt sBsBBtsBBt 1 q¨

− ∆q s q˙ ≤ t ≤ s q˙+ (33) Thus, the optimization problem has been transformed into t t l ∆t ﬁnding the vector sB ∈ R . 2 − ∆qt ∆qt−1 2 + − − − In order to transform this optimization problem into a st st 1 q¨ ≤ 2 st 1 − 2 st ≤ st st 1 q¨ (34) (∆t) (∆t) classical optimization problem, let us introduce a constant SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 5

R k ˜ k ǫ ∈ : 0 <ǫ ≪ 1 and the following deﬁnitions where sB is the unconstrained minimum of J(sB, λψ). Such k ∗ l T T updating rule will generate a sequence λψ converges to λψ k k [17]. In practice, a good schedule is to choose a moderate σ0, J (sB)= sB Bt dt + L (qsB , q˙sB , q¨sB ) dt (k t=0 t=0 ) and increase it as follows X=1 Z Z

T k+1 k −sB Bt + ǫ σ = ασ (44) N n × τõ + fõ |n p+  sB sB sB  o õ − where α is between 5 and 10. A threshold σmax is chosen N n × τ˜ − f |np k  sB sB sB  and the update rule of σ stops when σ becomes higher than  o   f˜ |n + ǫ  σmax.  sB     − τõ |n + µ fõ |n + ǫ  For more details on the algorithm of augmented Lagrange  sB sB    multiplier method see [18], [15], [17].  τõ |n + µ fõ |n + ǫ   sB sB  G(sB )=   3) Implementation Algorithm: The algorithm of the imple-    ∆qt + T  mentation can be summarized as follows  − q˙ s Bt   ∆t B    1) Given a path which is supposed to be statically stable.  ∆qt − T   − + q˙ s Bt  2) Split the path into various time segments depending  ∆t B    on the place and shape of support polygon during the  ∆qt T ∆qt−1 T +   2 s Bt− − 2 s Bt − q¨  motion. The support polygon for each time segment is  (∆t) B 1 (∆t) B sB    fixed.    ∆qt T ∆qt−1 T −  3) Choose ∆t, this value is usually determined by the − 2 s Bt−1 + 2 s Bt + q¨s   (∆t) B (∆t) B B  sampling period of the humanoid robot’s control loop.   (39) For instance, for the humanoid robot HRP-2 t Thus the optimization problem (37) can be transformed to the ∆ = 5.0 × 10−3sec. following classical form 4) Transform each time segment of the initial path into min J(sB) a trajectory by considering a uniform time distribution sB (40) function (st =1, ∀t). subject to ci ci i 5) Calculate ∆qt, Xt , ∆Xt and Rt (i =0, 1, ··· ,n), G(sB ) ≤ 0 for each time segment of the path. Recall that the path c0 0 The above optimization problem has been extremely studied in is defined by the parameters qt, Xt , Rt , where qt the literature of optimization theory. To solve this optimization designs the configuration vector of the humanoid robot c0 0 problem, one can use the augmented Lagrange multiplier joints, Xt and Rt denote the position and the rotation method, which is a very efficient and reliable method [15], matrix in the Euclidean space of the free-flyer joint [16]. Based on the augmented Lagrange multiplier method, (pelvis joint). the optimization problem (40) is transformed into the mini- 6) Calculate the cubic B-spline functions. mization of the following function 7) Solve the optimization problem (37) for each time segment with the initial solution obtained by applying ˜ T 1 T min J(sB, λψ)= J(sB)+ λψ ψ + σψ ψ (41) the above steps. sB ,λψ 2 −1 where ψ = max G(sB ), σ λψ , and σ > 0. Then there ∗ ∗ exist λψ such that sB is an unconstrained local minimum of ˜ ∗ B. Case 2: joint velocity and torque limits J(sB , λψ) for all σ smaller than some finite σ¯. To solve the unconstrained optimization problem of J˜(s , λ ) B ψ In this case, we suppose that we have a path which consists with respect to s , one can use Gauss-Newton method. B of K points. Similarly to the precedent case, we transform Note that the function J˜(s , λ ) is differentiable in s if B ψ B this path into a trajectory by considering a uniform time and only if J(s ) and G(s ) are differentiable in s . B B B distribution function. We denote the time horizon T = K ∆t, As we have prove G(s ) is polynomial with respect to s B B where ∆t is the sampling time. and its derivative can be calculated easily. The cost function In this case, the time parameterization problem of transform- J(sB ) is defined by the user and it is supposed to be derivable ing the initial path to a dynamically stable trajectory within with respect to sB and its derivative is continuous. So in this case we can write the joint velocity and torque limits of the humanoid robot can be expressed by the following optimization problem ∂J˜(sB, λψ) ∂J(sB ) T ∂G(sB) = + max {0, λψ + σG(sB )} ∂sB ∂sB ∂sB (42) As λ∗ is unknown, an update rule is used T T min J(st) = min stdt + L (qst , q˙st , τst ,st) dt k+1 k k st st ( t=0 t=0 ) λψ = λψ + σψ(sB ) (43) Z Z SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 6

subject to Thus, the time parameterization problem can be transformed into the classical optimization problem st > 0 − + min J(st) s pst ≤pst ≤ pst t o subject to (48) fst |n < 0 o o o (45) µ f |n < τ |n < −µ f |n G(st) ≤ 0 st st st q˙− q˙ q˙+ Similarly to the previous case, the above problem can ≤ st ≤ − + be solved using the discretization of solution space and the τ ≤τ s ≤ τ t augmented Lagrange multiplier method. where τst designs the vector of exerted torques on the hu- − manoid robot’s joints. τ and τ + denote the lower and upper C. Discussion limits of the torque vector. In this section, we discuss how to split the initial path into By including the torque limits into the time parameterization time segments according to the support polygon and the global problem, the obtained motion will not damage the motors optimal solution of the optimization problem. located in the articulated joints, and the robot will not stop • The support polygon is a function of s , and it depends because of high exerted torques. B on the horizontal position of CoM. However, as the given However, solving the time parameterization problem, in this path is a statically stable one, it can be split into various case, becomes more difficult. This is because the equation of sections. Each section is a statically stable path which has motion of the humanoid robot should be taken into account, a fixed support polygon that is independent of s . Note this equation is a complicate and high nonlinear equation. B that the polygon of support can be determined according The motion equation has the following form to the position of feet and by verifying if the feet are in contact with the ground. This determination can be M qt ¨qt C qt, ˙qt τ ( ) + ( )= t (46) done independently from sB, this is because the position vector of the joints Xt is an invariant quantity in the time where M (qt) is the mass matrix, C (qt, ˙qt) is the Coriolis parameterization algorithm. matrix, and includes gravity and other forces . At a glance, Eq. • The optimal solution obtained by solving the optimization (46) might appear to be simple; nevertheless the analytical problem of time parameterization is a local optimum. expressions for a simple six-axis industrial robotic arm are However, if we prove that the optimization problem is extremely complex. convex, then the obtained solution is the global optimum. In order to solve the optimization problem (45), the deriva- To this end, we should prove that the function J˜ of Eq. tive of τ with respect to s should be calculated. This st t (41) is a convex function. The functions J and ψ of Eq. derivative can be calculated as follows (41) are convex, because the first one is defined by the dτ ∂τ dq ∂τ dq˙ ∂τ dq¨ st = st st + st st + st st (47) user and it is supposed to be convex and the second dst ∂qst dst ∂q˙st dst ∂q¨st dst one is convex on account of the convexity of G(sB ). Consequently, the function J˜ is convex and the obtained Because of the path of the vector q is expressed as discreet st local optimum solution is the global optimum. points in the configuration space, and the time parameterization algorithm will not change the positions of these points dq IV. CASE STUDIES AND EXPERIMENTAL RESULTS in the configuration space, therefore st ≈ 0. The quantities dst dq˙ dq¨ The considered example is a collision-free reaching motion st and st can be calculated easily by using the finite dst dst in a cluttered environment. Fig. 2 shows snapshots of the difference approximation of Eq. (26). simulated motion. In the initial configuration, the robot is The main difficulty is the calculation of the quantities ∂τ ∂τ standing on its right foot and surrounded by a torus, a cylinder st and st . Although, in principle, these quantities can ∂q˙st ∂q¨st and a box. In this example, the task for the humanoid robot is be numerically approximated analogously by using the finite to move its left hand to a specified position, and at same time difference, we have observed that this approximation leads to keep the projection of its CoM inside of the support polygon to illconditioning, and poor convergence behavior. This is (statically stable motion). Producing such kind of motion is because of the high non-linearity of the motion equation (46). a very challenging task. This is because the environment is To overcome this problem, an analytical formulation can be very cluttered, and the support polygon is small (standing on ∂τs ∂τs derived of the quantities t and t by using the recursive the right foot). During the motion the humanoid robot should ∂q˙st ∂q¨st dynamic algorithm proposed in [19], [20], which is based on avoid the collision between its left leg and an obstacle which Lie groups and algebras. (for more details see [19], [20], [21], consists of a cylinder and a box, at the same time the left hand [22]). should avoid the collision with the torus to reach the desired ∂τ ∂τ Note that the quantities st and st are calculated only position. This collision-free path is created using an efficient ∂q˙st ∂q¨st one time and then they are used as constants in the time pa- method proposed by Kanehiro et al [14]. rameterization algorithm. Therefore the torque limit constraint Once the collision-free path is available, this path is con- can be reformulated as polynomial function with respect to st verted to a trajectory using a uniform time distribution func- analogously to the previous case. tion. SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 7

x y

Fig. 2. Snapshots of the simulated whole body reaching with collision avoidance

The sampling time ∆t has been chosen to be equal to MATALB language to solve the optimization problems. How- 5.0 × 10−3sec. The duration of the initial trajectory is equal ever, the number of iterations of the augmented Lagrange to 77sec. multiplier method gives a good idea of the computational time In this section, we will consider three scenarios: [23], [24], [17]. In fact, the augmented Lagrange multiplier method is a fast and reliable method because it does not require A. First Scenario: the inversion of matrices which have, in our case, very huge The time parameterization problem has the form of Eq. (37), dimensions. in which L (qsB , q˙sB , q¨sB )=0. In this scenario, the objective From Table I, it is clear that the duration of the obtained of time parameterization is to transform the initial path into a trajectory decrease by increasing the number of B-spline minimum time and dynamically stable trajectory. functions. However, the number of iterations to reach the The effect of the number of B-spline functions on the optimal solution increases while increasing the number of B- duration of the obtained trajectory and the required number of spline functions. iterations to reach the optimal solution is reported in Table I. The trajectories of ZMP (Px and Py) corresponding to basis TABLE I of B-spline functions of 20 and 120 functions are given in DURATION OF OBTAINED TRAJECTORIES AND NUMBER OF ITERATIONS Fig. 3. The directions x and y for the humanoid robot are drawn in Fig. 2. Number of B-spline Duration of Number of From Fig. 3, we observe that the trajectories of ZMP are obtained trajectory (sec) iterations always maintained inside of the polygon of support. However 10 18.35 4 20 14.81 9 the ﬂuctuations of ZMP occur more frequently when a basis 40 12.68 19 of 120 Bsplines is used, this is because the motion is faster 120 8.92 27 than the motion obtained by using a basis of 40 Bsplines. In Fig. 4, the initial, the reference and the executed tra- We have chosen to report the number of iteration instead jectories of the roll axis of right shoulder joint are given. of the computational time. This is because we are using The initial trajectory is directly obtained from the statically SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 8

Time (s) Time (s) (a) Using a basis of 40 Bsplines (b) Using a basis of 120 Bsplines

Fig. 3. First scenario: ZMP trajectories, and the safety zones for dynamical stability which are designed by the red lines. The time scale of the two ﬁgures is not the same because the time parameterization functions are different

!"' 20 Bsplines Initial 40 Bsplines !"&# 120 Bsplines

!"&

Rad

!"%#

t !"% Time (s) s

!"#& Executed !"$# Reference !"#&$ !"$

!"#%

Rad !"!# !"#%$ ! !"#$ ! $! %! &! '! #! (! )! " ' % ( ) *" Time (s) Time (s)

Fig. 5. First scenario (minimum time trajectory): Time parameterization Fig. 4. First scenario (minimum time trajectory): The initial trajectory of the function (st) roll axis of right shoulder joint which is obtained directly from the statically stable path (figure above). The reference and the executed trajectories of the real experiment on the humanoid robot HRP-2 (figure below). The time scale after time parameterization (figure below) is much smaller than that of the robot HRP2 are given in Fig. 9. initial trajectory (figure above)

B. Second Scenario: stable path by considering a uniform distribution of the time In this scenario, the time parameterization problem has parameterization function (st). The reference trajectory is the also the form of Eq. (37), in which L (qsB , q˙sB , q¨sB ) = the minimum time trajectory obtained by using a basis of 120 ...left hand B-spline functions. We can observe that the executed trajectory β Xt . The function L captures the jerk function of of the real experiment on the humanoid robot HRP-2 is exactly the left hand joint, this joint plays the role of end-effector in following the reference trajectory. The humanoid robot HRP- the computation of the initial path. The jerk function can be 2 is a position controlled robot, that means the trajectories of approximated as follows the humanoid robot’s joints are tracked using a high-gain PID ... X¨t − X¨ t−1 controller. Xt = (49) st∆t The time parameterization functions st which correspond to basis of B-spline functions of 20, 40 and 120 functions are where X¨t is expressed as a function of st as in Eq. (23). The given in Fig. 5. duration of the obtained trajectories and the required number Snapshots of the conducted motion applied to the humanoid of iteration to reach the optimal solution is reported in Table II. SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 9

TABLE II DURATION OF OBTAINED TRAJECTORIES AND NUMBER OF ITERATIONS C. Third Scenario: In this scenario, the time parameterization problem has β Duration of Number of the form of Eq. (45), in which L (qs , q˙s , q¨s )=0. The obtained trajectory (sec) iterations B B B 10−5 10.73 16 objective is also to find the minimum time trajectory and 10−4 14.13 31 dynamically stable. The difference between this scenario and 10−3 23.18 39 the first one is that the torque limits of the humanoid robot are taken into account in this scenario. A comparison between the time parameterization function st which is obtained in this scenario and that one of the first The time parameterization functions st which correspond scenario is presented in Fig. 7. to β = 10−4 and 10−3 and the two configurations which are related to high variation in the jerk function are given in Fig. 6. !"'# without torque limits The high variation of the jerk function in the first config- !"' uration occurs because the left hand stops near the torus in with torque limits order to avoid the collision, in the second configuration the !"&# left hand is inside the torus and it changes the direction of its !"& motion to avoid collision and at the same time to reach the desired position. !"%#

s !"% = 10 −3 % β !"$# β = 10 −4

!"$

$"# !"!#

! ! $! %! &! '! #! (! )!

t $ s Time (s)

Fig. 7. Third scenario: time parameterization function (st) !"# In order to obtain a safe motion which is not near the physical limits of the humanoid robot, we used a safety margin ! ! $! %! &! '! #! (! )! of 20 percent of the humanoid robot’s torque limits. Time (s) A comparison between the applied torque on the yaw axis of the hip joint with and without the consideration of torque limits is given in Fig. 8. This ﬁgure shows that the constraints on the torque limits have been successfully respected, on the other hand the duration of the obtained trajectory is more than that one of the ﬁrst scenario. This is in order to respect the torque limits.

V. CONCLUSION In this paper, we proposed a numerical method to solve the time parameterization problem of humanoid robot paths. The main contribution of this method is transforming a statically stable path into a minimum time and dynamically stable trajectory which respects the physical limits of the humanoid robot’s joints. We have shown that not only minimizing the trajectory time but also some energy criteria such as the jerk function can be considered. The initial statically stable path can be calculated using inverse kinematic methods or the motion planning methods. Fig. 6. Second scenario: time parameterization function (st) and the two This path, by deﬁnition, is a pure geometric description of the conﬁgurations (front and side views) which are related to high variation in motion. the jerk function The effectiveness of the proposed method has been validated using the humanoid robot HRP-2. SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 10

1 2 3

4 5 6

Fig. 9. Snapshots of the real experiment using the humanoid robot HRP-2

without torque limits execution through online information structuring in real-world with torque limits environment”. The ﬁrst author thanks the JSPS for a postdoc- "## toral research fellowship.

$# REFERENCES [1] L. Sciavicco and B. Siciliano, Modelling and Control of Robot Manip- ulators, 2nd, Ed. Springer-Verlag, 2000. # [2] ——, Modelling and Control of Robot Manipulators. Springer-Verlag, 2000, ch. Trajectory Planning, pp. 185–212. [3] K. Shin and N. McKay, “Minimum-time control of robotic manipula- Torque (N.m) tors with geometric path constraints,” IEEE Trans. Automatic Control, !$# vol. 30, no. 6, pp. 531–541, Jun 1985. [4] V. Rajan, “Minimum Time Trajectory Planning,” in IEEE International Conference on Robotics and Automation, vol. 2, 1985, pp. 759–764. !"## [5] J. E. Bobrow, S. Dubowsky, and J. S. Gibson, “Time-optimal control of robotic manipulators along specified paths,” International Journal of Robotics Research, vol. 4, no. 3, 1985. # % & ' ( "# [6] J. Slotine and H. Yang, “Improving the efficiency of time optimal path Time(s) following algorithms,” IEEE Trans. Automatic Control, vol. 5, no. 1, pp. 118–124, 1989. Fig. 8. Third scenario: applied torque on the yaw axis of the hip joint. [7] M. Renaud and J. Y. Fourquet, “Time-optimal motions of robot manipulators including dynamics,” The robotics review 2, MIT Press, pp. 225–259, 1992. [8] F. Lamiraux and J.-P. Laumond, “From Paths to Trajectories for Multi- ACKNOWLEDGMENT body Mobile Robots,” in The Fifth International Symposium on Experi- mental Robotics V. London, UK: Springer-Verlag, 1998, pp. 301–309. [9] K. Jiang, L. D. Senevirante, and S. E. Earles, “Time-optimal smooth- This research was partially supported by a Grant-in-Aid for path motion planning for a mobile robot with kinematic constraints,” Scientific Research from the Japan Society for the Promotion Robotica, vol. 15, no. 5, pp. 547–553, 1997. [10] M. Vukobratović and B. Borovac, “Zero-Moment Point—Thirty Five of Science (20-08816), and JST-CNRS Strategic Japanese- Years of its Life,” International Journal of Humanoid Robotics, vol. 1, French Cooperative Program “Robot motion planning and no. 1, pp. 157–173, 2004. SUBMITTED TO IEEE TRANS. ROBOT. , VOL. XX, NO. XX, 2010 11

[11] J. Kuffner, K. Nishiwaki, S. Kagami, M. Inaba, and H. Inoue, “Motion Eiichi Yoshida received M.E and Ph.D degrees Planning for Humanoid Robots Under Obstacle and Dynamic Balance on Precision Machinery Engineering from Graduate Constraints,” in IEEE International Conference on Robotics and Au- School of Engineering, the University of Tokyo in tomation (ICRA2001), 2001. 1993 and 1996 respectively. In 1996 he joined for- [12] J. Lenarcic and B. Roth, Advances in Robot Kinematics: Mechanisms mer Mechanical Engineering Laboratory, currently and Motion. Springer, 2006. National Institute of Advanced Industrial Science [13] F. Kanehiro, W. Suleiman, F. Lamiraux, E. Yoshida, and J.-P. Laumond, and Technology (AIST), Tsukuba, Japan. From 1990 “Integrating Dynamics into Motion Planning for Humanoid Robots,” in to 1991, he was visiting research associate at Swiss IEEE/RSJ International Conference on Intelligent RObots and Systems Federal Institute of Technology at Lausanne (EPFL). (IROS), Nice, France, September 2008. He served as Co-Director of AIST/IS-CNRS/ST2I [14] F. Kanehiro, F. Lamiraux, O. Kanoun, E. Yoshida, and J.-P. Laumond, Joint French-Japanese Robotics Laboratory (JRL) at “A Local Collision Avoidance Method for Non-strictly Convex Objects,” LAAS-CNRS, Toulouse, France, from 2004 to 2008. He is currently Co- in 2008 Robotics: Science and Systems Conference, Zurich, Switzerland, Director of CNRS-AIST JRL (Joint Robotics Laboratory), UMI3218/CRT, June 2008. AIST, Japan, since 2009. His research interests include robot task and motion [15] R. Rockafellar, “Augmented Lagrange Multiplier Functions and Duality planning, modular robotic systems, and humanoid robots. in Nonconvex Programming,” SIAM J.Control, vol. 12, no. 2, May 1974. [16] J. Lo and D. Metaxas, “Recursive Dynamics and Optimal Control Techniques for Human Motion Planning,” Computer Animation, pp. 220–234, 1999. [17] D. P. Bertsekas, Nonlinear Programming. Athena Scientific, 1995. Jean Paul Laumond (IEEE Fellow) is Directeur [18] R. Rockafellar, “Penalty Methods and Augmented Lagrangians in Non- de Recherche at LAAS-CNRS in Toulouse, France. linear Programming,” in Proc. 5th IFIP Conference on Optimization With his group Gepetto (www.laas.fr/gepetto) he is techniques, 1973. exploring the computational foundations of anthro- [19] F. C. Park, J. E. Bobrow, and S. R. Ploen, “A Lie Group Formulation of pomorphic motion. He has been a Coordinator for Robot Dynamics,” International Journal of Robotics Research, vol. 14, two European Esprit projects, PROMotion (1992– no. 6, pp. 1130–1135, 1995. 1995) and MOLOG (1999–2002), both dedicated [20] G. Sohl and J. E. Bobrow, “A Recursive Multibody Dynamics and to robot motion planning technology. During 2001– Sensitivity Algorithm for Branched Kinematics Chains,” Department of 2002, he created and managed Kineo CAM, a spin- Mechanical Engineering, University of California, Tech. Rep., 2000. off company from the LAAS-CNRS to develop and [21] W. Suleiman, E. Yoshida, J.-P. Laumond, and A. Monin, “On Humanoid market motion planning technology. From 2005 to Motion Optimization,” in IEEE-RAS 7th International Conference on 2008 he was a co-director of JRL, a French-Japanese CNRS-AIST laboratory Humanoid Robots, Pittsburgh, Pennsylvania, USA, December 2007. dedicated to humanoid robotics. He teaches robotics at the ENSTA and [22] W. Suleiman, E. Yoshida, F. Kanehiro, J.-P. Laumond, and A. Monin, Ecole Normale Superieure in Paris. He published more than 150 papers in “On Human Motion Imitation by Humanoid Robot,” in IEEE Interna- international journals and conferences in computer science, automatic control, tional Conference on Robotics and Automation (ICRA), May 2008. robotics and neurosciences. He is a member of the IEEE RAS AdCom. [23] R. T. Rockafellar, “Lagrange multipliers in optimization,” SIAM-AMS Proceedings, vol. 9, pp. 145–168, 1976. [24] ——, “Lagrange multipliers and optimality,” SIAM Review, vol. 35, no. 2, pp. 183–238, 1993. Andre´ Monin was born in Le Creusot, France in 1958. He graduated from the Ecole Nationale Supérieure d’Ingénieurs Electriciens de Grenoble in 1980. From 1981 to 1983, he was a teaching assis- tant in the Ecole Normale Supérieure de Marrakech, Morocco. Since 1985, he has been with the Labora- Wael Suleiman received his Master and PhD de- toire d’Automatique et d’Analyse des Systèmes of grees in Automatic Control from Paul Sabatier Uni- the Centre Nationale de la Recherche Scientifique versity in Toulouse, France, in 2004 and 2008 re- (LAAS-CNRS), France, as “Chargéde Recherche” spectively. He is now postdoctoral fellowship of the from 1989. After some work on non linear systems Japan Society for the Promotion of Science (JSPS) representation for his Doctoral thesis at Université at CNRS-AIST JRL, UMI3218/CRT, at National In- Paul Sabatier, Toulouse, France, in 1987, his interests moved to the area of stitute of Advanced Industrial Science and Technol- non linear filtering, systems realization and identification. He received the ogy (AIST), Tsukuba, Japan. His research interests Habilitation from the University Paul Sabatier at Toulouse in 2003.. include nonlinear system identification and control, numerical optimization and humanoid robots.

Fumio Kanehiro was born in Toyama, Japan, in 1971. He received the B.E., M.E., and Ph.D. degrees in 1994, 1996, and 1999, respectively, all from the University of Tokyo, Tokyo, Japan. He was a Re- search Fellow of the Japan Society for the Promotion of Science (JSPS) from 1999 to 2000 and a visiting research of LAAS-CNRS, Toulouse, France in 2007. He is now a member of the Intelligent Systems Research Institute, National Institute of Advanced Industrial Science and Technology Tsukuba, Japan. His research interests include humanoid robots and developmental software systems.