RECENT progress in motion planning research now makes it possible to compute complex motions that avoid collisions for complex robots in cluttered 3-D environments. This development can be attributed to efficient algorithms based on sampling-based planning methods [1], [2], [3]. In industry, such techniques are used to plan the assembly or disassembly sequence of products composed of many parts with complicated geometry [4].

Those motion planning techniques are usually based on geometric reasoning in configuration space. We believe that the advances in this research field have now arrived at a stage where those techniques should be applied to dynamic motion planning for robots with many DOFs.

Typical applications that require dynamic motion planning include nonlinear space manipulators and humanoid robots that require dynamic balance during task execution and locomotion.

The difficulty of planning motions for those robots lies in the high complexity of geometric and kinematic planning of 3-D collision-free motion in complex environments while accounting for dynamic constraints. Due to this complexity, there have been few planning systems that can deal with such dynamic robotic systems.

In this paper, we propose a general and practical planning framework that integrates a geometric path planner and a dynamic motion generator. It is based on an iterative two-stage motion planning method, by exploiting the efficiency of sampling-based planning and advanced dynamic motion generation. Within the classification proposed in [5], our two-stage approach fits alongside state-space and sensor-based approaches. In the first stage, a “path” is generated by geometric and kinematic planning, which is transformed into a dynamically executable “trajectory” through appropriate dynamic motion generators in the second stage. Due to dynamic effects, the path supporting the new trajectory may differ slightly from the initial path. Then, the output trajectory is again verified with respect to the collision avoidance by the first stage and reshaped if necessary. This process is iterated until a valid dynamic trajectory is obtained.

Although the necessity of reshaping for dynamic collision-free motion has been recognized [6], it has not been systematically addressed. The proposed general framework recaptures the two-stage method with a global view by introducing two types of iterative reshaping methods: temporal and spatial. The former resolves collisions by reducing the motion velocity along the generated trajectory, whereas the latter deforms the trajectory itself.

In this paper, we emphasize the practical aspect of our approach through realistic simulations and experiments. To our knowledge, there are currently few motion planners that can generate 3-D collision-free dynamic motions including locomotion and manipulation by humanoids with as much geometric and dynamic complexity as we deal with here.

The paper is organized as follows. Section IIprovides related work and states the main contribution. The proposed iterative two-stage framework is outlined in Section III. Geometric and kinematic path planning utilized in the first stage is briefly presented in Section IV. Then, the trajectory reshaping methods used in the second stage, the temporal and spatial approaches are presented in Sections V and VI, respectively, together with simulation and experimental results. After discussing the limitations and future developments of the proposed method in Section VII, conclusions are given in Section VIII.

SECTION II

## Related Work and Contribution

The two-stage approach has also been studied as “decoupled planning,” in which a collision-free path is computed first and a time-scaling function is next derived to define the execution velocity. In such an approach, the problem is stated as finding a time-optimal scaling function, subject to the actuator limits, which allows the robot to precisely follow the path provided from the first stage [7].

The state-space method has been addressed as “kinodynamic motion planning” that includes the derivatives in the system state spaces [8], [9]. In this method, the next state of the system is estimated by taking into account its dynamics to find also time-optimal solutions. Although the dynamic motion is directly planned, the size of search space increases exponentially as the number of the states increases. It is also known that finding exact kinodynamic solutions is intractable even for 2-D problems [10].

For far more complicated robots such as space manipulators or humanoids, it is now reasonable to employ efficient algorithms based on sampling-based planning methods such as Rapidly-exploring Random Trees (RRT) [1], [2], Probabilistic RoadMap(PRM) [3], and all their variants (see [11] and [12] for recent overviews). The sampling methods compute a graph called a roadmap whose nodes are collision-free configurations chosen at random and whose edges model the existence of collision-free local paths between two nodes. One extension for sampling-based methods adjusts the obstacle size for easier planning [13], [14]. An iterative method has been proposed based on the automatic tuning of obstacle penetration tolerance [14]. In this method, the thinned obstacles are used to make the planning easier. Then, they are grown gradually by increasing the tolerance to deform the path so that the robot stays away from them. We will utilize this method in our planning scheme.

