Cyrille François
email: cyrille.francois@enpc.fr

Jean-Pierre Nicolas

Natacha Gondran

Environment Assessment of people's daily mobility in an urban area: coupling a Land-Use and Transport Interaction model with Life Cycle Assessment

Introduction

Regarding the complexity of the urban system, the environmental assessment of its associated people's daily mobility needs to consider social, spatial and technological dimensions. Mobility is indeed greatly affected by both urban forms (e.g. density, accessibility, functional diversity, design …) and socio-economic characteristics of people (e.g. income, age, education, car ownership …) [START_REF] Ewing | Travel and the Built Environment: A Meta-Analysis[END_REF][START_REF] Stead | The relationships between urban form and travel patterns. An international review and evaluation[END_REF]. And associated environmental impacts are correlated with transport technologies performances including vehicles, fuels and infrastructures [START_REF] Chester | Environmental assessment of passenger transportation should include infrastructure and supply chains[END_REF]. Specific issues also arise from the territorial level, especially from the urban stakeholders who need more spatial results and information to make decisions, identify hotspots or optimize systems [START_REF] Loiseau | Territorial Life Cycle Assessment (LCA): What Exactly Is It about? A Proposal towards Using a Common Terminology and a Research Agenda[END_REF]. Then mobility needs to be contextualized for both technology and behaviors and environmental impacts should be disaggregated and spatialized.

To integrate these different parameters, the proposed method couples two tools developed in two distinct scientific fields -land-use planning and environmental sciences: with on the one hand, Land-Use and Transport Interaction models (LUTI) and, on the other hand, Life Cycle Assessment (LCA) methods.

Materials and methods

To account for both technological processes involved in daily mobility, as well as interactions among stakeholders and territories, the method uses the SIMBAD model developed on the Lyon urban area. This dynamic urban model represents daily mobility through feedback loops between land occupancy (residential, employment, commerce, leisure …) and transport performance (travel time and cost) and it provides travel flows on road and public transport networks. These flows are then considered as inputs for the LCA which includes all terrestrial and motorized transport modes of Lyon urban area (car, bus, train, subway and tramway) and their dedicated infrastructures. Most inventories are based on the EcoInvent database combined, for the use phase of road vehicles, to the Copert emission model. Spatial specifications where introduced on routes, vehicles fleets, technologies and usages [START_REF] François | Environmental Assessment of Urban Mobility: Combining Life Cycle Assessment with Land-Use and Transport Interaction Modelling-Application to Lyon (France)[END_REF].

Then assessed impacts are analysed at three different levels (urban area, spatial units or households) providing allowing multiple interpretations. However, by crossing emissions of vehicles use located at the link level with population dispersion an exposure indicator to local pollutants was developed to emphasis human health damage.

Results and discussion

Disaggregated analysis

The average GHG emissions for the Lyon urban area was estimated to 3.2 kg CO2-eq per inhabitant per day, dominated by exhaust emissions and fuel production (respectively 55% and 15%). Nevertheless, disaggregation by residential locations or by households shows a high heterogeneity on the territory for all environmental impacts, that strongly increase in periurban area. Figure 2 describes this territorial variance for GHG emissions. In depth analyses on various factors show the significant influence of social and spatial aspects on results through family size, employment rate, density or accessibility. For example, a member of a dual-career couple in low density location emits approximately 18 times more GHG that a single person unemployed in a dense location. However, technological factors contribute to environmental impacts by modifying indicators balances through depollution technologies, vehicles weight or motorization technologies

Local pollutants exposure indicator

The developed local NOx exposure indicator highlights the externality of compact urban form with higher health damage in city centre and it offsets the conclusions made on the emitters point of view.

Conclusions

To assess an object deeply anchored in a territory such as mobility, LCA methods need to be revisited with more spatial approaches. The proposed method on urban mobility presents some advantages to use urban model but also raises new questions such as territorial boundary, allocation system and comparability.

