Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization

Abstract

Abstract-We present a novel approach for incorporating collision avoidance into trajectory optimization as a method of solving robotic motion planning problems. At the core of our approach are (i) A sequential convex optimization procedure, which penalizes collisions with a hinge loss and increases the penalty coefficients in an outer loop as necessary. (ii) An efficient formulation of the no-collisions constraint that directly considers continuous-time safety and enables the algorithm to reliably solve motion planning problems, including problems involving thin and complex obstacles.We benchmarked our algorithm against several other motion planning algorithms, solving a suite of 7-degree-of-freedom (DOF) arm-planning problems and 18-DOF full-body planning problems. We compared against sampling-based planners from OMPL, and we also compared to CHOMP, a leading approach for trajectory optimization. Our algorithm was faster than the alternatives, solved more problems, and yielded higher quality paths.Experimental evaluation on the following additional problem types also confirmed the speed and effectiveness of our approach: (i) Planning foot placements with 34 degrees of freedom (28 joints + 6 DOF pose) of the Atlas humanoid robot as it maintains static stability and has to negotiate environmental constraints.(ii) Industrial box picking. (iii) Real-world motion planning for the PR2 that requires considering all degrees of freedom at the same time.

Autonomous multilateral debridement with the Raven surgical robot

“…We use trajopt [32], a low-level motion planning algorithm based on sequential convex optimization to plan locallyoptimal, collision-free trajectories simultaneously for both arms. An important feature of trajopt is the ability to check continuous collisions: the arm shafts are very narrow, which could allow them to pass through each other between points on the path.…”

Learning from Demonstrations Through the Use of Non-rigid Registration

“…In particular, after decomposing a pose T into translation p and quaternion rotation q, the error vector is simply given by (p x , p y , p z , q x , q y , q z ). We solve this problem using the trajectory optimization method from [21]. We initialize with the joint trajectory from the demonstration, which is in roughly the right part of configuration space.…”

A general technique for fast comprehensive multi-root planning on graphs by coloring vertices and deferring edges

“…While single-query sampling-based algorithms such as the RRT [5], [6] generally consider a single start and goal state, extensions [7] have allowed such planners to consider both multiple starts and multiple goals. Trajectory optimization can also be applied to the motion planning problem; while typically considered for single start and goal states, recent generalizations [8], [9] have extended them to sets of starts or goals.…”