Spatial reshaping has been studied in various contexts. In robotics, a general reshaping method has been introduced in [15]. The idea is to perturb the input functions of the control system along the path to be modified, in such a way as to decrease an optimization criterion. However, this method requires the analytical expression of the control model, which is not the case for the humanoid robot we deal with. The concept of elastic band [16], [17], [18] is a more practical approach. An energy function is applied along the path, and an optimization algorithm allows path reshaping in order to minimize its cost. The energy function is derived from a distance computation between the moving body and the obstacles. Distance computation is a costly operation that often requires simplified geometry for a good performance.

Our spatial reshaping method is inspired by a technique for key frame editing in the context of computer animation [19]. This approach places more emphasis on the gradual transition from the colliding trajectory by maintaining the motion timing in constrained environments. Here, the complexity is determined by the resolution induced by the key framing, and less influenced by the geometric complexity.

One typical application of the proposed framework is space manipulation systems [20], [21]. Space manipulators have long arms in order to transport high payload and are usually teleoperated. Although path-planning methods have been proposed for space manipulators [22], it is not practical to analytically plan the trajectory of such a highly nonlinear system in a cluttered environment. In this research, we take advantage of capacity of a sampling-based planner by combining it with a dynamic simulator in a two-stage planning scheme. So far, automatic planning of 3-D collision-free motions in cluttered environments for such a highly nonlinear system has not been achieved.

Another application is planning of dynamic humanoid motions, which is an even more complex problem where we must cope with more than20 DOF as well as dynamics. Kuffner *et al.* made the initial progress in this area and have been actively working on this topic. They proposed a method to generate collision-free dynamically stable humanoid motion using RRT [6]through a two-stage method. The random planner makes a search to find a collision-free path using precomputed statically stable configurations. The path is then transformed into dynamic trajectory by verifying dynamic zero moment point (ZMP)constraints. In case of collision, the trajectory is slowed down, which corresponds to temporal reshaping in our research. In this research, support state (double or single support) does not change and locomotion is not included. Khatib and his colleagues have been working on dynamic whole-body motion generation for humanoid robots by using task specification in operational space [23]. Their research on local reactive motions is being extended to be combined with global planner of mobile manipulators [24].

Several other studies have been conducted on whole-body motion planning for humanoids for specific tasks [25], [26], [27] and on geometrical and kinematic planning digital actor animation [28].However, 3-D collision-free motion planning including simultaneous locomotion and manipulation tasks by humanoids has not yet been accomplished. Although simpler versions of spatial reshaping for humanoids have been reported in our previous work [29], [30], it has not been discussed in a global framework. Moreover, this paper provides a more detailed reshaping algorithm, an experiment in a more complex environment with extended analysis of the results.

The main contribution of this paper is to provide a general framework for motion planning including dynamics for robots with many DOFs to generate 3-D collision-free motions in complex environments, based on an iterative two-stage planning scheme.

As an additional contribution, we show practical applications of the proposed framework for two different types of robots: space manipulators and humanoids. These two robots correspond to typical examples of temporal and spatial reshaping whose scope is introduced in the next section.

SECTION III

## General Framework of Dynamic Collision-Free Motion Planning in 3-D

The proposed general framework of dynamic collision-free motion planning based on the two-stage planning method is illustrated in Fig. 1. It outputs dynamic collision-free trajectories from the inputs of initial and goal configurations together with the geometry information of the environment. The resulting dynamic trajectories should be ready to be given to low-level robot controllers.

The path planner finds a geometric and kinematic collision-free path in 3-D at the first stage (upper part of Fig. 1). Any available planning method can be used for this part. Then, in the second stage, the dynamic motion generator to transform the given path into dynamically executable robot trajectory (lower part in Fig. 1). A dedicated dynamic controller can be put depending on the application. The path planner will be detailed in Section IV.

The generated trajectory may deviate from the planned path due to robot dynamics, which may cause unpredicted collision with obstacles. The reshaper is placed in the first stage as a mechanism that interacts with the dynamic motion generator iteratively to remove those local collisions. This corresponds to the inner iteration loop in Fig. 1.