Figure 1 :

 1 Figure 1: Synthetic scheme of the coupling method SIMBAD-LCA.

Figure 2 :

 2 Figure 2: Distribution of GHG emissions levels depending on residential locations.


Fumio Kanehiro

Wael Suleiman

Florent Lamiraux

Eiichi Yoshida

Jean-Paul Laumond

Integrating Dynamics into Motion Planning for Humanoid Robots

This paper proposes an whole body motion planning method for humanoid robots in which dynamics is integrated. The method consists of two stages. A collision-free and statically stable path is planned in the first stage and it is transformed into a dynamically stable trajectory in the second stage. Contributions of the method is summarized as follows.

(1) A local method plans a C 1 path while avoiding collisions between non-strictly convex objects. (2) The second stage gives the minimum time trajectory by time parameterization under dynamic balance constraints. (3) Any path reshaping for recovering collision-freeness is not required since the second stage doesn't change shape of the path. Effectiveness of the method is examined by applying it to scenarios of a humanoid robot HRP-2.

I. INTRODUCTION

An whole body motion planning of a humanoid robot is a challenging problem mainly from the following reasons.

(1) Most of humanoid robots have more than 30 degrees of freedom. [START_REF] Kuffner | Motion Planning for Humanoid Robots Under Obstacle and Dynamic Balance Constraints[END_REF] The whole body motion must satisfy constraints, collision-freeness, physical capabilities and dynamic stability at the same time. Because of these reasons, it is difficult to solve the problem within reasonable time by applying existing motion planning techniques in straightforward way.

Several challenges have already been done on this topic. [START_REF] Sentis | A Whole-Body Control Framework for Humanoids Operating in Human Environments[END_REF] proposes a whole-body control framework which provides joint torques by projecting operational tasks into the constraint null-space. We prefer to generate motions in the joint velocity space. Because most of existing humanoid robots are position controlled and it's difficult to control them using joint torques from a practical point of view.

[2] proposes a motion planning method under obstacles and dynamic balance constraints. It consists of two stages. In the first stage, it finds a collision-free path by exploring a set of pre-computed statically stable postures. In the second stage the path is transformed into a dynamically stable trajectory by applying a dynamics filter. Since the dynamics filter modifies shape of the path, it possibly cause collisions. Therefore, the trajectory is confirmed to be collision-free and its duration is extended if collisions are found. [START_REF] Sanada | Passing Under Obstacles with Humanoid Robots[END_REF] also proposes a two stages approach for passing under obstacles. It plans a statically stable path in the first stage like [START_REF] Kuffner | Motion Planning for Humanoid Robots Under Obstacle and Dynamic Balance Constraints[END_REF] but the path is transformed into a dynamically stable trajectory without changing shape of the path.

The authors are with IS/AIST-ST2I/CNRS Joint Japanese-French Robotics Laboratory(JRL) F. Kanehiro and E. Yoshida are with Intelligent Systems Research Institute, National Institute of Advanced Industrial Science and Technology(AIST), Tsukuba Central 2, 1-1-1 Umezono, Tsukuba, Ibaraki, 305-8568 Japan {f-kanehiro, e.yoshida}@aist.go.jp W. Suleiman, F. Lamiraux and J.-P. Laumond are with LAAS-CNRS, university of Toulouse, 7 Avenue du Colonel Roche, 31077 Toulouse, France {suleiman, florent, jpl}@laas.fr Fig. 1. Whole body reaching with avoiding collisions and keeping stability [START_REF] Yoshida | Humanoid Motion Planning for Dynamic Tasks[END_REF], [START_REF] Yoshida | Humanoid Motion Planning using Multi-Level DOF Exploitation based on Randomized Method[END_REF] and [START_REF] Harada | Motion Planning for Walking Pattern Generation of Humanoid Robots[END_REF] propose methods that integrate motion planning techniques and a bipedal walking pattern generator. The method of [START_REF] Yoshida | Humanoid Motion Planning for Dynamic Tasks[END_REF] is applied to a bar carrying task. It plans a collision-free path at first by approximating a robot by a parallelepiped and then a trajectory is generated by the walking pattern generator. If collisions are caused by dynamic effects, the initial path is reshaped. Methods in [START_REF] Yoshida | Humanoid Motion Planning using Multi-Level DOF Exploitation based on Randomized Method[END_REF] and [START_REF] Harada | Motion Planning for Walking Pattern Generation of Humanoid Robots[END_REF] are applied to passing motion under obstacles. [START_REF] Yoshida | Humanoid Motion Planning using Multi-Level DOF Exploitation based on Randomized Method[END_REF] regards a robot as a height adjustable box and plans its position, orientation and height. Then the planned path is transformed into a trajectory by a walking pattern generator. [START_REF] Harada | Motion Planning for Walking Pattern Generation of Humanoid Robots[END_REF] generates a walking pattern at first with monitoring collisions and then colliding parts are modified while keeping the horizontal position of the center of mass.

