Sampling-based Global Path Planning using Convex Polytope Approximation for Narrow Collision-free Space of Humanoid

In this paper, we propose a computationally fast method of sampling-based global path planning for humanoids under Manifold Constraints such as closed kinematic chains and Volume-Reducing Constraints such as collision avoidance. In multi-contact and whole-body manipulation of humanoids, narrow collision-free space causes path planning to take a long computation time (narrow corridor problem). In previous research of constrained planning, Manifold Constraints are locally approximated by tangent plane, and steering motions along Manifold Constraints are found efficiently by projection or continuation methods. In this paper, we applied constrained planning algorithms to collision avoidance. Since tangent plane cannot approximate Volume-Reducing Constraints, we adopted convex polytope instead of tangent plane. Both Manifold Constraints and Volume-Reducing Constraints are locally approximated by convex polytope with linear equality and inequality constraints, and steering motions inside the convex polytope are found efficiently by the SQP-based prioritized inverse kinematics. We developed CP-KPIECE (Convex Polytope approximation-based KPIECE) with this approach. Benchmarks using the humanoid JAXON proved the effectiveness of our approach for fast path planning in narrow collision-free space of humanoids.