We introduce two temporal and spatial reshaping methods as summarized in Fig. 2. Iterative application and different types of reshaping have not been explicitly addressed in previous work of two-stage planning.

Temporal reshaping is described in Fig. 3.If collisions are detected in the trajectory, reshaping is applied to the colliding portion of trajectory by slowing down the control inputs. This procedure is repeated several times until a collision-free dynamic motion is obtained.

Spatial reshaping locally deforms the portions of the path whose corresponding dynamic motion is colliding (see Fig. 4). If the dynamically colliding local paths become blocked by the “grown” obstacles, the replanning process is activated, as shown in the bottom left of Fig. 2. In the replanning process, the path planner searches for another path that avoids the blocked passage. This corresponds to the outer loop in Fig. 1.

Temporal reshaping makes the planning framework converge once a statically stable motion is found. However, this condition does not guarantee completeness for planning of dynamic motions. There can exist dynamic motions that cannot be executed statically. Moreover, temporal reshaping may create unnecessarily slow motions. The practical interest of the spatial reshaping is to overcome this drawback, though it does not cover all possible dynamic motions.

Since our framework allows the integration of existing path planning and control techniques, we can choose among the variety of existing algorithms for those that apply best to the problem at hand. The proposed method is offline on the assumption that the environment is completely known.

Two applications of the proposed method, for space manipulators and humanoid robots, are shown in Sections V and VI to demonstrate the validity of the method.

SECTION IV

## Planning Geometric and Kinematic Path

For the geometric and kinematic motion planner needed by the first stage, we have adopted an automatic parameter-tuning algorithm [14] that is appropriate for highly constrained spaces where the motion to be computed is close to the contact space.

The algorithm is characterized by iterative refinement of the path and automatic parameter tuning, as illustrated in Fig. 5. In this algorithm, the first path is computed allowing some penetration in the obstacles [see Fig. 5(a) and (c)]. Then, the current paths are iteratively replanned by decreasing the allowed penetration threshold. The cases of failure of the iterative process are automatically detected and solved. Concretely, when a portion of path cannot be locally modified into a collision-free one, then the search starts again to find a roadmap composed of the portions of the roadmaps that are collision-free [see Fig. 5(b) and (d)]. The method is parameter free in the sense that the penetration threshold is automatically tuned through the observation of the distances during the search. The steering directions are also automatically controlled by shooting random configurations in function of the already constructed roadmap. This algorithm has been verified to be efficient in constrained environments [14].

SECTION V

## Temporal Reshaping: Space Manipulator

The proposed method is applied to an on-board space manipulator equipped in Russian space shuttle “Buran.” Temporal reshaping is adopted to make full use of its property to converge to the planned collision-free path in the narrow passages. Spatial reshaping is not suitable because of the small clearance.

### A. System Description

The manipulator is 15.5 m long and has 6 DOF. It has been developed as the virtual robotic test-bed (VRT), in accordance with project “Servicing” of Russian Space Agency. A dynamic simulator dedicated to space manipulator has been developed in this framework [31].

Fig. 6 shows a 3-D collision-free path planned in the first stage to dock the satellite by the Buran on-board manipulator inside the Buran cargo bay. The output of the path planner is a finite sequence of waypoints, connected by linear interpolation in the configuration space.

The manipulator base can be regarded as fixed since the large difference in inertia renders the movement of the base negligible. However, in contrast to industrial robot arms, the dynamic behavior of the Buran on-board manipulator is extremely complex due to elasticity of its joints, large mass and inertia of its links, and the presence of nonlinear elements in mechanical gears. In particular, the following dynamic features have been revealed during investigation of the dynamic model of the Buran manipulator and experiments with real robot on the air-bearing test bed:

oscillation with large amplitude (up to 0.5 m) and small frequency (0.2–1 [Hz])

large accelerations and decelerations along path, resulting from the important inertia of the manipulator links.

Fig. 7 illustrates the collision detected after applying the dynamic motion generator in the second stage. To prevent collisions, we introduce a temporal reshaping of the path.

### B. Temporal Reshaping

Let us consider a geometrically and kinematically planned end-effecting gripper path. The path is discretized into“control points” *A*_{i} according to several criteria to determine the commands of velocity control at each point, as illustrated in Fig. 8. The velocity commands are validated using a simulator [31] that computes the dynamic motion of the manipulator.