Our strategy is also one of two stages strategies. A collision-free and statically stable path is planned in the first stage and it is transformed into a dynamically stable trajectory in the second stage. Contributions of the method are summarized as follows. (1) A local method plans a C 1 path while avoiding collisions between non-strictly convex objects. [START_REF] Kuffner | Motion Planning for Humanoid Robots Under Obstacle and Dynamic Balance Constraints[END_REF] The second stage gives the minimum time trajectory by time parameterization under dynamic balance constraints. (3) Any path reshaping for recovering collisionfreeness is not required since the second stage doesn't change shape of the path.

This work is a continuation of [START_REF] Kanehiro | A Local Collision Avoidance Method for Non-strictly Convex Objects[END_REF] and integrates dynamics into a local method in [START_REF] Kanehiro | A Local Collision Avoidance Method for Non-strictly Convex Objects[END_REF].

The paper is organized as follows. In section II, an outline of our method is introduced. In section III, a local method to get a collision-free and statically stable path is explained. In section IV, a timing planning method to transform the path into a minimum time and dynamically stable trajectory is presented. In section V, the effectiveness of the proposed method is confirmed by applying it to two scenarios for a humanoid robot HRP-2 [START_REF] Kaneko | Humanoid Robot HRP-2[END_REF]. In section VI, we summarize and conclude the paper.

II. TWO STAGES APPROACH

An whole body motion of a humanoid robot must satisfy the following constraints to be feasible for the real robot.

1) There is no self-collision and collision with the environment. 2) All joint angles stay within joint movable ranges.

3) All joint velocities don't exceed their limits. 4) ZMP stays inside of the support polygon. We propose a method which consists of two stages. The first stage plans a collision-free path and it is transformed into a dynamically stable trajectory in the second stage like [START_REF] Kuffner | Motion Planning for Humanoid Robots Under Obstacle and Dynamic Balance Constraints[END_REF]. Here, path is a series of configurations and trajectory associates time and these configurations. In that sense, path is a projected image of trajectory onto the configuration space.

If the transformation of the second stage modifies shape of the path, its collision-freeness might be broken. In the case, we need to go back to the first stage and iterate these two stages until a collision-free and dynamically stable trajectory is obtained. In order to prevent this iteration, we plan a statically stable path in the first stage and transform it into a dynamically stable trajectory without changing shape of the path by timing planning. By using a statically stable path as an input of timing planning, the second stage can always obtain a dynamically stable trajectory by slowing down [START_REF] Belousov | Motion planning for the large space manipulators with complicated dynamics[END_REF].

The timing planning problem is an old problem in robotic research. In the research works on manipulators, the main objective was to reduce the execution time of the tasks, thereby increasing the productivity. Most of these approaches is based on time-optimal control theory (see [START_REF] Renaud | Time-optimal Motions of Robot Manipulators Including Dynamics[END_REF] for an overview). In the framework of mobile robots, the timing planning problem arises also to transform a feasible path to a feasible trajectory [START_REF] Lamiraux | From Paths to Trajectories for Multibody Mobile Robots[END_REF]. The main objective, in this case, is to reach the goal position as fast as possible.

