Jennifer E King
email: jeking@andrew.cmu.edu

Marco Cognetti
email: cognetti@diag.uniroma1.it

Siddhartha S Srinivasa

Rearrangement planning using object-centric and robot-centric action spaces

This paper addresses the problem of rearrangement planning, i.e. to find a feasible trajectory for a robot that must interact with multiple objects in order to achieve a goal. We propose a planner to solve the rearrangement planning problem by considering two different types of actions: robot-centric and object-centric. Object-centric actions guide the planner to perform specific actions on specific objects. Robot-centric actions move the robot without object relevant intent, easily allowing simultaneous object contact and whole arm interaction. We formulate a hybrid planner that uses both action types. We evaluate the planner on tasks for a mobile robot and a household manipulator.

I. INTRODUCTION

In this paper, we offer a method for solving rearrangement planning problems. In these problems, a robot must work in clutter, reasoning about moving multiple objects in order to achieve a goal. In particular, we focus on rearrangement planning problems that require nonprehensile interactions, such as pushing, to interact with objects.

Several previous works have solved the problem using purely object-centric actions [START_REF] Barry | Manipulation with multiple action types[END_REF]- [START_REF] Stilman | Manipulation planning among movable obstacles[END_REF]. Here the planner is guided to perform specific actions on specific objects. These object-centric actions are often exposed to the planner in the form of user-defined high-level primitives. These primitives can be highly effective -allowing the planner to make large advancements toward the goal. However, the use of only these actions limits the types of solutions generated by the planner. In particular, these interactions usually involve only contact with a single part of the robot, i.e. the end-effector, and often forbid simultaneous object contact.

Recent work [START_REF] King | Nonprehensile whole arm rearrangement planning on physics manifolds[END_REF] proposed the use of lower-level primitives that describe only robot motions, with no object-relevant intent or explicit object interaction. The use of these robotcentric motions relaxes the restrictions imposed by the objectcentric actions, allowing the planner to generate solutions that exhibit whole arm interaction and simultaneous object contact. However, the resulting planner often suffers from long plan times due to the lack of goal directed motions available to the planner.

Humans use a diverse set of actions when interacting with the world. While many of these actions are object-centric, focusing on interacting in a specific way with a single object, other interactions are purely coincidental. These interactions are the unplanned result of a motion with different intent. Consider the example of reaching for a milk jug in the back of a refrigerator. One might first carefully slide the juice jug In each scenario, the robot must push the green box from its start pose to the goal region indicated by the green circle.

Our planner combines Robot-centric (dashed purple) and object-centric (solid orange) actions to generate feasible solutions.

out of the way, then simply reach for the milk, and trust that other objects that are touched will naturally be pushed out of the way, without requiring specific actions to move them. We wish to enable our motion planners to generate similar solutions to planning in clutter.

In this paper, we address the following research question: How can we blend the use of object-centric actions and robot-centric actions to best produce solutions to rearrangement problems? Our insight is that both types of actions are critical to generating expressive solutions quickly. By integrating the two actions types, we can use the freedom of interaction fundamental to the robot-centric actions while still allowing for the goal oriented growth central to the object-centric methods.

We propose a planner that incorporates both object-centric and robot-centric actions. In addition we propose to use physics models like those used in [START_REF] King | Nonprehensile whole arm rearrangement planning on physics manifolds[END_REF]- [START_REF] Haustein | Kinodynamic randomized rearrangement planning via dynamic transitions between statically stable configurations[END_REF] to forward propagate both the object-centric and robot-centric actions. This allows the motion primitives central to object-centric planners to produce simultaneous object contact and whole arm interaction without explicitly requiring the primitive designer to express and model such interaction. We demonstrate our algorithm on rearrangement planning problems for both a mobile robot KRex and the high degree-of-freedom HERB robot from Fig. 1. We show that by allowing the planner to consider hybrid paths that combine both action types, such as the one in Fig. 2, our planner perfoms as well or better than planners that use only object-centric or robot-centric actions.

The remainder of this paper is organized as follows. In Section II we describe the rearrangement planning problem this paper addresses in detail. We offer an overview of existing solutions and our method in Section III. We then provide experimental evaluation of our planner in Section IV. Finally, we offer limitations and ideas for future extensions in Section V.

II. THE REARRANGEMENT PLANNING PROBLEM

Assume we have a robot, R, endowed with configuration space C R . The robot is working in a bounded world populated with a set, M, of objects that the robot is allowed to manipulate. Each object is endowed with configuration space C i for i = 1 . . . m. Additionally, there is a set, O, of obstacles which the robot is forbidden to contact. Fig. 3 depicts each of these sets.

We define the state space X as the Cartesian product space of the configuration spaces of the robot and objects:

X = C R ⇥ C 1 ⇥ • • • ⇥ C m . We define a state x 2 X by x = q, o 1 , . . . , o m , q 2 C R , o i 2 C i 8i.
We define the free state space X f ree ✓ X as the set of all states where the robot and objects are not penetrating themselves, the obstacles or each other. Note that this allows contact between entities, which is critical for manipulation.

The task of the rearrangement planning problem is to find a feasible trajectory ⇠ : R 0 ! X f ree starting from a state ⇠(0) 2 X f ree and ending in a goal region ⇠(T ) 2 X G ✓ X f ree at some time T 0.

The state x evolves nonlinearly based on the physics of the manipulation, i.e. the motion of the objects is governed by the contact between the objects and the manipulator. We describe this evolution as a non-holonomic constraint: where u 2 U is an instantaneous control input. The function f encodes the physics of the environment. A path ⇠ is feasible if there exists a control, u 2 U , at every time t 0 that allows the constraint f to be satisfied while following ⇠.

ẋ = f (x, u) (1)
R Robot C R Robot c-space M Movable objects C i Movable i c-space O Obstacles X f ree Free state space U Control space X G Goal region

III. RANDOMIZED REARRANGEMENT PLANNING IN DETERMINISTIC ENVIRONMENTS

We utilize a Rapidly Exploring Random Tree (RRT) [START_REF] Lavalle | Rapidly-exploring Random Trees: A new tool for path planning[END_REF] to solve the rearrangement problem. Traditional implementations of the algorithm solve the two-point boundary value problem (BVP) during tree extension. Because we must plan in the joint configuration space of the robot and objects, solving the two-point BVP is as difficult as solving the full problem. In particular, the objects are not directly controllable. An object can only move as a result of contact between the manipulator and the object. Therefore, solving the two-point BVP to connect x 1 , x 2 2 X f ree requires finding a path for the robot that moves each object in M from its position in x 1 to its position in x 2 .

Previous works have handled this limitation in two ways, described in the following sections. The distance between the achieved state and targeted state. As can be seen, sampling object-centric actions leads to significant improvement in reaching the target state.

A. Random sampling

Algorithm 1 Kinodynamic RRT with random action sampling and physics model propagation

1: T {nodes = {x 0 }, edges = ;} 2: while not ContainsGoal(T ) do 3:
x rand SampleConfiguration() 4:

x near Nearest(T ,x rand ) 5:

