On-Manifold Strategies for Reactive Dynamical System Modulation With Nonconvex Obstacles

In this work, we present a novel, reactive, modulated control strategy based on dynamical systems (DS) for planning in the context of multiple nonconvex obstacles. Our DS modulation strategy leverages an on-manifold planning methodology and provides several methods for real-time on-manifold navigation around nonconvex obstacles. We introduce a sample-based obstacle representation for complex, nonconvex obstacles, as well as a projection-based method for representing surfaces, such as tables, cylinders, and ellipsoids. These representations can be combined to represent multiple obstacles and obstacle types (sample- or projection-based) with a single, continuously differentiable function. We validate our approach in several real-world scenarios, including navigation within (simulated) constrained environments, as well as reactive control of a real 7-DoF manipulator with dynamic obstacles (including humans) while utilizing a 1 kHz control loop rate. Using our sample-based representation, we can calculate the obstacle representation function in less than 1 ms with up to 35 k points using a CPU implementation, and up to 600 k points with a GPU implementation.