However, in our case the application of time optimal control theory is a difficult task. This is because not only the dynamic equation of the humanoid robot motion is very complex, but also applying time optimal control theory requires the calculation of the derivative of the configuration space vector of humanoid robot with respect to the parameterized path. Although such a calculation can be evaluated from differential geometry, it is a very difficult task in the case of high dimensional degree of freedoms and branched kinematic chains, which is the case of humanoid robot. For that, we propose to solve the timing planning problem numerically using finite difference approach as it will be explained later in the sequel.

In order to get a short trajectory by timing planning, the first stage plans a C 1 path. If the path is not C 1 at some points, the robot must stop at those points. Because if the robot doesn't stop, discontinuous velocities and infinite joint accelerations are applied to joints and it is impossible to keep dynamic stability. Therefore a C 1 path is preferable to prevent such a "stop-and-go" trajectory is generated. Requirements for a path planned in the first stage is summarized as follows.

1) There is no self-collision and collision with the environment. 2) All joint angles stay within joint movable ranges.

3) The path is composed of statically stable postures. 4) The path is C 1 . These constraints are not broken by timing planning in the second stage. The second stage transforms the path under the following constraints.

5) All joint velocities don't exceed their limits. 6) ZMP stays inside of the support polygon.

III. COLLISION-FREE AND STATICALLY STABLE PATH

A. Local Based Path Planning Method

In order to plan a collision-free and statically stable path, we use a local based path planning method proposed by Faverjon and Tournassoud [START_REF] Faverjon | A Local Based Approach for Path Planning of Manipulators With a High Number of Degrees of Freedom[END_REF]. The method produces the velocity of the robot under constraints. In the method, a nonconstrained initial velocity to achieve a task is computed and this initial guess is projected over the subspace of velocities satisfying linear equality and inequality constraints.

Let q denote the configuration and q the velocity of the robot. The projection is formulated as an optimization problem described as follows.

min q J τ (q) q -τ 2 subject to A q + b = 0, C q + d ≤ 0. (1)
where J τ (q) and τ denote Jacobian matrix of the task and the desired task velocity respectively. The path under constraints is generated by solving this problem repeatedly until a configuration which satisfies τ (q) = 0 is obtained.

B. Constraints 1) Collision Avoidance:

A linear inequality constraint to avoid collision is also proposed in [START_REF] Faverjon | A Local Based Approach for Path Planning of Manipulators With a High Number of Degrees of Freedom[END_REF]. It is called velocity damper. Let d be the distance between points, p 1 and p 2 on two objects, O 1 and O 2 (Fig. 2). When d is smaller than a threshold called influence distance and denoted by d i , velocity damper is activated for velocity of d:

ḋ ≥ -ξ d -d s d i -d s ( 2
)
where ξ is a positive coefficient for adjusting convergence speed, d s (< d i ) is a positive value called security distance. d is constrained not to be smaller than d s . ḋ is computed by the following equation.

ḋ = ( ṗ1 -ṗ2 |n)
where n is the unit vector (p 1p 2 )/d and notation (u|v) refers to the inner product of vectors u and v. The region where d is smaller than influence distance is called influence zone. velocity damper expresses that d must not decrease too fast when it is smaller than d i .

The velocity of p i (i = 1, 2) can be expressed as:

ṗi = J (q, p i ) q
where J (q, p i ) is a Jacobian matrix of O i at p i . Inequality (2) thus becomes a linear inequality constraint over the robot velocity q:

( q|J (q, p 1

) T n -J (q, p 2 ) T n) ≥ -ξ d -d s d i -d s (3)
If O 1 and O 2 are strictly convex objects, p 1 and p 2 are the closest points on them. In order to use non-strictly convex polyhedra as geometric models of the robot and the environment, several pairs of points must be selected to get continuous velocities as solutions of Problem [START_REF] Sentis | A Whole-Body Control Framework for Humanoids Operating in Human Environments[END_REF].