for i = 1 . . . k do 6: (u i , d i ) SampleUniformAction() 7:
(x i , d i ) PhysicsPropagate(x near , u i , d i ) 8: i ⇤ = argmin i Dist(x i , x rand ) 9: if Valid((x near , x i ⇤ ), u i ⇤ , d i⇤ )) then 10: T.nodes [ {x i ⇤ } 11: T.edges [ {((x near , x i ⇤ ), u i ⇤ , d i ⇤ )} 12: path ExtractPath(T )
As suggested by Lavalle [START_REF] Lavalle | Randomized kinodynamic planning[END_REF] a useful alternative to solving the two-point BVP is to use a discrete time approximation to [START_REF] Barry | Manipulation with multiple action types[END_REF] to forward propagate all controls and select the best using a distance metric defined on the state space. In particular, define an action set A : U ⇥ R 0 where a = (u, d) 2 A describes a control, u, and associated duration, d, to apply the control. Then, a transition function, : X ⇥ A ! X, is used to approximate the non-holonomic constraint.

Our control space U is continuous, rendering full enumeration of the action set infeasible. Instead, we can sample k actions, forward propagating each under and selecting the best from this discrete set. Algorithm 1 shows the basic implementation.

This technique relies on robot-centric actions and uses the transition function to model the interactions that result from these robot motions. Recent work [START_REF] King | Nonprehensile whole arm rearrangement planning on physics manifolds[END_REF], [START_REF] Haustein | Kinodynamic randomized rearrangement planning via dynamic transitions between statically stable configurations[END_REF] has shown the use of physics models to implement this transition function. This is attractive because it allows complex interactions like multi-object pushing and whole arm manipulation to evolve naturally. It is also straightforward to implement, requiring only a physics model and a sampling method defined on the action space.

The drawback to this technique is its lack of focused tree growth. In particular, during each extension the tree is not strongly pulled toward the sampled configuration. Fig. 4 shows an example of the reduction in distance between the end of an extension and the sampled state as the number of samples actions, k, to select from increases. Even with a large value of k the tree makes very little progress toward the sampled state when using robot-centric actions. This is particularly detrimental when the sampled state is a goal state. The result is that the tree must randomly "stumble" on a goal rather than intentionally growing in that direction. This often leads to higher than desired plan times.

B. High-Level Actions

Algorithm 2 Kinodynamic RRT using motion primitives

1: T {nodes = {x 0 }, edges = ;} 2: while not ContainsGoal(T ) do 3:
x rand SampleConfiguration() 4:

x near Nearest(T ,x rand ) 5:

a 1 , . . . , a j GetPrimitiveSequence() 6:

x new PrimitivePropagate(x near , (a 1 ...a j ))

7:

if Valid((x near , x new ), (a 1 , . . . , a j )) then 8:

T.nodes [ {x new } 9:
T.edges [ {((x near , x new ), (a 1 , . . . , a j ))} 10: path ExtractPath(T )

An alternative method employed in [START_REF] Barry | Manipulation with multiple action types[END_REF], [START_REF] Jentzsch | MOPL: A multi-modal path planner for generic manipulation tasks[END_REF] is to use a a set of object-centric primitives capable of solving the two-point BVP in a lower dimensional subspace. For example, a "pushobject" primitive would be capable of providing a sequence of actions that moves an object from a start configuration to a sampled configuration. Robot-centric primitives are restricted to transit motions -actions where the robot moves without making contact with any object in the scene. Algorithm 2 shows the integration of primitives into the RRT.

This method is attractive because it can allow large extensions of the tree, and the sampling method is highly connected to tree growth. Fig. 4 shows the reduction in distance to the sampled state on an extension is much better when using these object-centric primitives. This is particularly useful when the sample is a goal state: it allows the tree to grow to the goal.

However, the reliance on object-centric actions to generate all object motion is detrimental in two ways. First, the actions are limited in their expressiveness. In particular, contact is restricted to only interactions between the manipulator and the single object targeted by the action. The PrimitivePropagate function (Algorithm 2-line 6) explicitly prohibits contact with other movable objects or obstacles in the scene. This prevents simultaneous object interactions, eliminating many feasible solutions when the robot is working in clutter.

Second, and possibly more importantly, this method is susceptible to failure if the primitive cannot be successfully applied. Consider the example in Fig. 5a. An example primitive may be to move the hand near the box with palm facing in the direction of the desired push, then push the box in the direction of its sampled location. The box is near the edge of (a) Desired end-effector pose for a "push-object" primitive is not within the reachable workspace of the robot (b) An alternative achievable endeffector pose that cages and pulls the object. Fig. 5: An example failed "push-object" primitive. The desired end-effector pose is not reachable. An alternate primitive that cages and pulls the object must be defined for the planner to find a solution in object-centric primitive based approaches.

the reachable workspace of the manipulator. As a result, all attempts at applying the high-level action will fail because the robot cannot reach the desired pose relative to the box. Even more problematic, a solution to the scene cannot be found given the current action space. To generate a solution, the programmer must define alternative primitives (Fig. 5b). x rand SampleConfiguration() 4:

C. Hybrid Approach

x near Nearest(T ,x rand )

(x i , A i ) PhysicsPropagate(x near , A i ) 12: i ⇤ = argmin i Dist(x i , x rand ) 13: if Valid((x near , x i ⇤ ), A i ⇤ )) then 14: T.nodes [ {x i ⇤ } 15: T.edges [ {((x near , x i ⇤ ), A i ⇤ )} 16: path ExtractPath(T )
We propose a method that allows for the freedom of interaction fundamental to the robot-centric methods while still allowing for the goal oriented growth central to the objectcentric methods. Algorithm 3 shows the modified algorithm.

Like in the method described in Section III-A, at each tree extension, the best of k possible extensions is selected. However, each candidate extension i expresses a sequence of actions, A i . With some probability, p rand , the sequence A i contains a single action a = (u, d) drawn uniformly at random from the space of feasible actions. With probability 1 p rand , A i contains a sequence of actions, a 1 . . . a j , that are equivalent to the primitives described in Section III-B with noise applied to the primitive parameters. In all cases, the sampled action sequence A i is propagated through the physics model and the sequence is truncated at the first infeasible state encountered, i.e. collision with a static obstacle. This method is attractive because it combines the strengths of the methods described in Section III-A and Section III-B. Incorporating the physics model in the propagation removes the restriction that object-centric primitives can only allow interaction between the manipulator and the object the primitive is defined on. Instead, any unintended contact with other objects in the scene can be modeled. Often, this unintended contact is not detrimental to overall goal achievement and should be allowed. Sampling random actions with some probability allows the planner to generate actions that move an object when all primitives targeted at the object would fail (i.e. the example in Fig. 5).

IV. EXPERIMENTS

We implement the planner described in Section III-C in the Open Motion Planning Library [START_REF] Sucan | The Open Motion Planning Library[END_REF]. We test three versions of the planner. First, we set p rand = 0. This forces the planner to always sample primitive sequences. We denote this planner as object-centric in all results. Second, we set p rand = 1. This forces the planner to always sample a random action. We denote this planner as robot-centric in all results. Finally, we set p rand = 0.5. This allows the planner to choose primitives or random actions with equal probability. We denote this planner as hybrid in all results. We discuss alternative selections for p rand and their impact on results in Section V.

Using the planner, we execute a set of experiments to test the following hypotheses:

H1: On scenes easily solvable using object-centric actions, the hybrid planner performs equivalent to the object-centric planner in both success rate and plan time. Additionally, both the object-centric and hybrid planners outperform the robot-centric planner that samples only random actions. H2: The hybrid planner achieves higher success rate and faster plan times than the object-centric or robotcentric planners on difficult scenes.

We test the hypotheses in two scenarios: a steered car pushing boxes and a 7 degree-of-freedom household manipulator pushing objects on a table. We define "easy" and "difficult" scenes more precisely in the scenario descriptions.

A. Mobile manipulator 1) Problem setup: We first test our planner using a mobile manipulator (Fig. 1c). The robot behaves as a steered car. For this robot, C R = SE(2) and a control u = (v, ) 2 U describes the forward velocity and steering angle applied to the robot. The robot interacts with objects in the plane. We use the Open Dynamics Engine (ODE) [START_REF]Open Dynamics Engine[END_REF] as our physics model to forward propagate all actions. We use objects that are quasistatic in nature, i.e. the object comes to rest as soon as the robot ceases to apply forces. This allows us to represent the state of objects by only their configuration, ignoring object velocities. As a result, C i = SE(2) for i = 1 . . . m.

We task the robot with pushing a box from its start configuration into a goal region with radius 0.5m. We confine the robot to move in a 15m ⇥ 10m region. Any actions that move the robot or any object outside of this bounded region are considered invalid.

For this problem the following primitive set is defined: 1) Transit: The transit primitive moves the robot from a start to a goal configuration in SE(2) by finding the shortest length Dubins curves [START_REF] Dubins | On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents[END_REF] connecting the two configurations (Fig. 6a). 2) Push: The push primitive pushes an object along the straight line connecting a start and goal configuration for the object. The primitive returns a set of actions that first move the robot to a position and orientation "behind" the object, then drives the robot straight along the ray, pushing the object (Fig. 6b). When sampling random actions, we sample forward velocity from the range [ 0.5, 0.5] m/s and duration from the range 0.5s to 5.0s. We define the sampling range for steering angle using the same minimum and maximum angles as used for generating the Dubins paths.

2) Simple scenes: We first test our planner on three scenes solved in less than 10s on average using the object-centric primitives defined in the previous section. We denote these three scenes "simple" or "easy" scenes. Fig. 7 shows an example scene and a single solution from each planner. We run each planner 50 times on each scene, for a total of 150 trials per planner. For each trial, we record the total time to find a solution. Fig. 7a shows the success rate as a function of plan time for up to 60 seconds of total plan time budget. As can be seen, both the hybrid and object-centric planners solve all scenes in the allotted plan time, while the robotcentric planner fails often. A one-way ANOVA with Tukey HSD post-hoc analysis reveals there is no significant difference in mean plan time between the hybrid and object-centric planners (p = 0.297). There is a significant difference between hybrid and robot-centric (p < 0.001) and object-centric and robot-centric (p < 0.0001). This supports our hypothesis:

The hybrid planner outperforms the robot-centric planner, and performs as well as the object-centric planner on simple scenes.

3) Difficult scenes: Next we test our planner on three scenes that require more than 60s on average to be solved with objectcentric actions. We denote these three scenes as "difficult" scenes. Fig. 1d and Fig. 8 show these scenes and example solutions. Again we run each planner 50 times on each scene. Fig. 9 shows the success rate of all three planners as a function of plan time. As can be seen, the hybrid planner is far more successful than the object-centric or robotcentric planners. A one-way ANOVA with Tukey HSD posthoc analysis reveals that the difference in mean plan-time between the hybrid and object-centric planners is statistically significant (p < 0.001) as is the difference between the hybrid and robot-centric planners (p < 0.05). This supports our hypothesis: The hybrid planner achieves higher success rate and faster plan times than the object-centric or robot-centric planners on difficult scenes.