The number of control points depends on the desired maximal linear and angular velocities for the end effector *v*_{max} and ω_{max}. Let the robot control period be *t*_{contr} (defines the frequency of the control inputs change at the high level of the robot control system). Then, the approximate number of necessary control points is *N* = *T*/*t*_{contr} as follows:
TeX Source
$$T=\max\bigg({{L_{p}}\over {v_{{\rm max}}}},{{\phi_{p}}\over {\omega_{{\rm max}}}}\bigg)\eqno{\hbox{(1)}}$$where *L*_{p} and φ_{p} represent the length of the planned path and difference of orientation angle.

Parameters *v*_{max} and ω_{max} define how fast the robot will follow the trajectory. Using these parameters, we can define the profile for linear *v*_{i} and angular *ω*_{i} velocities of the end effector along the trajectory:
TeX Source
$$\eqalignno{{\bm v}_i &= {{d_i}\over { d_{{\rm imax}}}}v_{{\rm max}}{{A_{i}A_{i+1}}\over {\vert \vert A_{i}A_{i+1}\vert \vert }} \cr{\bm \omega}_i &= {{d_i}\over {d_{{\rm imax}}}} \omega_{{\rm max}} {\bm n}&{\hbox{(2)}}}$$where *d*_{i} and *d*_{imax} are the distance between the robot and the closest obstacle at point *A*_{i} and its maximum value *d*_{imax} = max (*d*_{i}), *n* is the vector of rotation between two end effector orientations in points *A*_{i} and *A*_{i+1}. Such a choice of *v*_{i} *ω*_{i} allows for accelerated motion when the robot is far away enough from the obstacles (i.e., the coefficient *d*_{i}/*d*_{imax} is about 1), and decelerated motion when the clearance *d*_{i} is small (i.e., the coefficient *d*_{i}/*d*_{imax} is correspondingly less than 1).

Then, for each point on the trajectory, we calculate desired values for joint velocities, multiplying the vector by the inverse Jacobian matrix of the manipulator. If those velocities are derived, a proportional–derivative (PD) control law can be applied to determine the command for the manipulator.

Therefore, the approximation problem here is simply choosing admissible velocities to keep collision-free motion and to minimize overall time for trajectory following. We have used a dichotomy method to cope with that problem. This means that we first try maximum values of linear and angular velocities, which can be realized physically by the robot. If these values lead to collisions during dynamics simulation, we divide them by 2, try new values, and so on.

Note that in practice, we do not demand the end effector to arrive to the proximity of the current control point, such as in the aforementioned algorithm—regardless the end effector will achieve the control point or not, the control system switch it to the next one with fixed frequency 1/*t*_{contr}.

### C. Simulation Results

The resulting dynamic trajectory for the task of docking the satellite inside the Buran cargo bay is presented in Fig. 9. The payload for this task is 3000 kg.

The number of control points necessary to guarantee collision avoidance is 26. The algorithm provides the dynamic solution for the extremely tight space—the range of minimal distances between the robot with satellite and workspace varied from just0.02−0.03 m to 0.18 m for a 15.5-m-long manipulator with a4-m-long satellite. Trajectory length is 18.20 m and total time to perform the operation is 98.09 s. The average end effector linear speed is 0.186 m/s. This value exceeds maximum speed for Buran manipulator (0.1 m/s) with the maximum payload (50 000 kg) and just a little below its maximum possible speed without payload, equal to 0.3 m/s.

This result demonstrates the benefit of the proposed reshaping method since a very high velocity of the end effector becomes possible. Conventionally, the end-effector speed has been limited to 0.01 m/s because motions with higher speed without such planning may cause oscillations and collisions due to highly nonlinear dynamic behavior of the manipulator.

On a 1.2 GHz PowerPC G4, the computation time for kinematic path is0.67 s and the dynamic motion generator takes 0.003 s to generate the dynamic trajectory.

SECTION VI

## Spatial Reshaping: Dynamic Task By Humanoid

This section deals with an application of the proposed framework to an object transportation task by a humanoid robot. The humanoid robot must manipulate an object, avoid obstacles, and keep its balance while walking. We employ spatial reshaping to obtain high-quality motions through efficient dynamic motion generators for humanoid locomotion (e.g., [32]).Usage of temporal reshaping on quasi-static paths would make the robot motions unnecessarily slow.

### A. Whole-Body Motion Generation

We utilize here a functional decomposition of the robot body that has already been applied to motion planning for virtual mannequins [28]. At this level, the robot is modeled as a geometric parallelepiped bounding box (see Fig. 10). Only that box and the object to be manipulated are considered with respect to collision avoidance. The robot motion is expressed by the planar position and orientation of its waist *r*(*x*, *y*, θ) (3 DOF) and the object motion by its position and orientation *R*_{o}(*x*_{o},*Θ*_{o}) (6 DOF) with respect to a global coordinate system Σ_{0}. The configuration space to be searched is then nine-dimensioned. To explore such a space, we use a sampling approach as in [28].

In our case, the robot path is planned as Dubins curves composed of line segments and arcs of a circle [33]. Given the configuration of the robot waist and object, the joint angles(*q*_{u}) of the upper-body motion are derived by using inverse kinematics (IK) described later.

Then, at the second stage, the planned motions *r* and*q*_{u} are given to the dynamic pattern generator [32] of humanoid robots to transform the input planar path into a dynamically executable motion. The walking pattern generator is based on preview control of ZMP proposed by Kajita *et al.* [32]. The reference ZMP trajectory is derived from the foot placements obtained from the planned planer robot path. Based on preview control of ZMP position for an invert pendulum model, this method is able to generate dynamically stable biped walking motion that always maintains the ZMP inside the support polygon formed by the foot(feet).

Moreover, the pattern generator accepts upper-body motion*q*_{u} as auxiliary input to compute the mixed whole-body motion for desired upper-body movement and walking motion in the following manner. First, the pattern generator estimates the difference of the resulting ZMP from the reference when applying the desired upper-body trajectory for manipulation motion. This disturbance on the ZMP may perturb the robot stability. Then, the biped locomotion trajectory is slightly modified by shifting the waist position to compensate this difference to maintain the ZMP inside the support polygon. The dynamically stable motion is thus obtained that performs the manipulation and locomotion at the same time [32].

As a result, the whole-body humanoid motion is computed as the 6DOF waist position and orientation *R*_{d}(*X*_{d},*Θ*_{d}) and joint angles of whole body (*q*) at sampling time of 5 ms by taking dynamic balance based on ZMP into account.

### B. Smooth Spatial Reshaping

Given the list of whole-body configurations at each sampling time as input, each configuration is tested for collision. For each colliding portion of the trajectory, the value of minimum tolerance from the obstacle is augmented and configurations are rechecked for collisions. If collisions are found within the lower part of the robot, the original path is modified with the new tolerance value to generate dynamic motion again.

If collisions are found within the upper part of the body, the following reshaping procedure is applied, as illustrated in Fig. 11. This process includes two essential parts: smooth motion reshaping and IK calculation for the smoothed path.

*Smooth Motion Reshaping*

The output of a dynamic simulation of the planned trajectory is a sequence of robot's configurations uniformly sampled at 5 ms intervals, which is the control sampling rate of the robot. With this output, the colliding portions of his trajectory can be reshaped using motion-editing techniques that enforce spatial and temporal constraints usually employed in computer animation [19]. Our reshaping method is inspired on the motion-editing step described in [28].

After identifying the endpoints *u*_{1} and *u*_{2} of each colliding portion as in Fig. 11(a), a collision-free configuration is found with the augmented tolerance by randomly sampling in the task space (the object configuration) within a nearby range of the middle point of the portion, as shown in Fig. 11(b). The new collision-free configuration should also be within the reachable area of the arms. All the configurations within the portion are then replaced by this new collision-free configuration.

Even though collisions are eliminated at this stage, discontinuities in velocity and acceleration may occur at *u*_{1}and *u*_{2}. In order to smoothly anticipate the new configuration and regain the original trajectory, the reshaping portion is extended by adding the number of samplesns_{before} and ns_{after}. The anticipation and regaining times are computed to perform the motion between two configurations (*q*(*u*_{1} −ns_{before}), *q*(*u*_{1})) for anticipation, (*q*(*u*_{1}),*q*(*u*_{1} +ns_{after})) for regaining using the joint angular velocity and *v*_{ref} of the object in workspace as reference [see Fig. 11(c)].

Finally, in order to obtain smooth motion for the end effector, configurations of the carried object are interpolated between*R*_{o}(*u*_{1}−ns_{before}) and*R*_{o}(*u*_{2}+ns_{after}). We use well-known interpolation techniques such as cubic spline. In order to respect time constraints, the time at which each of the specified configurations should be reached is specified along with position when fitting the curve [34]. Finally, the curve is resampled to get the new object's configuration at each 5ms interval [see Fig. 11(d)].

### C. Simulation and Experimental Results

We have conducted simulations and experiments of the proposed humanoid motion planner using simulator OpenHRP [38]and hardware humanoid platform HRP-2 [39]. HRP-2 has30 DOF, is 1.54 m tall, and weighs 58 kg. This robot has two chest joints for pitch and yaw rotation, which extends the motion capability to include lying down on the floor and standing up. It can carry load up to 2 kg at each hands.

For the following experiment, we took an example task of carrying a bar in an environment populated by obstacles. The length, diameter, and weight of the bar are 2.0 m, 24 mm, and 1.2 kg, respectively, with two disks whose diameter and thickness 0.4 m are 40 mm at 0.1 m from each extremity. The reference velocity*v*_{ref} in workspace is 0.3 m/s and 30°/s for translation and rotation, and reference joint angles are defined from the hardware specification.

A 3-D collision-free whole-body motion of the humanoid robot is generated based on the proposed planning method in an environment of a flat plane with obstacles. Fig. 12 shows the top view of the environment with the initial and goal position and orientation (*x*, *y*,θ) on the plane, and planned walking path composed of line segments and arcs (dotted line) through the narrow passage. Two high poles are set with the distance of1.75 m, which is shorter than the bar length 2 m. The robot holds the bar at the height of 0.95 m at the initial configuration. Since the robot cannot tilt the bar completely perpendicular to the ground, the planning and reshaping explore all the 6 DOF of the object in order to pass between the poles and underneath the table whose height is 1.1 m. The environment is therefore an example of cluttered one that imposes 3-D collision avoidance to verify the effectiveness of the proposed method.

Fig. 13 shows the planned results of 3-D collision-free dynamic motion. Fig. 13(a) and (b) shows the initial and goal configurations. Since the distance between the two lamps is shorter than the bar length, the bar should pass through with an angle. At the beginning of the motion, the computed trajectory for the bar makes the robot move to the left, then walk forward with a certain angle to path through the gap [see Fig. 13(c)– (e)]. The motion of the upper part of the robot is computed using a generalized IK algorithm based on (4) and (6). The position and orientation of each hand is dealt with as a single task. We can observe that the motions of two chest joints are involved in completing both tasks.

This example also shows that the complete geometry of the object is exploited in the collision-detection and path-planning procedure and that no bounding box has been used [see Fig. 13(f)] where the concave part of the disk and the bar is close to the lamp. At the end of the motion [see Fig. 13(g)], the tall table is avoided. The complete trajectory execution time is around 28.8 s. The simulation has been done with KineoWorks and OpenHRP dynamic simulator. Fig. 14 shows the real execution of the robotic platform HRP-2 #14 at the Laboratoire d'Architecture et d'Analyse des Systèmes, Centre National de la Recherche Scientifique (LAAS–CNRS).

The computation time of kinematic path in the first stage is3.21 s. Then, dynamic motion generation for locomotion taking28.8 s in the second stage takes 8 s. The time to detect and to identify collision portions to find a new configuration and to interpolate in workspace is 0.76 s. Finally, the computation of IK takes 145 s (5609 configurations) with precision of 1 mm.

The reshaping takes place three times to remove the collisions, with tolerances of 2, 10, 50 mm. With small tolerance values, collisions are detected since deviation from the planned trajectory occurs due to the dynamic motion. The tolerance was therefore increased twice to eliminate collisions.

Fig. 15 shows the minimum distances between the robot including carried object and the obstacles for those three iterations, where the distance 0 means that there are collisions. The transition of robot configurations through the three iterations can be well observed in Fig. 16. At the first iteration, there are collisions with all obstacles. The collision with obstacle 1 was resolved at the second iteration, but collisions with obstacle 2still remains. Finally, all the collisions are removed at the third iteration.

Although the minimum distance sometimes becomes smaller than the desired tolerance 50 mm due to dynamic motion, a collision-free trajectory has been derived.

SECTION VII

## Discussions: Limitation and Future Developments

In this section, several discussion points are raised that mainly concern humanoids.

### A. Box Approximation in Path Planning

The bounding box in Fig. 10 is set so that it gives a good approximation of the swept volume during walking. Its depth is derived from the knee-bended walking posture and upper body, and itswidth considers the lateral sway during walking without arms. The box is therefore a sufficiently accurate indicator to know if the robot can go through a given environment at the first stage. We can reduce the search space of the robot path into 3 DOF instead of whole robot joint space. Total DOFs for the path planner at the first stage depend on the constraints imposed on the carried object. For example, if a horizontal constraint is imposed on the object, its DOF becomes 4, which makes a total of 7DOF with robot path.

Since the collisions at upper and lower body are separately resolved in the proposed spatial planning, there are limitations. The robot cannot make whole-body coordinated motions to avoid collisions, for example, swaying motion including lower body or stepping over obstacle in its way. This should be addressed through the close incorporation of dynamics into the global motion planner framework. Especially for humanoid applications, the next stage of the research is to develop a more efficient planner toward the unification of two stages of kinematic and dynamic motion planning. One of the possibilities is to include an efficient whole-body motion planner [40]based on a generalized IK framework with prioritized tasks including stepping. Another possibility is applying a reactive dynamic motion generation [23] based on task specification in operational space. In this case, a tool that integrates torque controller to global position-based motion planner need to be devised.

### B. Low-Level Control and Perception Integration

So far, we have described an offline planning framework. Once the motion generator outputs a dynamic trajectory in the second stage, we assume that it can be executed accurately enough by a low-level controller of the robot. In the case of humanoid, this low-level controller is composed of a servo controller and a stabilizer mechanism. In general, the fidelity of the real motion with respect to the planned motion is quite high, up to 10 mm at the end effectors. We estimate this error as maximum 50 mm at the tip of the object, including oscillation of the arm. This value was used as the tolerance.

The main purpose of the experiment is to demonstrate the capacity of the planner to generate dynamic 3-D collision avoidance motion in a relatively small environment. However, the integration of sensor feedback in the planning becomes a critical issue to overcome the limitations of offline planner against the accumulated motion errors or unexpected obstacles during longer motions in real environments. One possible extension is the integration of online modeling of static or moving obstacles. The localization capacity should also be incorporated to be able to correct the accumulated positioning errors during the motion.

For real-time execution, the computation is fast enough in the case of the space manipulator. However, for the humanoid robot, reshaping, including IK and collision avoidance, takes a long computation time. By improving the implementation efficiency, we could increase the frequency of the loop between planning and perception. Efficient online walking pattern generators are now studied intensively for reactive modification of humanoid locomotion [41], [42]. They will help to extend the planner to deal with challenging reactive planning problems with moving obstacles.

In this paper, we have presented a general planning framework for collision-free dynamic motion in 3-D using iterative reshaping. The proposed framework has two stages and benefits from advanced motion planning technology and powerful dynamic motion generators at each stage. The method allows us to plan motions for robots that have complex dynamics, such as space manipulators and humanoid robots, by applying appropriate reshaping methods. Temporal reshaping is utilized to make use of its convergence to the planned collision-free path in narrow passage. We employ spatial reshaping for the humanoid robot taking advantage of an efficient dynamic locomotion generator. As a result, the planned3-D collision-free motion has successfully been verified by simulations and experiments in both cases.

Our approach still does not resolve the general motion-planning problem for dynamic systems. Nevertheless, it constitutes a general framework that can successfully apply in practice.

### Acknowledgment

The authors would like to thank J. Himmelstein of LAAS–CNRS for his help.