The interaction between polyhedra is decomposed into a set of interactions between faces. Polygonal faces are assumed to be decomposed into triangles 1 . The interaction between triangles is managed by the combinatorics between edges of the triangle and Voronoi regions of the other triangle. Let us recall the definition of Voronoi region. Definition: Voronoi region VR(X) for feature X. A Voronoi region associated with a feature X of a triangle is a set of points that are closer to X than any other feature.

Pairs of points are selected as follows. Case1 : The edge is in VR(F) Two pairs, (V i , V ′ i )(i = 1, 2) are constrained, where V i (i = 1, 2) are end points of the edge. (a, b) denotes a pair of points, a and b and V ′ i (i = 1, 2) denotes a projection of V i onto F along its normal vector.

Case2 : The edge is in VR(E) Three pairs, (V i , V ′ i )(i = 1, 2, 3) are constrained, where V ′ i (i = 1, 2) is a projected point of the end point V i onto E.
V 3 and V ′ 3 are the closest points between the edge and E. Case3 : The edge is in VR(V) Three pairs, (V i , V)(i = 1, 2, 3) are constrained, where V i (i = 1, 2) are end points of the edge. V 3 is the closest point between the edge and V.

For more details on continuous constraints generation between polyhedra, please refer [START_REF] Kanehiro | A Local Collision Avoidance Method for Non-strictly Convex Objects[END_REF].

2) Joint Angle Limits: Joint angles can stay within joint movable ranges by constraining those velocities using velocity damper. velocity damper for joint angles limits are defined as follows.

qmax i (q i ) ≥ qi ≥ qmin i (q i ), for i ∈ {1, ..., n}. (4) qmax i (q i ) =    ξ (q + i -q i ) -q s q i -q s if q + i -q i ≤ q i , q+ i otherwise (5) qmin i (q i ) =    -ξ (q i -q - i ) -q s q i -q s if q i -q - i ≤ q i , q- i otherwise (6)
where q + i and q - i are physical upper bound and lower bound of joint angle of i th joint respectively.

3) Horizontal Position of the Center of Mass: The robot is statically stable when a projected point of the center of mass(CoM for short) is in the support polygon. CoM is allowed to move as far as the projected point is in the polygon. This can be expressed as a set of inequality constraints. But in order to maximize margin for dynamic effect and minimize the duration of trajectory in the second stage, we constrain the horizontal position of CoM on the line which is orthogonal to the ground. This constraint is expressed as an equality constraint as follows.

J c q = ṗc (7)
where J c and ṗc are Jacobian matrix of CoM and a desired velocity of CoM respectively. ṗc is used to switch supporting legs.

IV. DYNAMICALLY STABLE TRAJECTORY

A. Dynamic Stability and ZMP

Dynamically stable trajectory is a trajectory for which the ZMP trajectory is always inside of the polygon of support (i,e, the convex hull of all points of contact between the support foot (feet) and the ground). Theoretically, any statically stable trajectory can be transformed into a dynamically stable one by slowing down the humanoid robot's motion. However, our objective is to find a minimum time and dynamically stable trajectory from a statically stable one. In order to obtain a motion within the humanoid robot capacities, the joint velocity limits of the humanoid robot should be taken into account.

Finally, the timing planning problem can be seen as an optimization problem under inequality constraints.

Let the ZMP on the horizontal ground be given by the following vector p = p x p y T

To compute p, one can use the following formula

p = N n × τ (f |n) (9)
where N is a constant matrix

N = 1 0 0 0 1 0 (10)
the vector n is the normal vector on the horizontal ground (n = z). The operator × refers to the cross product. Recall that (f |n) is the inner product of the vectors f and n.

The vector f is the result of the gravity and inertia forces