4) Qualitative analysis: Next we examine some qualitative aspects of the solutions. Neither the push primitive nor the transit primitive allow the robot to move in reverse. As a result, in difficult scenes like in Fig. 8, we see random actions used to back the robot away from boundaries and obstacles (Fig. 8b, Fig. 8c).

Additionally, in the difficult scenes (Fig. 1d, Fig. 8) a static obstacle blocks any path from the start configuration to an object. Thus the push primitive fails in most applications from the start. In addition, the static obstacles also cause many applications of the transit primitive to fail, as they often drive the robot into an obstacle. Here, the use of random Fig. 8: In this scene, the wall (black) serves as an obstacle preventing application of the push primitive from the start configuration. Robot-centric actions are needed to grow the tree until such a primitive can be applied. Once available, the object-centric push primitive extends the tree to the goal. Fig. 9: Success rate as a function of plan time on scenes difficult to solve using only the available primitives (i.e. Fig. 1d and Fig. 8)

actions available to the hybrid planner is advantageous because they can be used to start tree growth from the root. Then, after the tree begins growing, the push primitive and transit primitive can be applied more freely. In fact, if we analyze all solutions generated by the hybrid planner (across easy and difficult scenes), we see that 89.2% (207/232) end in an objectcentric primitive. This supports our intuition that object-centric actions help grow the tree to the goal.

B. Household manipulator

1) Problem setup: Next, we execute a series of tests in simulation for the HERB robot [START_REF] Srinivasa | HERB: A Home Exploring Robotic Butler[END_REF]. We task HERB to push an object on a table top from a start configuration to a 10 cm radius goal region. We plan for the 7 degree-of-freedom (DOF) left arm of the robot, thus C R = R 7 and a control for the arm defines joint velocities for each of the 7 DOFs. Following the example in [START_REF] King | Nonprehensile whole arm rearrangement planning on physics manifolds[END_REF], we confine the end-effector of the robot to move in the plane parallel to the table surface. We note that we allow and model pushing contact between the full arm and all objects. Any motions that push an object off of the table are considered invalid.

For this problem the following primitive set is defined: 1) Transit: The transit primitive moves the end-effector from a start pose to a goal pose. The motion of the end-effector follows a straight line in workspace along the plane parallel to the table surface. 2) Push: The push primitive pushes an object along the straight line connecting a start and goal configuration for the object. The primitive returns a set of actions that first move the end-effector to a position and orientation "behind" the object, then move the end-effector straight along the ray, pushing the object. The motion of the end-effector is confined to the plane parallel to the table surface during the entire primitive. We use a quasistatic model of planar pushing as our physics model [START_REF] Lynch | Stable pushing: Mechanics, controllability, and planning[END_REF]. Because we only model objects moving in the plane, C i = SE(2) for i = 1 . . . m. We note that more complicated primitives could be defined. For example, we could implement the transit primitive by calling a motion planner for the arm and allowing the end-effector to move out of the plane. The primitives we use are selected to have computational complexity similar to that of sampling random actions, allowing us to more fairly compare plan times between the object-centric and robot-centric approaches.

