Motion planning with sequential convex optimization and convex collision checking

TLDR

CHOMP is a known optimization-based motion planner that can generate collision‑free trajectories from straight‑line initializations that may be in collision. The study introduces TrajOpt, a new optimization‑based motion‑planning algorithm, and evaluates its performance against CHOMP and randomized OMPL planners. TrajOpt employs a sequential convex optimization loop that penalizes collisions with a hinge loss and incrementally increases penalty coefficients, coupled with an efficient continuous‑time collision constraint, and is tested on 7‑DOF arms, 18‑DOF robots, a 34‑DOF humanoid, and steerable needle trajectories. Source code, videos, and additional details are available at http://rll.berkeley.edu/trajopt/ijrr.

Abstract

We present a new optimization-based approach for robotic motion planning among obstacles. Like CHOMP (Covariant Hamiltonian Optimization for Motion Planning), our algorithm can be used to find collision-free trajectories from naïve, straight-line initializations that might be in collision. At the core of our approach are (a) a sequential convex optimization procedure, which penalizes collisions with a hinge loss and increases the penalty coefficients in an outer loop as necessary, and (b) an efficient formulation of the no-collisions constraint that directly considers continuous-time safety Our algorithm is implemented in a software package called TrajOpt. We report results from a series of experiments comparing TrajOpt with CHOMP and randomized planners from OMPL, with regard to planning time and path quality. We consider motion planning for 7 DOF robot arms, 18 DOF full-body robots, statically stable walking motion for the 34 DOF Atlas humanoid robot, and physical experiments with the 18 DOF PR2. We also apply TrajOpt to plan curvature-constrained steerable needle trajectories in the SE(3) configuration space and multiple non-intersecting curved channels within 3D-printed implants for intracavitary brachytherapy. Details, videos, and source code are freely available at: http://rll.berkeley.edu/trajopt/ijrr .