f = M g - n i=1 m i ci (11)
where g denotes the acceleration of the gravity (g = -gz), and M is the total mass of the humanoid robot. The quantities m i , ci are the mass of the i th link and the acceleration of its center of mass c i respectively. Finally, τ denotes the moment of the force f about the origin of the fixed world frame p O . The expression of τ is the following

τ = n i=1 m i c i × (g -ci ) -Lci (12)
where L ci is the angular momentum at the point

c i Lci = R i (I ci ωi -(I ci ω i ) × ω i ) (13)
R i is the rotation matrix associated to the i th link. I ci , ω i and ωi are its inertia matrix, angular velocity and angular acceleration respectively.

B. Timing planning formulation

Generally speaking, the timing planning problem of a function f (x t ), where t denotes time, consists into finding a real function S t in such a way f (x St ) verifies some temporal constraints, e.g,

h(S t ) ≤ f (x St ) ≤ l(S t ) (14)
Definition: The spatial velocity vector V t ∈ R 6 is defined as follows

V t =   v t ω t   =   dXt dt dθt dt   (15)
Definition: The spatial acceleration vector Vt ∈ R 6 is defined as follows

Vt =   a t ωt   =   dvt dt dωt dt   (16)
In order to obtain a causal motion, the function S t should be a strictly increasing function, that means dSt dt > 0. Therefore we will express S t as the integral of a strictly positive function s t > 0, as follows

S t = t τ =0 s τ dτ (17)
Let us rewrite the quantities in Eqs ( 15) and ( 16) with respect to the mapping function S t

V St =   dXt dSt dθt dSt   ≈   ∆Xt st ∆t ∆θt st ∆t   (18) VSt = d V St d S t ≈ 1 ∆S t V St -V St-1 ≈     ∆X t ∆t st-1- ∆X t-1 ∆t st s 2 t st-1∆t ∆θ t ∆t st-1- ∆θ t-1 ∆t st s 2 t st-1∆t     (19)

C. Minimum Time and Dynamically Stable Trajectory

Let us suppose that the first stage provides a path which consists of L points. At first, we transform this path to a trajectory by considering a uniform time distribution function. In other words, we suppose that s t = 1 : ∀t in (17). We denote T = L ∆t.

In our case, we would obtain a minimum time trajectory which is not only dynamically stable but also it should respect the joint velocity limits of the humanoid robot. Therefore, the optimization problem can be formulated as follow

min S T = min st T t=0 s t dt subject to s t > 0 p - st ≤ p st ≤ p + st q-≤ qst ≤ q+ (20)
where p st is the ZMP vector, p - st and p + st design the polygon of support for the humanoid robot. The vector qst denotes the joint velocity of the humanoid robot, and qand q+ design its upper and lower limits. Let us write p st as function of s t

p st = N n × τ ( f |n) (21)
where

τ = n i=1 m i X ci t × g -ci -Lci f = M g - n i=1 m i ci (22) in which ci = ∆X c i t ∆t s t-1 - ∆X c i t-1 ∆t s t s t 2 s t-1 ∆t Lci = R i I ci ωi -(I ci ωi ) × ωi ωi = ∆θ c i t ∆t s t-1 - ∆θ c i t-1 ∆t s t s t 2 s t-1 ∆t ωi = ∆θ ci t s t ∆t . (23)
In similar way we obtain

qst = ∆q t s t ∆t (24)
It is clear that the optimization problem ( 20) is polynomial in s t , so the gradient and Jacobian functions can be calculated easily.

D. Discretization of solution space

In real fact, the space of the admissible solutions of the minimization problem (20) is very large. In order to transform this space to a smaller dimensional space, we can use a basis of shape functions (e,g cubic B-spline functions). Let us consider a basis of shape functions B t that is defined as follows

B t = B 1 t B 2 t • • • B l t T (25)
where B i t denotes the value of shape function number i at the instant t, the dimension of B t is l defines the dimension of the basis of shape functions. The projection of s t into the basis of shape functions B t can be given by the following formula

s t = l i=1 s i B B i t = s T B B t (26)
Thus, the optimization problem (20) can be written as follows