2) Simple scenes: We first test our planner on three scenes solved in less than 60s on average using the primitives defined in the previous section. We denote these scenes as "easy" scenes for the household manipulator. Fig. 10 shows an example scene and solution. We run each planner 50 times on each scene, for a total of 150 trials per planner. Fig. 11 shows the success rate as a function of plan time for up to 300 seconds of total plan time budget. As can be seen, both the hybrid and object-centric planners perform equivalently. A one-way ANOVA with Tukey HSD post-hoc analysis confirms the hybrid and object-centric planners to not differ significantly in mean plan time (p = 0.782). The hybrid and robot-centric do show significant difference (p < 0.0001) as do the objectcentric and robot-centric (p < 0.0001). This further supports H1.

3) Difficult scenes: We then test our planner on four scenes for HERB that require more than 300s on average to solve using object-centric actions. We denote these scenes as "difficult" scenes. Each either has clutter blocking the path to the goal, or the goal object near the edge of the reachable workspace of the robot. Again we run each planner 50 times on each scene. Fig. 12 shows the success rate of all three planners as a function of plan time. These scenes are difficult and each planner struggles to find a solution. The hybrid planner performs slightly better than the other planners but a one-way ANOVA Fig. 11: Success rate as a function of planning time for a set of 50 trials on each of 3 scenes. These scenes can easily be solved using the defined primitives. As can be seen, the hybrid approach performs as well as the objectcentric approach. with Tukey HSV post-hoc analysis reveals the difference in mean plan time is not significant when compared to the objectcentric (p = 0.629) or robot-centric (p = 0.566) planners. This does not support H2.

We believe this result is strongly tied to the expressiveness of our primitives enabled by the physics propagation. Specifically, by propagating the primitives through a physics model, we eliminate many of the failure cases that would prevent application of object-centric primitives in our scenes, i.e. collision between the robot and other movable objects. 4) Qualitative analysis: Similar to the KRex results, we see the random robot-centric actions used often to move objects to a location where object-centric primitives can be applied. For example, in Fig. 2 the push primitive cannot be applied to the object in its start location. The back of the forearm is used to move the box nearer the robot (Fig. 2c), enabling the primitive to then be applied (Fig. 2d-Fig. 2e). Similar to KRex, 80.3% (155 / 193) of paths across all scenes end with an object-centric primitive, further supporting our intuition that object-centric primitives are important to goal achievement. Fig. 1a and Fig. 10 show two other example paths ending in object-centric primitives.

V. DISCUSSION

In this work, we have presented a method for combining robot-centric and object-centric action spaces to improve rearrangement planning. Our experiments show that by using a combination of the two action types, we are able to improve success rate and plan time when compared to planners that use only a single action type. In addition, we show that by using a physics model to forward propagate object-centric primitives, we are able to allow the primitives to express simultaneous object interaction and whole arm manipulation without explicit encoding. In all results, we used a value of p rand = 0.5 for our hybrid planner. This value allows object-centric primitives and robot-centric random actions to be selected with equal probability on each extension. However, we can vary this value to possibly improve performance. Fig. 14 shows the success rate as p rand is varied for one of the easy KRex scenes and one of the difficult KRex scenes. The results seem to indicate that low values of p rand are move effective. Future work should examine more closely the relationship between problem composition and p rand .

The randomized nature of the planner means the resulting paths may be highly suboptimal in path length. One common approach to improving optimality is to apply shortcutting techniques to the paths [START_REF] Geem | Mobility analysis for feasibility studies in cad models of industrial environments[END_REF]- [START_REF] Sekhavat | Multilevel path planning for nonholonomic robots using semi-holonomic subsystems[END_REF]. These techniques attempt to connect two points on the path with shorter segments that solve the two-point BVP. Applying this technique to rearrangement planning is difficult because the two-point BVP cannot be solved in the general case. However, it may be possible to extend prior work [START_REF] King | Nonprehensile whole arm rearrangement planning on physics manifolds[END_REF] and select points where the two-point BVP only needs to be solved in a lower-dimensional subspace. Then an object-centric primitive sequence could be applied.

We focus on scenarios that can be solved using only pushing interactions. Future work should examine the impact of expanding the search to include other interactions such as grasping or toppling. This expanded search increases the size of the state space, requiring objects to be represented in SE(3) rather than SE(2), and the action space, likely requiring we consider actions that move the end-effector in SE(3). However, it may result in simpler plans for scenarios that can easily be solved using alternative interaction primitives, i.e. pick-and-place.

Finally, while we have shown success in executing the generated trajectories on a real robot (Fig. 13), some trajectories fail to achieve the goal when executed due to uncertainties in the object perception, robot forward kinematics and physical model of robot-object interaction. Prior work has shown that actions similar to the "push-primitive" used in Section IV-B can be uncertainty reducing [START_REF] Dogar | Push-grasping with dexterous hands: Mechanics and a method[END_REF]. Including such primitives, and guiding the planner to select them, may improve the robustness of the generated trajectories to uncertainty, increasing the planner's applicability in everyday use.

Fig. 1 :

 1 Fig. 1: Rearrangement planning examples for two manipulators, the HERB robot and KRex robot. In each scenario, the robot must push the green box from its start pose to the goal region indicated by the green circle. Our planner combines Robot-centric (dashed purple) and object-centric (solid orange) actions to generate feasible solutions.

  (a) Start configuration (b) A short transit primitive followed by a random motion (c) The robot uses the back of the forearm to move the box to a location where a push primitive can be applied. (d) An object-centric push primitive is used to move the box to the goal region. (e) The final configuration. The robot moves the object, blue box and glass simultaneously to achieve the goal.

Fig. 2 :

 2 Fig.2:(Top) The execution of a trajectory that moves the green box from its start configuration to the goal region. The robot uses a mix of random robot-centric motions and object-centric primitives to accomplish the goal. By using a physics model to rollout all actions the planner can easily generate solutions that use the whole arm and move multiple objects simultaneously. (Bottom) A top-down render of the path of the end-effector. Random robot-centric motions are shown in purple, object-centric transit and push primitives are shown in orange.

Fig. 3 :

 3 Fig. 3: The planning environment

  Fig.4: The distance between the achieved state and targeted state. As can be seen, sampling object-centric actions leads to significant improvement in reaching the target state.

Algorithm 3

 3 Kinodynamic RRT using hybrid action sampling 1: T {nodes = {x 0 }, edges = ;} 2: while not ContainsGoal(T ) do 3:

Fig. 6 :

 6 Fig. 6: Two primitives defined for the mobile manipulator. Dubins paths are used to generate paths between two poses in SE(2).

