Consider the problem of planning the motion of a humanoid robot that must execute a manipulation task, possibly requiring stepping, in environments cluttered by obstacles. This is a particularly challenging problem, for a number of reasons. The first is the high number of degrees of freedom. Second, a humanoid robot is not a free-flying system in its configuration space: its motion must be generated appropriately. Moreover, the task constraint drastically reduces the size of the admissible planning space. Finally, the implicit requirement that the robot maintains equilibrium, either static or dynamic, typically constrains the trajectory of the robot center of mass.
We propose a solution approach that - contrary to large part of the literature - does not separate locomotion from task execution. In particular, using a previously proposed task-constrained planning framework, the proposed method explores the submanifold of the configuration space that is admissible with respect to tasks and all other constraints, including humanoid equilibrium. Expansion of the search tree within the constrained manifold is obtained through a hybrid scheme that generates simultaneously feet positions and whole-body motions, which are then validated via collision checking.
The algorithm has been implemented for the humanoid robot NAO and validated through planning experiments and dynamic playback in V-REP.
The proposed planner has been implemented in V-REP, a cross-platform software by Coppelia Robotics, suitable also for motion planning. We report two scenarios in which the right hand is used as end-effector for the task trajectory.
Pick and place
In the first scenario, the robot has to grasp a ball from the lowest shelf of a bookcase and place it on the upper shelf.
A few snapshots from a solution computed by our planner are shown above. The robot moves from its start configuration at t = 0 s and it has to take several steps before grasping the ball at t = 9 s and placing it on the upper shelf at t = 15.5 s. In this case, the planner has chosen to perform the actual pick and place operation entirely in double support; note how collisions with the bookcase are carefully avoided. Once the ball is released, the robot takes a last step forward and successfully completes its task at t = 18 s.
Opening a door
In the second scenario, the robot must open a door. In this case, a composite trajectory was assigned to the robot hand to first approach the knob and then pull it along an arc of circle.
Above is a selection of snapshots from a solution found by our planner for this problem. At the start, NAO starts approaching the knob with its right hand. Knob grasping occurs at t = 1.5 s. Since its starting configuration is very close to the door, the robot takes a few backward steps between t = 1.5 s and t = 7 s in order to be able to open the door without colliding with it. Note how NAO continues opening the door while stepping back - the robot does not need to stop walking to perform manipulation. At t = 7 s, the door opening phase ends. Then, the robot releases the knob and takes one more backward step.
This clip explains the working principle of the planner and shows dynamic simulations of the plans generated for the two above scenarios.
 M. Cognetti, P. Mohammadi, G. Oriolo, M. Vendittelli, Task-Oriented Whole-Body Planning for Humanoids based on Hybrid Motion Generation. 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2014), Chicago, IL, 2014 (pdf).