min sB l k=1 s k B T t=0 B k t dt subject to s T B B t > 0 p - sB ≤ p sB ≤ p + sB q-≤ qsB ≤ q+ (27)
The optimization problem has been transformed into finding the vector s B ∈ R l , and, in its new form, it can be solved using C-FSQP [START_REF] Lawrence | Tits, User's Guide for CFSQP Version 2.5: A C Code for Solving (Large Scale) Constrained Nonlinear (Minimax) Optimization Problems, Generating Iterates Satisfying All Inequality Constraints[END_REF].

Note that the support polygon is a function of s B , and it depends on the horizontal position of CoM. However, as the path provided by the first stage is a statically stable one, the path can be split into various sections. Each section is a statically stable path which has a fixed support polygon and it is independent from s B .

E. Implementation Algorithm of Second Stage

The algorithm of the second stage can be summarized as follows 1) Given a path.

2) Transform the path to a trajectory by considering a uniform time distribution function. 3) Calculate ∆q t , ∆X ci t and ∆θ ci t from the obtained trajectory. 4) Choose ∆t, e,g, ∆t = 5.10 -3 [s]. 5) Calculate the cubic B-spline functions. 6) Split the path into verious sections depending on the place and shape of support polygon during the motion.

The support polygon for each section is fixed and independent from the time. 7) Solve the optimization problem (27) for each path with the initial solution obtained from the above steps.

V. SIMULATION

In this section, we confirm effectiveness of our strategy using two examples.

A. Toy World Scenario

The first example is a collision-free reaching motion in the cluttered environment. In this example, the task of the robot is to move its left hand to the specified position p h . The objective function of Problem ( 1) is given as follows. The reaching motion is obtained by solving Problem (1) until the hand reaches p g . Figure 4 shows the initial configuration and the final one. Figure 1 also shows the final configuration from another viewpoint. Red lines show pairs of points to be constrained. The robot is standing on its right leg with surrounded by a torus, a cylinder and a box in the initial configuration. The reaching target is placed at the other side of the torus. In the final configuration, the left arm and the head are avoiding collisions with the torus and the left leg and the right arm are with the cylinder and the box respectively.

J h q -ṗh 2 where ṗh = δp max p h -p h (q) p h -p h (q) (28) Inequality (3)
The computational time of the first stage is mainly decided by the number of constraints for collision avoidance and it depends on the number of pairs of triangles in influence zone. The computational time can be saved by reducing the number of pairs. The reduction can be done by the following two methods. The first one is to make influence zone thin by making the difference between d i and d s small. But it leads to big accelerations since the velocity of constrained points are decreased quickly. This is not preferable to obtain a short trajectory in the next stage. The second method is to simplify geometric models of the robot and the environment. If the simplified model contains the precise one in it, the path obtained using simplified model is also valid for the precise one.

Figure 5 shows a precise model and a simplified one of HRP-2. The precise one consists of 20858 triangles and the simplified one does of 1140 triangles. The computational times and the number of constraints are compared while combining these models and models of torus. The precise torus consists of 800 triangles and the simplified one does of 288 triangles. Table I shows averages of the computational time in [s] and the number of constraints (the number between parentheses) for one computational step. The computational time can be drastically reduced by using a simplified model.

Once the collision-free path is available, this path is converted to a trajectory using a uniform time distribution function. The obtained trajectory is then used to initialize Problem (27). Solving Problem (27) yields a minimum time and dynamically stable trajectory. The computation time of the second stage is obviously depends on the length of the path and the dimension of the basis of cubic Bspline functions. For this scenario, we have chosen: ∆t = 5.10 -3 [s], and a basis of 120 cubic B-spline functions. The function s t is given in Figure 6. The duration of minimum time dynamically stable trajectory is around 24[s] instead of 77[s] for the original uniform time distribution trajectory. That means the optimized trajectory is around 3 times faster than the original one. However it's not always the case, for example if the original trajectory is fast and dynamically unstable, then the optimized trajectory will be slower. Though the optimized trajectory is the minimum time trajectory of the dynamically stable trajectories. The trajectory of ZMP is given in Figure 7.