Fig. 7 :

 7 Fig. 7: Example solutions for each of the three planners for an easy scene.

  (a) Start configuration (b) A robot-centric random action is used to reverse the robot away from the boundary (c) A transit primitive moves the robot near the wall (d) Two robot-centric random actions are used to reverse the robot away from the obstacle and then drive the robot into open space (e) Finally, an objectcentric action is used to push the target to the goal region.

  Fig.10:(Top) The execution of a trajectory that moves the green box from its start configuration to the goal region. In easy scenes like this, a path can be found by the hybrid planner that uses mostly transit and push primitives. (Bottom) Top down view of the end-effector motion in the resulting path.

Fig. 12 :

 12 Fig.12: Success rate as a function of plan time for scenes difficult to solve using object-centric primitives

Fig. 14 :

 14 Fig.14: Success rate as a function of p rand for the scenes in Fig.7(red) and Fig.8(blue)

Fig. 13 :

 13 Fig. 13: Successful execution of a planned path containing object-centric and robot-centric actions on the HERB robot.

VI. ACKNOWLEDGEMENTS This material is based on work supported by the NASA Space Technology Research Fellowship program (award NNX13AL61H), the National Science Foundation IIS (award 1409003), Toyota Motor Engineering and Manufacturing (TEMA), and the Office of Naval Research.


Marco Cognetti

Valentino Fioretti

Giuseppe Oriolo

Whole-body planning for humanoids along deformable tasks

HAL is

I. INTRODUCTION

In recent years, humanoid robots have been the subject of constantly increasing attention. While earlier work was mostly aimed at obtaining stable and efficient walking gaits, many researchers are now looking at the generation of taskoriented, whole-body motions for humanoid robots.

Motion planning for humanoids is particularly challenging for various reasons. First, the planning space is highdimensional because these robots possess many degrees of freedom. Second, a humanoid robot is not a free-flying system in its configuration space. The various kinematic and dynamic constraints of the specific robot must also be taken into account, as does the necessity of maintaining equilibrium (either static or dynamic) at all times. In view of this all complexity, the problem is usually addressed by making simplifying assumptions on either the robot geometry or the environment [START_REF] Kuffner | Footstep planning among obstacles for biped robots[END_REF][START_REF] Kuffner | Motion planning for humanoid robots[END_REF][START_REF] Chestnutt | Footstep planning for the honda ASIMO humanoid[END_REF][START_REF] Perrin | Fast humanoid robot collision-free footstep planning using swept volume approximations[END_REF][START_REF] Pettré | A 2-stages locomotion planner for digital actors[END_REF][START_REF] Yoshida | Humanoid motion planning for dynamic tasks[END_REF][START_REF] Kuffner | Dynamically-stable motion planning for humanoid robots[END_REF].

Assigning a task to the robot further increases the planning difficulty. The task may be a single action (e.g., 'grasp this object') or a sequence of navigation and manipulation actions ('take that object and bring it in the other room').

A common approach for generating task-constrained movements is kinematic control. One popular example is the task-priority method [START_REF] Siciliano | A general framework for managing multiple tasks in highly redundant robotic systems[END_REF], extended to handle inequality constraints in [START_REF] Kanoun | Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task[END_REF]. This framework was used in [START_REF] Kanoun | Planning foot placements for a humanoid robot: A problem of inverse kinematics[END_REF] for computing footsteps of humanoids that must execute manipulation tasks. Despite its efficiency, kinematic control by itself remains a purely local technique, well suited for reactive behaviors but in general outperformed by full-fledged planning techniques.

Three main approaches can be found in the literature for task-oriented humanoid planning: (i) separating locomotion

The authors are with the Dipartimento di Ingegneria Informatica, Automatica e Gestionale, Sapienza Università di Roma, via Ariosto 25, 00185 Roma, Italy. E-mail: {cognetti, oriolo}@diag.uniroma1.it. This work is supported by the EU FP7 COMANOID project. from task execution [START_REF] Burget | Whole-body motion planning for manipulation of articulated objects[END_REF], [START_REF] Hauser | Randomized multi-modal motion planning for a humanoid robot manipulation task[END_REF]; (ii) planning statically stable collision-free trajectories, later converted to dynamically stable motions [START_REF] Dalibard | Dynamic walking and whole-body motion planning for humanoid robots: An integrated approach[END_REF]; (iii) achieve acyclic locomotion and task execution through whole-body contact planning [START_REF] Bouyarmane | Humanoid robot locomotion and manipulation step planning[END_REF].

A fundamentally different approach, which we introduced in [START_REF] Cognetti | Whole-body motion planning for humanoids based on com movement primitives[END_REF], is to solve the task-constrained planning problem without separating locomotion from whole-body motion generation. Our method hinges on the concept of CoM movement primitives, defined as precomputed trajectories of the CoM that are associated to specific actions. The proposed planner builds a tree in configuration space by concatenating feasible, collision-free whole-body motions that realize a succession of CoM movements and, at the same time, the assigned task. Results showed that the planner was able to generate sensible plans for a variety of composite tasks requiring a combination of navigation and manipulation.

In [START_REF] Cognetti | Whole-body motion planning for humanoids based on com movement primitives[END_REF], it was assumed that the assigned task is not modifiable. While this may sometimes be the case, it is often just a working assumption that does not represent an actual requirement. For example, suppose we want the humanoid to move an object from a location to another: the actual trajectory of the object may not be relevant, provided that it is reasonably efficient (e.g., it does not wander too much). Typically, the user may specify1 a very simple tentative task trajectory (e.g., a line segment joining the initial and the final task point), which can be deformed by the planner if this is convenient for finding a solution.

Task path/trajectory deformation was considered in [START_REF] Geraerts | Creating high-quality paths for motion planning[END_REF], via length and clearance optimization, and in [START_REF] Vannoy | Real-Time Adaptive Motion Planning (RAMP) of Mobile Manipulators in Dynamic Environments with Unforeseen Changes[END_REF], where multiple trajectories are maintained and deformed in realtime using a genetic algorithm. The approach of [START_REF] Brock | Elastic strips: A framework for motion generation in human environments[END_REF], [START_REF] Yang | Elastic roadmaps -motion generation for autonomous mobile manipulation[END_REF] combines motion planning with reactive behaviors to generate task trajectories for mobile manipulators. Functional gradient techniques are used in [START_REF] Zucker | CHOMP: Covariant Hamiltonian optimization for motion planning[END_REF] to improve the quality of an initial trajectory. However, none of these works addresses the specific problems posed by humanoid robots.

The method proposed in this paper consists of two main components: a constrained motion planner and a deformation mechanism. The basic idea is that the constrained motion planner attempts to solve the problem for the original task. If this proves to be too difficult, a deformation mechanism is triggered that modifies the task path using simple heuristic functions, and the constrained motion planner is invoked on the deformed task. If needed, the procedure is iterated.

The paper is organized as follows. In the next section, the planning problem is formulated. A quick overview of the planner is given in Sect. III. The constrained motion planner is described in Sect. IV, while the deformation mechanism is presented in Sect. V. Motion planning experiments for the NAO humanoid robot are presented in Sect. VI. Future research directions are briefly discussed in Sect. VII.

II. PROBLEM FORMULATION

Before formulating the planning problem, we recall the motion model of [START_REF] Cognetti | Whole-body motion planning for humanoids based on com movement primitives[END_REF] for generating humanoid motion.

