Optimal whole-body motion planning of humanoids in cluttered environments

Due to high degrees-of-freedom of humanoids and induced redundancy, there are multiple ways of performing a given manipulation task. Finding optimal ways of performing tasks is one desirable property of any motion planning framework. This includes optimizing both the path with respect to a certain objective function and also the final pre-grasp or goal position. Additionally, a variety of constraints need to be satisfied such as stability, self-collision and collision with objects in the environment and also kinematic loop-closure formed during the task. In this paper, an asymptotically optimal sampling based approach for generating motion plans is presented. A novel constraint solver extension to the bidirectional Fast Marching Trees (BFMT*) algorithm in the form of a way-point generator is proposed such that it can be applied for whole-body motion planning of humanoids. Moreover, a comparison of the performance of the proposed extension of BFMT* and the state-of art RRT* based motion planner is shown. A gradient based inverse kinematics solver has also been implemented in combination with an optimization technique to generate goal configurations in order to ensure optimality in the pre-grasp position. The efficacy of the proposed approach is evaluated in a simulation environment on Hubo(+) robot model. The results show a significant improvement in path costs, as well as overall optimality of given tasks for the proposed approach. Additionally, rigorous analysis over the choice of planning algorithms considered in this paper is present for the considered scenarios. (C) 2019 Elsevier B.V. All rights reserved.