In this paper, we propose a novel inverse Dynamic Reachability Map (iDRM) that allows a floating base system to find valid end-poses in complex and dynamically changing environments in real-time. End-pose planning for valid stance pose and collision-free configuration is an essential problem for humanoid applications, such as providing goal states for walking and motion planners. However, this is non-trivial in complex environments, where standing locations and reaching postures are restricted by obstacles. Our proposed iDRM customizes the robot-to-workspace occupation list and uses an online update algorithm to enable efficient reconstruction of the reachability map to guarantee that the selected end-poses are always collision-free. The iDRM was evaluated in a variety of reaching tasks using the 38 degree-of-freedom (DoF) humanoid robot Valkyrie. Our results show that the approach is capable of finding valid end-poses in a fraction of a second. Significantly, we also demonstrate that motion planning algorithms integrating our end-pose planning method are more efficient than those not utilizing this technique.
Planning balanced and collision-free motion for humanoid robots is non-trivial, especially when they are operated in complex environments, such as reaching targets behind obstacles or through narrow passages. We propose a method that allows us to apply existing sampling--based algorithms to plan trajectories for humanoids by utilizing a customized state space representation, biased sampling strategies, and a steering function based on a robust inverse kinematics solver. Our approach requires no prior offline computation, thus one can easily transfer the work to new robot platforms. We tested the proposed method solving practical reaching tasks on a 38 degrees-of-freedom humanoid robot, NASA Valkyrie, showing that our method is able to generate valid motion plans that can be executed on advanced full-size humanoid robots. We also present a benchmark between different motion planning algorithms evaluated on a variety of reaching motion problems. This allows us to find suitable algorithms for solving humanoid motion planning problems, and to identify the limitations of these algorithms.