A. Humanoid Motion Model

A configuration q of a free-flying humanoid can be identified by its joint n-vector q jnt ∈ C jnt and the pose (position plus orientation) q CoM ∈ SE(3) of a reference frame attached to the robot Center of Mass (CoM):

q = q CoM q jnt .
In our planner, CoM motions will be generated by patching CoM subtrajectories taken from a catalogue of CoM movement primitives, that represent typical humanoid motions, e.g. static steps, dynamic steps, crouching, and so on. Each primitive has a certain time duration and may specify a trajectory for other points of the robot, in addition to the CoM; for example, a stepping primitive will include a trajectory for the swing foot. Once a CoM movement primitive has been selected, the joint trajectory can still be chosen by the planner among the infinite whole-body motions compatible with the primitive, in such a way that other requirements are met (task execution, obstacle avoidance, etc).

The above approach to motion generation is reflected in the hybrid (partly algebraic, partly differential) model

q CoM (t) = q CoM (t k ) + A(q CoM (t k )) u CoM (t k ) (1) qjnt (t) = v jnt (t) (2)
describing the robot motion in an interval [t k , t k+1 = t k +T k ] in which the CoM is performing a primitive movement of duration T k . Here, q CoM (t k ) is the CoM frame pose at the start of the interval; A(q CoM (t k )) is the transformation matrix from the CoM frame at t k to the world frame; u k CoM (t) is the pose displacement of the CoM frame at t, relative to its pose at t k ; v jnt is the vector of joint velocities.

Note that in (1-2) the CoM displacement and the joint velocity profile are not independent: as explained in detail in Sect. IV, v jnt | [t k ,t) is chosen so as to realize the CoM movement primitive u k CoM (t).

B. Task-Oriented Planning

We consider tasks defined as trajectories for the position (and possibly the orientation) of a specific point (body) of the humanoid: e.g., a manipulation task may be a trajectory for one hand of the robot, while a navigation task may be a trajectory for the midpoint between its feet. Denoting task coordinates by y, a forward kinematic map relates them to generalized coordinates y = f (q) = f (q CoM , q jnt ).

An initial task trajectory y

[0] (t), t ∈ [t i , t f ], is assigned 2 composed by a geometric path y [0] (s), s ∈ [s i , s f ], and a time history s [0] (t), t ∈ [t i , t f ], with s i = s(t i ), s f = s(t f ).
In particular, the geometric path y [0] is assumed to be a deformable curve in task space: this means that the endpoints y [0] (s i ) and y [0] (s f ) are fixed, but the actual shape of the curve may be changed if necessary by acting on certain parameters. For illustration, in the rest of the paper we will consider B-splines as deformable curves, and the action parameters will be control points. The planner is allowed to change the shape of the task path (and adapt the time history), if this is deemed necessary to find a solution.

The considered planning problem consists in finding a feasible whole-body motion of the humanoid that realizes the task trajectory, possibly deformed, while avoiding collisions and satisfying kinematic constraints. More formally, a solution to our problem consists of:

1) A final task trajectory y

* (t), t ∈ [t i , t f ], consisting of a geometric path y * (s), s ∈ [s i , s f ],
obtained by (repeated) deformation of y [0] (s), and an associated time history s

* (t), t ∈ [t i , t f ]. 2) A whole-body motion of the humanoid q * (t), [t i , t f ],
that satisfies the following requirements: R1 The final task trajectory is realized:

f (q * (t)) = y * (s * (t)) = y * (t), t ∈ [t i , t f ].
R2 The robot maintains static or dynamic equilibrium. R3 All collisions with obstacles are avoided. R4 Joint limits and velocity bounds, respectively expressed in the form q jnt,m < q jnt < q jnt,M and v jnt,m < v jnt < v jnt,M , are satisfied. The above formulation includes directly two special cases of practical interest:

• Set-point task: When the task reduces to a desired setpoint y(t f ) (e.g., bring the humanoid hand to a certain placement), the initial task path y [0] (s) may be chosen as any deformable curve (e.g., a line) joining the starting task position y(t i ) = f (q(t i )) with the final task position y(t f ). The time history may be simply set as

s [0] (t) = t, ∀t ∈ [t i , t f ].
See also footnote 1.

• Partially deformable task: In some problems, it may be desiderable to allow deformation only on a subset of task components. For example, consider a manipulation problem in which the robot has to carry a glass containing some liquid. The glass should be kept vertical, so as not to spill the liquid. A deformation can therefore be applied only to the position components of the task (i.e., to the Cartesian trajectory of the glass), whereas the orientation components should not be affected. ] , s [i+1] ] ← TaskDeformation(y [i] , s [i] , T , ỹ);

