Multi-Modal Manipulation Planning for an Upper-Torso Humanoid System

Motion planning problems for manipulators are often considered and planned on a single submanifold. To solve a more challenging manipulation task, the manipulator needs to use both prehensile and non-prehensile manipulation actions to solve the task. Non-Prehensile manipulation actions operate on a lower dimension compared to prehensile manipulation actions. In this work, we propose a multi-modal motion planner for tree kinematic structure robotic systems, which plans through different submanifolds to provide a continuous trajectory to fulfill the given task. As both manipulators share the same torso joints, a movement involving the torso and one of the manipulators often causes an undesired behavior in the second manipulator, such as being in a collision state or an object that cannot be received from the second manipulator. A multi-task propagation function is developed to find optimal joint angles for the tree kinematic structure while propagating on the submanifolds of the planning space. This planner is evaluated and tested on an upper-torso humanoid system in two different scenarios. The results show that our planner achieved a higher success rate and needs less planning time compared to a planner which uses dual-arm manipulators mounted on a rigid body.