Dual Fast Marching Tree Algorithm for Human-Like Motion Planning of Anthropomorphic Arms With Task Constraints

This article presents a dual fast marching tree algorithm that consists of constrained fast marching tree planning in a Cartesian space (C_FMT
^∗
) and human-like fast marching tree planning in self-motion manifolds (H_FMT
^∗
) for human-like motion planning for anthropomorphic arms with task constraints. The key idea of the proposal is to exploit dual sampling in a Cartesian space and self-motion manifolds to explore the entire configuration space. The C_FMT
^∗
reduces the constrained planning problem to the unconstrained instance by sampling in the obstacle-free Cartesian space and satisfying the task constraints; it can solve most of these constrained path planning tasks quickly and obtain lower cost solutions compared to the existing techniques. In addition, a validity checking method of Cartesian sampling points based on self-motion manifolds is introduced to ensure the probabilistic completeness of the new planner. By analyzing musculoskeletal models of the human arm and the muscle strength property, a torque effort criterion was deduced to generate biomimetic motion for anthropomorphic arms. Then, an H_FMT
^∗
that incorporates the FMT
^∗
algorithm with the torque effort criterion is also proposed and used to bias the tree growth toward human-like movements in the self-motion manifolds of the obtained path. Finally, the proposed approach has been illustrated with several real examples executed with a humanoid robot. The obtained results show that the paths obtained with the proposed approach are quicker and more human-like.