Dynamic Collision-Free Motion Planning for Robotic Manipulation using Graphs of Convex Sets

Abstract

Enabling robots to generates physically feasible and collision-frees trajectories is a fundamental problem in robotics. Current solutions take one of two approaches, using sampling based motion planners to probabilistically find a path between obstacles, or using trajectory optimization to exactly handle the dynamic constraints of the robot. The sampling based motion planners can handle the messy problem of planning a configuration-space trajectory in the presence of task-space obstacles despite the nonlinear mapping between the two spaces. However, they struggle as the dimension of the robot's configuration space grows due to the curse of dimensionality and cannot handle dynamic constraints directly. Meanwhile, trajectory optimization can handle the nonlinear dynamics and scales well to high degree of freedom robots, but the collision avoidance constraints make the optimization difficult, requiring extensive solve times or good initialization. We present a motion planning pipeline that seeks to fill the gap between these two approaches. The pipeline starts by decomposing the free-space into convex collision-free regions of the configuration space using Iterative Regional Inflation by Semidefinite & Nonlinear Programming (IRIS-NP). These regions can then be planned between using Graph of Convex Sets (GCS) Trajectory Optimization to create smooth collision-free trajectories. These trajectories can be made dynamically feasible using existing time parametrization algorithms, such as Time Optimal Path Parameterization by Reachability Analysis (TOPP-RA). Finally, we demonstrate how GCS Trajectory Optimization can be expanded to plan sequential trajectories using multi-modal planning where multiple interconnected graphs are planned through. We validate our algorithms performance on a variety of robot platforms and tasks, demonstrating that they serve as a foundation for future work in collision-free motion planning.