Accurate evaluation of a distance function for optimization-based motion planning

We propose three novel methods to evaluate a distance function for robotic motion planning based on semiinfinite programming (SIP) framework; these methods include golden section search (GSS), conservative advancement (CA) and a hybrid of GSS and CA. The distance function can have a positive and a negative value, each of which corresponds to the Euclidean distance and penetration depth, respectively. In our approach, each robot's link is approximated and bounded by a capsule shape, and the distance between some selected link pairs is continuously evaluated along the joint's trajectory, provided by the SIP solver, and the global minimum distance is found. This distance is fed into the SIP solver, which subsequently suggests a new trajectory. This process is iterated until no negative distance is found anywhere in the links of the robot.We have implemented the three distance evaluation methods, and experimentally validated that the proposed methods effectively and accurately find the global minimum distances to generate a self-collision-free motion for the HRP-2 humanoid robot. Moreover, we demonstrate that the hybrid method outperforms other two methods in terms of computational speed and reliability.