B. In a Room Scenario

The second example is on HRP-2 in its room. Figure 8 shows snapshots of this scenario. Each row shows snapshots along time-line and each column does from different viewpoints. In the first frame, HRP-2 is sleeping but there is no The scenario is split into three parts, (A) stretching, (B) stepping and (C) reaching to switch objective functions and constraints in Problem [START_REF] Sentis | A Whole-Body Control Framework for Humanoids Operating in Human Environments[END_REF]. In part (A) the task is to going to a goal configuration q and the objective function is defined as follows.

qq 2 where q = δq max qq max i=1,••• ,n |q i -q i | The part (B) is split into two parts again, (B-1) moving CoM and (B-2) moving foot. Multiple steps can be done by repeating these two parts. Since the first stage must generate a statically stable path, a resultant walking is a static one. In part (B-1), ṗc is computed in the same way with Eq.(28). Paths for the part (B-2) and (C) are generated in the same way with the previous example. Goal configurations and points of feet and the hand are selected by hand.

VI. CONCLUSIONS AND FUTURE WORKS

In this paper, we proposed a collision-free and dynamically stable whole body motion planning method. The method and its contributions are summarized as follows.

• The method consists of two stages. A collision-free statically stable path is planned in the first stage. The path is transformed into a minimum time and dynamically stable trajectory in the second stage. • A local method in the first stage can generate a differentiable path while avoiding collisions between nonstrictly convex objects. • Since the first stage plans a statically stable path and the second stage doesn't change shape of the path, the trajectory can be always obtained without iterating two stages. Since the path is generated using a local method, the planner might be trapped by local minima. We are woking on integration with a global method like PRM [START_REF] Kavraki | Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces[END_REF] or RRT [START_REF] Lavalle | Rapidly-exploring random trees: A new tool for path planning[END_REF].

The followings are limitations of our strategy and future works.

• Since the first stage of the method must plan a statically stable path, this method can't be applied to faster motions where CoM goes out of the support polygon like a biped walking. • The robot might slip or jump. Because forces and the moment around the vertical axis are not constrained.

Fig. 2 .

 2 Fig. 2. Collision avoidance: Faverjon and Tournassoud's method

Fig. 3 .

 3 Fig. 3. Constraints generated between an edge and a triangle

,( 4 )

 4 and Equality[START_REF] Kanehiro | A Local Collision Avoidance Method for Non-strictly Convex Objects[END_REF] are used as constraints over joint velocities. ξ, d i and d s of Inequality (3) are set to 0.5[m/s], 0.05[m] and 0.03[m] respectively. Those of Inequality (4) are set to 0.5[rad/s], 0.2[rad] and 0.05[rad] respectively. In this example, since the robot is standing on the right leg from the beginning, ṗc is set to 0.

Fig. 4 .

 4 Fig. 4. Initial and final configurations of whole body reaching: In the final configuration, the left arm and the head are avoiding collisions with the torus and the left leg and the right arm are with the cylinder and the box respectively.

Fig. 5 .Fig. 6 .Fig. 7 .

 567 Fig. 5. Precise model(left) and simplified model(right)TABLE I AVERAGES OF THE COMPUTATIONAL TIME[S] AND THE NUMBER OF CONSTRAINTS

Fig. 8 .

 8 Fig. 8. Snapshots of in a room scenario

  contacts among HRP-2, the chair and the table. We inserted gaps which are bigger than d s since constraints for collision avoidance don't work correctly if distances are smaller than d s . And then it stands up(b), gets out of a gap between the table and the chair(c-e) and tries to grasp something on the shelf(f).

In the following, features of a triangle are the triangular (open)face, the three edges and the three vertices.

VII. ACKNOWLEDGMENTS

We gratefully acknowledge AEM Design, developer of a powerful QP solver, C-FSQP [13].