[i] ← y [i] (s [i] ); 5 [T , ỹ] ← ConstrainedMotionPlanner(y [i] ); 6 if ỹ = y [i] (t f ) then 7 sol found ← true; 8 y * ← y [i] ; s * ← s [i] ; 9 else 10 [y [i+1

III. PLANNER OVERVIEW

The underlying principle of the proposed planner, whose pseudocode 3 is given in Algorithm 1, is simple.

First, we invoke a constrained motion planner that expands a tree T in configuration space in the attempt to find a wholebody motion realizing the initial task trajectory y [0] and satisfying requirements R2-R4 (line 5). If this proves to be too difficult, the planner returns a partial solution whose limit task point ỹ (defined as the furthermost task point realized by configurations contained in the tree T ) is different from y(t f ). In this case, the initial task path is modified using deformation actions which take place at the limit task point (lines 9-10), and the constrained motion planner is invoked again. This deformation-planning cycle is repeated until a solution is found.

In the following sections we describe in detail the two main components of our algorithm, i.e., the constrained motion planner and the deformation mechanism.

IV. CONSTRAINED MOTION PLANNER

The constrained motion planner, whose pseudocode is shown in Algorithm 2, is essentially the same of [START_REF] Cognetti | Whole-body motion planning for humanoids based on com movement primitives[END_REF]. The task trajectory to be realized is denoted by y d ; in the i-th iteration of Algorithm 1, the planner is invoked with y d set to y [i] . The goal is to find a whole-body motion that realizes y d and is feasible, i.e., satisfies requirements R2-R4.

The planner works in an iterative fashion and builds a tree T in configuration space, rooted at q(t i ). The nodes are configurations associated with a time instant and realize samples of y d , while arcs represent feasible whole-body motions that join adjacent nodes and realize portions of y d .

The planner uses a task compatibility metric γ over the configuration space. In particular, γ(q, ȳ) defines the compatibility of the robot configuration q with respect to a certain sample ȳ of the task trajectory.

The generic iteration starts by selecting a random sample y d rand from the task trajectory y d (Algorithm 2, line 6). A configuration q near is then randomly extracted from the tree 3 For compactness, we omit arguments t and s and their range of variation in all pseudocode; for example, y [0] (t), t ∈ [t i , t f ], is simply denoted by y [0] . We use the same notation in the text when no confusion is possible.

Algorithm 2: ConstrainedMotionPlanner(y d ) 1 root the tree T at q(ti); 2 q ← q(ti); ỹ ← y d (ti); j ← 0 ; 3 ỹ.exp fail ← 0; ỹ.coll fail ← 0; ỹ.jnt fail ← 0; 4 repeat 5 j ← j + 1; 6 select a random sample y d rand from the task trajectory; 7 select a random node q near from T with probability proportional to γ(•, y d rand );

8
get the time instant t k associated with q near ; 9

[q new ,q near q new ,t k+1 ,ỹ]←MotionGeneration(q near ,t k ,ỹ);

10 if q new = ∅ then 11
add node q new and arc q near q new to T ; Procedure 1: MotionGeneration(q near , t k , ỹ) ỹ.exp fail ← 0; ỹ.coll fail ← 0; ỹ.jnt fail ← 0; 28 end 29 return [q new , q near q new , t k + T k , ỹ] using γ(•, y d rand ) to bias the process (line 7). Once q near has been selected, the associated time instant t k defines the start time for the subsequent motion generation (line 8).

Procedure 1 contains a pseudocode for motion generation. This starts by randomly choosing a CoM movement primitive u k CoM (•) of duration T k , simply denoted by u k CoM so forth, from the set4

U = U S CoM ∪ U D CoM ∪ free CoM . (3)
U S CoM and U D CoM collect stepping primitives, respectively extracted from a static and a dynamic gait; the former subset includes forward, backward, left and right steps, whereas the latter contains starting, cruise and a stopping steps for various directions. All these primitives satisfy requirement R2 by construction. As for free CoM, the CoM movement is unconstrained, but both feet must remain fixed and the robot should maintain static equilibrium at all times. The duration of this primitive is free, and the planner can use it to adjust the total plan duration (remember that all the other primitives have a fixed duration).

Plugging u k CoM in the algebraic equation of (1) yields the desired trajectory q d

CoM of the CoM frame within [t k , t k+1 ], with t k+1 = t k + T k . Denote by z d

CoM the position components of this trajectory. If the chosen primitive is of the stepping type, the swing foot trajectory z d swg is also assigned within the time interval.

Joint motions are then generated so as to realize the portion of the task trajectory y d between t k and t k+1 , together with z d

CoM and z d swg . Let y a = (y, z CoM , z swg ) be the augmented task vector (y a = (y, z swg ) for free CoM), and denote by J a its Jacobian matrix w.r.t. q jnt . Moreover, define the augmented task error e(t) = y d a (t) -y a (t), where y d a (t) is the reference value of the augmented task in [t k , t k+1 ]. Joint velocity commands are computed as

v jnt = J † a (q jnt )( ẏd a +Ke)+(I -J † a (q jnt )J a (q jnt ))w, (4
) where J † a is the pseudoinverse of J a , K is a positive definite gain matrix, and w is a randomly chosen n-vector which is projected in the null space of J a through the orthogonal projection matrix I -J † a J a . Use of eq. ( 4) guarantees that ė = -K e, i.e., exponential convergence of the augmented task vector to the desired trajectory y d a ; since we start on the trajectory, stable exact tracking is achieved.

The whole-body motion generated by ( 4) is continuously checked for collisions as well as w.r.t. joint limits and velocity bounds; if free CoM is used, static equilibrium is also checked (equilibrium is guaranteed by construction when using stepping primitives). If no violation occurs, integration proceeds up to t k+1 ; we have then obtained a feasible joint motion q jnt (t), t ∈ [t k , t k+1 ], that complies with a portion of the task. If this portion extends beyond the current limit task point ỹ, the latter is updated accordingly (Procedure 1, lines 25-28). On termination, control goes back to Algorithm 2, which adds any generated joint motion as an arc (and its final configuration as a node) in T (lines 10-12).

In the presence of a violation, motion generation is interrupted and no node is added to T (Procedure 1, lines 7-23). In the particular case y d (t k ) = ỹ (i.e., a tree expansion was being attempted from a configuration associated to the current limit task point), a failed expansion counter is incremented together with a specific counter recording the nature of the violation (Procedure 1, lines 8-12 and 16-20).

At the end of each iteration (Algorithm 2, line 13), the constrained motion planner checks whether the final task point y d (t f ) has been reached. In this case, a solution has been found for the current task trajectory y d = y [i] , which is returned, together with the joint motions that realize the associated whole-body motion plan. If, however, y d (t f ) has not been reached, the following conditions are checked:

• if a maximum number of iterations has been reached;

• if the number of failed expansions from the current limit task point has reached a predefined threshold. Whenever one of these becomes true, we argue that the planner is not likely to find a solution constrained to the current task trajectory y d = y [i] , which is then deformed as explained in the next section.

V. TASK DEFORMATION The pseudocode of the task deformation procedure is given in Algorithm 3. In addition to the latest task trajectory y [i] , the procedure receives in input the limit task sample ỹ reached on y [i] by the constrained motion planner, as well as the associated exploration tree T .

Recall that the current task path5 y [i] is parametrized by a set of control points, collected in a vector σ [i] ; the endpoints are however fixed. The idea is to deform y [i] (s) by inserting a new control point in σ [i] . The time history s [i] (t), t ∈ [t i , t f ] will then be adapted to the new path. Two heuristics are used to choose where to place the new control point.

The first heuristic is obstacle-based. Its rationale is very simple: pushing the task path away from the closest obstacle may increase the possibility of finding a solution. To this purpose, the limit task point ỹ is considered: a new control point is inserted on the line joining ỹ with a representative point of the closest obstacle (e.g., its closest point) at a predefined distance from ỹ. The result is that the entire task path is pushed away from the obstacle, as shown in Fig. 1.

The second heuristic we use is robot-based. The intuition behind this mechanism is that constrained planning is more difficult if the task path is far from the humanoid CoM, because (1) motion generation must realize an extended task which includes both the original task variables and the CoM position (2) outstretched postures typically push the joints towards the limit of their available range. To bring the task path closer to the humanoid, a limit configuration is associated to the limit task point ỹ by extracting from T a node q such that ỹ = f (q), and a new control point is inserted on the line joining ỹ with the humanoid CoM at q, again at a predefined distance from ỹ (see Fig. 2).

It is relatively easy to devise a policy that automatically selects among the two deformation heuristics. Recall that deformation is invoked after a predefined number of expansion attempts from ỹ have failed due to joint limit violations or collisions. If the number of failures due to the first is larger (Algorithm 3, lines 1-5), the robot-based heuristic is used; otherwise (lines 6-10), the obstacle-based heuristic is used. This selection strategy is a straightforward consequence of the above discussion on the rationale of each heuristic.

Finally, the time history s [i+1] is adapted to the deformed task path y [i+1] via uniform scaling within a total duration proportional to the new path length.

VI. PLANNING EXPERIMENTS

The proposed planner has been implemented in V-REP on an Intel Core 2 Quad at 2.66 GHz using NAO by Aldebaran Robotics as humanoid robot. As deformable descriptions of the task path, we have used B-splines. Since manipulation tasks were considered in all experiments, the task compatibility metric γ(ȳ, q) has been defined as the inverse of the distance between the task point ȳ and the robot CoM at q.

The CoM movement primitive set is defined as in [START_REF] Chestnutt | Footstep planning for the honda ASIMO humanoid[END_REF]. For the static step set U S CoM , we have used different lengths in the range [0.03, 0.08] m for forward and backward steps, and [0.01, 0.03] m for lateral steps. A composition of the two is used for diagonal steps. All these steps have a fixed duration of 2 s. The dynamic set U D CoM is composed by a starting step with length 0.038 m and duration 1.6 s, a cruise step with length 0.04 m and duration 0.425 s and a stopping step with length 0.038 m and duration 1.325 s, all in the forward direction. A total of 14 CoM movement primitives were used. In motion generation (4) we use K = diag{2, 2, 1} and a norm bound at 0.4 rad/sec on the null-space component.

We present two planning scenarios (see the accompanying video for details). In the first, the humanoid must pick up a ball and move it from one end of a long table to the other (Fig. 3). The initial task path y [0] is a simple line segment in task space, obtained as a B-spline with only the endpoints as control points. The initial time history is set to s [0] = t with t i = 0 s and t f = 17.2 s. The constrained motion planner is not able to find a solution for the initial task path: after covering less than one third of it, further expansion fails repeatedly due to joint limit violations as the robot extends its upper body and arm in order to stay on the path while avoiding collisions between its legs and the table. The robotbased deformation heuristic is then automatically activated and a new control point is generated that brings the task path closer to the robot. The constrained motion planner is then able to find a feasible solution.

In the second scenario, the robot must place a ball on a small round table (Fig. 4). As before, the initial task path y [0] is a line segment in task space and the time history is s [0] = t, with t i = 0 s and t f = 13.3 s. Once again, the constrained motion planner cannot find a solution for the initial task path: this is due to the stool located halfway, which prevents further expansion of the exploration tree due to repeated collisions with the left leg of the humanoid. As a consequence, obstacle-based deformation is automatically triggered and a new control point is generated that pushes the task path away from the stool. This allows the constrained motion planner to identify a feasible solution.

These results show that our simple policy for selecting the deformation heuristic is effective. Indeed, it is easy to realize that obstacle-based deformation would not work in the first scenario (the task path would move away from the table in the vertical direction, and hence further away from the robot), and conversely robot-based deformation would not work in the second scenario (the task path would be pushed towards the robot, and hence even closer to the stool).

Note that the final whole-body motion in both experiments is composed by different kinds of CoM movement primitives: dynamic steps are chosen for the walking phases, whereas free CoM is used for picking up and releasing the ball in double support. We emphasize that this natural choice is automatically made by the planner.

Table I collects some averaged performance figures for the two scenarios: planning time until task deformation is activated, time needed for path deformation and planning time on the deformed task, plus the number of nodes in the final tree and the motion duration.

VII. CONCLUSIONS

We have presented a two-level framework aimed at solving task-constrained motion planning problems for humanoid robots. The task is assigned as a deformable trajectory in task space, a feature that the planner may exploit if this is

TABLE I PLANNER PERFORMANCE AT A GLANCE

considered convenient for finding a solution. Our planner automatically detects when the task path must to be deformed and selects one of two heuristic deformation actions, i.e., obstacle-based and robot-based. Results obtained in V-REP for the humanoid robot NAO show that the proposed method performs successfully in a variety of scenarios.

We are currently working to improve this work in several directions, including an extension to on-line replanning, in which the nominal plan must be adapted at runtime to cope with dynamic scenarios. A preliminary result of this extension is included in the accompanying video.

Another interesting development would be to allow deformation (rather than simple adaptation) on the time history as well. For example, this may allow to solve difficult planning problems where the main reason for failed expansions is violation of joint velocity bounds (rather that collision or violation of joint limits) by slowing down the motion.

Algorithm 1 :

 1 Planner 1 sol found ← false; i ← 0; 2 get the initial task path y [0] and time history s [0] ; 3 repeat 4 build the current task trajectory y

11 end 12 i

 1112 ← i + 1; 13 until sol found = true or i = MAX DEFORM;

  t k+1 = t f or ỹ.exp fail = MAX FAIL or j = MAX IT; 14 return [T , ỹ];

7 /Fig. 1 .Fig. 2 .

 712 Fig. 1. Obstacle-based deformation heuristic. For the sake of clarity, only the obstacle point closest to the limit task point is shown.

Fig. 3 .

 3 Fig. 3. Planning experiment 1: Snapshots from a solution. Dotted line: initial task path; solid line: final task path.

Fig. 4 .

 4 Fig. 4. Planning experiment 2: Snapshots from a solution. Dotted line: initial task path; solid line: final task path.

  1 pick from (3) a random CoM primitive u k CoM of duration T k ; 2 compute the associated CoM trajectory z d CoM and swing foot trajectory z d swg ; 3 extract the portion of task trajectory y d in [t k , t k + T k ]; 4 build the augmented task y a = (y, zCoM, zswg) ;

	5 repeat
	6	generate motion by integrating joint velocities (4);
	12	end
	13	return [∅, ∅, ∅, ỹ];
	14	else if joint limit/velocity bound violation then
	15	if joint limit violation then
	16	if y d (t k ) = ỹ then
	17	// (expanding from a limit configuration);
	20	end
	21	end
	22	return [∅, ∅, ∅, ỹ];
	23	end
	24 until t = t k + T k ;
	25 if new limit task point reached then
	27

7 if collision then 8 if y d (t k ) = ỹ then 9 // (expanding from a limit configuration); 10 ỹ.exp fail ← ỹ.exp fail + 1; 11 ỹ.coll fail ← ỹ.coll fail + 1; 18 ỹ.exp fail ← ỹ.exp fail + 1; 19 ỹ.jnt fail ← ỹ.jnt fail + 1; 26 ỹ ← y d (t k + T k ) // (update the limit task point);

The reader may argue that no trajectory at all is needed in this situation, as one may simply specify the final task value as a set-point. However, it should be realized that even in this case the use of a pseudoinversebased kinematic control method (by itself or within a planner) implicitly determines which trajectory will be followed in task space. For example, it is easy to verify that using diagonal gain matrices for error feedback yields a rectilinear trajectory to the set-point in task space.

Since we are addressing a planning problem, it will be assumed that the initial configuration q(t i ) is assigned, and that the corresponding task value matches the starting point of the trajectory, i.e., y[0] (t i ) = f (q(t i )).

For the sake of illustration, we consider a catalogue that contains only stepping primitives, plus a non-stepping primitive. Other CoM movements may be obviously added to improve the versatility of the planner.

With a slight abuse of notation, in this section (and in Algorithm 3) y[i] stands for the path y[i] (s), s ∈ [s i , s f ], rather than the full trajectory.

Algorithm 3: TaskDeformation(y [i] , s [i] , T , ỹ)

// (robot-based heuristic); 3 retrieve a limit configuration q from the tree T ; 4 compute the line joining the limit task point ỹ to the CoM at qCoM ;