A Homotopy Method for Motion Planning

We propose a novel method for motion planning and illustrate its implementation on several canonical examples. The core novel idea underlying the method is to define a metric for which a path of minimal length is an admissible path, that is path that respects the various constraints imposed by the environment and the physics of the system on its dynamics. To be more precise, our method takes as input a control system with holonomic and non-holonomic constraints, an initial and final point in configuration space, a description of obstacles to avoid, and an initial trajectory for the system, called a sketch. This initial trajectory does not need to meet the constraints, except for the obstacle avoidance constraints. The constraints are then encoded in an inner product, which is used to deform (via a homotopy) the initial sketch into an admissible trajectory from which controls realizing the transfer can be obtained. We illustrate the method on various examples, including vehicle motion with obstacles and a two-link manipulator problem.