Finding Locally Optimal, Collision-FreeTrajectories with Sequential Convex Optimization

John Schulman, Jonathan Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow and Pieter Abbeel

Abstract—We present a novel approach for incorporatingcollision avoidance into trajectory optimization as a method ofsolving robotic motion planning problems. At the core of ourapproach are (i) A sequential convex optimization procedure,which penalizes collisions with a hinge loss and increases thepenalty coefficients in an outer loop as necessary. (ii) An efficientformulation of the no-collisions constraint that directly considerscontinuous-time safety and enables the algorithm to reliably solvemotion planning problems, including problems involving thin andcomplex obstacles.

We benchmarked our algorithm against several other mo-tion planning algorithms, solving a suite of 7-degree-of-freedom(DOF) arm-planning problems and 18-DOF full-body planningproblems. We compared against sampling-based planners fromOMPL, and we also compared to CHOMP, a leading approachfor trajectory optimization. Our algorithm was faster than thealternatives, solved more problems, and yielded higher qualitypaths.

Experimental evaluation on the following additional problemtypes 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 maintainsstatic stability and has to negotiate environmental constraints.(ii) Industrial box picking. (iii) Real-world motion planning forthe PR2 that requires considering all degrees of freedom at thesame time.

I. INTRODUCTION

Trajectory optimization algorithms have two roles in roboticmotion planning. First, they can be used to smooth and shortentrajectories generated by some other method. Second, they canbe used to plan from scratch: one initializes with a trajectorythat contains collisions and perhaps violates constraints, andone hopes that the optimization converges to a high-qualitytrajectory satisfying constraints. Using an optimization algo-rithm to plan from scratch is an especially attractive optionin problems with many degrees of freedom (DOF), since thecomputation time scales favorably with the number of DOF.

Two of the key ingredients in trajectory optimization formotion planning are (1) the numerical optimization method,and (2) the method of checking for collisions and penalizingthem. For numerical optimization, we use sequential convexoptimization, with `1 penalties for equality and inequalityconstraints. This approach involves solving a series of con-vex optimization problems that approximate the cost andconstraints of the true problem, which is non-convex. Forcollisions, we compute signed distances using convex-convexcollision detection, and we ensure the continuous-time safetyof a trajectory by considering the swept-out volume. These twoaspects of our approach are complementary, since our collisionchecking method yields a polyhedral approximation of the free

Saturday, February 2, 13

Fig. 1. Several problem settings were we have used our algorithm for motionplanning. Top left: planning an arm trajectory for the PR2 in simulation, in abenchmark problem. Top right: PR2 opening a door with a full-body motion.Bottom left: industrial robot picking boxes, obeying an orientation constrainton the end effector. Bottom right: humanoid robot model (DRC/Atlas) duckingunderneath an obstacle while obeying static stability constraints.

part of configuration space, which can be directly incorporatedinto the convex optimization problem that is solved at eachiteration of the optimization.

The first advantage of our approach is speed. Our imple-mentation solves typical arm planning problems in around100 − 200 ms and solves problems involving many moredegrees of freedom in under a second. This is largely enabledby our novel formulation of the the collision penalty, whichguarantees safety in continuous time by considering swept-out volumes. This cost formulation has little computationaloverhead in collision checking and allows us to use a sparselysampled trajectory. The second advantage of our approachis its reliability—it solves a surprisingly large fraction ofplanning problems. In our experiments, our algorithm solveda larger fraction of problems than any of the sampling-basedplanners, which were given a ten second time limit. Thethird advantage of our approach regards path quality: oncethe trajectory is free of collisions, our approach will treatcollision avoidance as a hard constraint (i.e., keep a certainsafe distance from obstacles.) Our algorithm will converge toa locally optimal solution subject to this constraint, withoutcompromising the other objective criteria. The fourth advan-tage of our approach is flexibility: new constraints and cost

terms can easily be added to the problem since the underlyingnumerical optimization method is numerically robust, and itcan deal with initializations that are deeply infeasible.

We performed a quantitative comparison between our al-gorithm and several open-source implementations of motionplanning algorithms, including sampling based planners fromOMPL [26], as well as a recent implementation of CHOMP.Overall, our algorithm was not only faster than the alternatives,but it solved a larger fraction of the problems. (All plannerswere given a ten second time limit.)

We have successfully used our algorithm on high-DOFproblems in the real world involving a mobile robot (PR2)and sensor data. We solve problems where we simultaneouslyneed to plan for two arms along with the base and torso.

We also validated our approach on a very high-DOF prob-lem: planning foot placements with 28 degrees of freedom (+6DOF pose) of the Atlas humanoid robot as it maintains staticstability and avoid collisions.

Videos corresponding to the results described here areavailable at the website [2].

II. RELATED WORK

Optimizing over trajectories is one of the fundamental ideasof optimal control, especially the direct methods, which solvefor a sequence of states and controls [3]. In the domainof robotics, the set of collision-free configurations is highlynon-convex, and collision checking is usually computation-ally expensive, making trajectory optimization challenging.Khatib proposed the use of potential fields for avoidingobstacles, including moving ones [13]. Warren [30] suggestedusing a global potential field to push the robot away fromconfiguration-space obstacles, starting with a trajectory thatcontained collisions. Quinlan and Khatib [24] suggested lo-cally approximating the free part of configuration space as aunion of spheres around the current trajectory as part of alocal optimization that tries to shorten the trajectory. Brockand Khatib [4] improved on this idea, enabling trajectoryoptimization on paths of a robot in 3D, by using the Jacobianto map distances from task space into configuration space.

CHOMP (Covariant Hamiltonian Optimization for MotionPlanning) is a set of ideas for how to formulate the objective inrobotic motion planning problems and how to perform the nu-merical optimization [25, 32, 9]. The most notable features oftheir approach are (1) using trajectory costs that are invariant totime parameterization of the trajectory, (2) using pre-computedsigned distance fields for collision checking, (3) using pre-conditioned gradient descent for numerical optimization, withprojections to enforce constraints. While the motivation forthe presented work is very similar to the motivation behindCHOMP (and, indeed, CHOMP is the most closely relatedprior art), our algorithm differs fundamentally in many ways,and we will discuss the relative merits between our approachand CHOMP in the Discussion section.

Other recent work on robot trajectory optimization for (typi-cally) kinematic motion planning includes STOMP (StochasticTrajectory Optimization for Motion Planning), which uses a

gradient-free, stochastic scheme for optimization [12]; andITOMP (Incremental trajectory optimization for real-time re-planning in dynamic environments), which deals with dynamicobstacles and real-time replanning [22].

Some recent work in robotics uses sequential quadraticprogramming for trajectory optimization and incorporates col-lision avoidance as constraints, in a similar way to this work.Lampariello et al. [15] incorporate signed distances betweenpolytopes as inequality constraints in an optimal control prob-lem. Unlike this work, they don’t consider continuous-timecollision checking or deal with trajectories that start deeply incollision (for which we resort to a penalty method.) Werner etal. use sequential quadratic programming to optimize walkingtrajectories, also incorporating obstacle avoidance as hardconstraints, along with stability constraints [31].

Finally, there recently has been considerable progress intrajectory optimization in situations with contact, some of themost notable results include the ones described by Mordatchet al. [20], Posa et al. [23], and Tassa et al. [28].

III. BACKGROUND: SEQUENTIAL CONVEX OPTIMIZATION

Robotic motion planning problems can be formulated asnon-convex optimization problems, i.e., minimize an objectivesubject to inequality and equality constraints:

minimize f(x) (1)subject to (2)gi(x) ≤ 0, i = 1, 2, . . . , nineq (3)hi(x) = 0, i = 1, 2, . . . , neq (4)

where f, gi, hi, are scalar functions.In kinematic motion planning problems, the optimization is

done over a T×K-dimensional vector, where T is the numberof time-steps and K is the number of degrees of freedom.Henceforth, we will denote the optimization variables as θ1:T ,where θt describes the configuration at the tth timestep. Toencourage minimum-length paths, we use the sum of squareddisplacements,

f(θ1:T ) =

T−1∑t=1

‖θt+1 − θt‖2. (5)

In problems with dynamics, the optimization might also in-clude joint torques and contact forces. This paper just con-siders kinematic problems, but the methods developed can bestraightforwardly extended to find collision-free paths in prob-lems with dynamic constraints. Besides obstacle avoidance,common inequality constraints in motion planning problemsinclude joint limits (which are simply bound constraints onthe variables), speed limits (in Cartesian space or joint space),and static stability constraints. Common equality constraintsinclude the end-effector pose (i.e., reach a target pose at theend of the trajectory) and orientation constraints (keep a heldobject upright). We will discuss some of these constraints inSection VII.

Sequential convex optimization solves a non-convex op-timization problem by repeatedly constructing a convex

subproblem—an approximation to the problem around thecurrent iterate x. The subproblem is used to generate a step∆x that makes progress on the original problem. Two keyingredients of a sequential convex optimization algorithm areas follows: (1) a method for constraining the step to be small,so the solution vector remains within the region where the ap-proximations are valid; (2) a strategy for turning the infeasibleconstraints into penalties, but eventually ensuring that all ofthe constraint violations are driven to zero. For (1), we use atrust region (a box constraint around the current iterate). For(2) we use `1 penalties: each inequality constraint gi(x) ≤ 0becomes the penalty |gi(x)|+, where |x|+ = max (x, 0); eachequality constraint hi(x) = 0 becomes the absolute valuepenalty |hi(x)|. In both cases, the penalty is multiplied bysome coefficient µ, which is adjusted during the optimizationto ensure that the constraint violation is driven to zero. Notethat `1 penalties are non-differentiable but convex, and convexoptimization algorithms can efficiently minimize them. Ourimplementation uses a variant of the classic `1 penalty method[21], which is described in Algorithm 1.

In the outer loop (PenaltyIteration) we increase the penaltycoefficient µ until all the constraints are satisfied, terminat-ing when the coefficient exceeds some threshold. The nextloop (ConvexifyIteration) is where we repeatedly constructa convex approximation to the problem and then optimizeit. In particular, we approximate the objective and inequalityconstraint functions by convex functions that are compatiblewith a quadratic program (QP) solver, and we approximatethe nonlinear equality constraint functions by affine functions.The nonlinear constraints are incorporated into the problem aspenalties, while the linear constraints are directly imposed inthe convex subproblems. The next loop (TrustRegionIteration)is like a line search; if the true improvement (TrueImprove)to the non-convex merit functions (objective plus constraintpenalty) is a sufficiently large fraction of the improvement toour convex approximations (ModelImprove), then the step isaccepted. (Usually we only need one iteration.)

The approach of using `1 penalties is called an exact penaltymethod, because if we multiply the penalty by a high enoughcoefficient, then the minimizer of the penalized problem isexactly equal to the minimizer of the constrained problem.This is in contrast with the typical `2 penalty method thatpenalizes squared error, i.e., gi(x) ≤ 0 → (|gi(x)|+)2 andhi(x) = 0 → hi(x)2. `1 penalty methods give rise tonumerically-stable algorithms that drive the error to zero. Notethat this procedure updates the penalty coefficients in a genericway, so one does not need to tune them when setting up a newproblem.

Note that the objective we are optimizing contains non-smooth terms like |a ·x+b| and |a ·x+b|+ However, the sub-problems solved by our algorithm are quadratic programs—aquadratic objective subject to affine constraints. Using a well-known trick, we accommodate these non-smooth terms whilekeeping the objective quadratic by adding auxilliary (slack)variables. To add term term |a ·x+ b|+, we add slack variable

Algorithm 1 `1 penalty method for sequential convex opti-mization.Parameters:

µ0: initial penalty coefficients0: initial trust region sizec: step acceptance parameterτ+, τ−: trust region expansion and shrinkage factorsk: penalty scaling factorftol, xtol: convergence thresholds for merit and xctol: constraint satisfaction threshold

Variables:x current solution vectorµ penalty coefficients trust region size

1: for PenaltyIteration = 1, 2, . . . do2: for ConvexifyIteration = 1, 2, . . . do3: f , g, h = ConvexifyProblem(f, g, h)4: for TrustRegionIteration = 1, 2, . . . do

5: x← arg minx

f(x) + µ

nineq∑i=1

|gi(x)|+ + µ

neq∑i=1

|hi(x)|

subject to trust region and linear constraints6: if TrueImprove /ModelImprove > c then7: s← τ+ ∗ s . Expand trust region8: break9: else

10: s← τ− ∗ s . Shrink trust region11: if s < xtol then12: goto 1513: if converged according to tolerances xtol or ftol then14: break15: if constraints satisfied to tolerance ctol then16: break17: else18: µ← k ∗ µ

t and impose constraints

0 ≤ ta · x+ b ≤ t (6)

Clearly, at the optimal solution, t = |a · x+ b|+ Similarly, toadd the term |a · x + b|, we add s + t to the objective andimpose constraints

0 ≤ s, 0 ≤ ts− t = a · x+ b (7)

At the optimal solution, s = |a · x+ b|+, t = | − a · x− b|+,so s+ t = |a · x+ b|.

IV. DISCRETE-TIME NO-COLLISIONS CONSTRAINT

This paragraph introduces some notation. A,B,O are labelsfor rigid objects, each of which is a link of the robot oran obstacle. The set of points occupied by these objects aredenoted by calligraphic letters A,B,O ⊂ R3. We sometimesuse a superscript to indicate the coordinate system of a point

or a set of points. Aw ⊂ R3 denotes the set of points in worldcoordinates occupied by A, whereas AA denotes the set ofpoints in a coordinate system local to object A. The poses ofthe objects A,B are denoted as Fw

A , FwB , where Fw

A is a rigidtransformation that maps from the local coordinate system tothe global coordinate system.

Our method for penalizing collisions is based on the notionof minimum translation distance, common in collision detec-tion [10]. The distance between two sets A,B ⊂ R3, which isnonzero for non-intersecting sets, is defined as

dist(A,B) = inf{‖T‖∣∣ (T +A) ∩ B 6= ∅} (8)

Informally, it’s the length of the smallest translation T thatputs the shapes in contact. The penetration depth, which isnonzero for overlapping shapes, is defined analogously as theminimum translation that takes two shapes out of contact:

penetration(A,B) = inf{‖T‖∣∣ (T +A) ∩ B = ∅} (9)

The signed distance is defined as follows:

sd(A,B) = dist(A,B)− penetration(A,B) (10)

Note that these concepts can also be defined using the notionof a configuration space obstacle and the Minkowski differencebetween the shapes—see e.g. [10].

A

B

ABT

pA

pB

TpA

pB

sd > 0 sd < 0

Tuesday, January 29, 13

Fig. 2. Minimal translational distance and closest points.

The distance between two shapes can be calculated by theGilbert-Johnson-Keerthi (GJK) algorithm [11]. The penetra-tion depth is calculated by a different algorithm, the ExpandingPolytope Algorithm (EPA) [29]. One useful feature of thesetwo algorithms, which makes them so generally applicable, isthat they represent an object A by its support mapping, i.e., afunction that maps vector v to the point in A that is furthestin direction v:

sA(v) = arg maxx∈A

v · x (11)

This representation makes it possible to describe shapes im-plicitly without constructing polyhedra or explicit representa-tions of their surfaces. We will exploit this fact to efficientlycheck for collisions against swept-out volumes.

Two objects are non-colliding if the signed distance ispositive. We will typically want to ensure that the robot has

a safety margin dsafe. Thus, we want to enforce the followingconstraints at each timestep

sd(Ai,Oj) ≥ dsafe ∀i ∈ {1, 2, . . . , Nlinks},∀j ∈ {1, 2, . . . , Nobstacles}

(obstacle collisions)sd(Ai,Aj) ≥ dsafe ∀i, j ∈ {1, 2, . . . , Nlinks} (12)

(self collisions)

where {Ai} is the collection of links of the robot, and {Oj}is the set of obstacles.

These constraints can be relaxed to the following `1 penalty

Nlinks∑i=1

Nobs∑j=1

|dsafe − sd(Ai,Oj)|+

+

Nlinks∑i=1

Nlinks∑j=1

|dsafe − sd(Ai,Bj)|+ (13)

A single term of this penalty function is illustrated in Figure 3.After we linearize the signed distance (described below), thiscost can be incorporated into a quadratic program (or linearprogram) using the trick from Equation 6.

The collision penalty 13 looks prohibitively expensive toevaluate because of the double sum. However, most of theterms are zero and correspond to pairs of faraway objects,and our optimization does not explicitly represent these terms.The collision penalty (or equivalently, the constraint violationsin Equation 12) can be computed by querying a collisionchecker for all pairs of nearby objects in the world. To locallyapproximate this cost around the current iterate, we query thecollision checker for all pairs of objects with distance smallerthan dcheck between them, where dcheck > dsafe. It is importantfor convergence that dcheck is strictly greater than dsafe, so thatour local approximation to the cost function includes terms forpairs of objects that are currently safely out of collision (withzero penalty). This way, the local approximation is aware ofthis pair of nearby objects when generating a step, i.e. solvingthe QP subproblem.

penalty

dcheckdsafe0sd

Saturday, February 2, 13

Fig. 3. Hinge penalty for collisions

We can form a linear approximation to the signed distanceusing the robot Jacobian and the notion of closest points. Asimilar calculation is performed in dynamics simulations toresolve contact constraints. LetAA,BB ⊂ R3 denote the spaceoccupied by A and B in local coordinates, and let pA ∈ AA

and pB ∈ BB denote the local positions of contact points. FwA

and FwB denote the objects’ poses.

To define closest points and our derivative approximation,first note that the signed distance function is given by thefollowing formula, which applies to both the overlapping andnon-overlapping cases:

sd({A,FwA }, {B,Fw

B }) = max‖n‖=1

minpA∈A,pB∈B

n · (FwApA − Fw

BpB)

(14)

The closest points pA,pB and normal n are defined as a triplethat achieve the optimum described in (14). Equivalently, thecontact normal n is the direction of the minimal translation T(as defined in Equations (8) and (9)), and pA and pB are apair of points (expressed in local coordinates) that are touchingwhen we translate A by T . See Figure 2 for illustration.

Let’s assume that the pose of A is parameterized by vectorθ (e.g., the robot’s joint angles), and B is stationary. (Thiscalculation can be straightforwardly extended to the casewhere both objects vary with θ, which is necessary for dealingwith self-collisions.) Then we can linearize the signed distanceby assuming that the local positions pA,pB are fixed, and thatthe normal n is also fixed, in Equation (14).

We first linearize the signed distance with respect to thepositions of the closest points:

sdAB(θ) ≈ n · (FwA (θ)pA − Fw

BpB) (15)

By calculating the Jacobian of pA with respect to the degreesof freedom θ, we can linearize this signed distance expressionat θ0:

∇θ sdAB(θ)

∣∣∣∣θ0

≈ nTJpA(θ0)

sdAB(θ) ≈ sdAB(θ0) + nTJpA(θ0)(θ − θ0)

(16)

The above expression allows us to form a local approximationof one collision cost term with respect to the robot’s degrees offreedom. This approximation is used for every pair of nearbyobjects returned by the collision checker.

Note that this formula, which assumes that the normaln and the closest points are fixed, is correct to first orderin non-degenerate situations involving polyhedra. However,in degenerate cases involving face-face contacts, the signeddistance is non-differentiable as a function of the poses ofthe objects, and the above formula deviates from correctness.Empirically, the optimization does not seem to get stuck at thepoints of non-differentiability.

V. ENSURING CONTINUOUS-TIME SAFETY

The preceding section describes how to formulate a col-lision constraint or penalty that ensures that a given robotconfiguration θ is not in collision. We can use this constraintor penalty to ensure that the robot is collision-free at eachwaypoint of a discretely-sampled trajectory. These waypointswill need to be converted to a continuous-time trajectory, e.g.by linear interpolation or cubic splines. However, the resultingcontinuous-time trajectory might have collisions between thewaypoints—see Figure 4.

We can modify the collision penalty from Section IV togive a cost that enforces the continuous-time safety of thetrajectory (though it makes a geometric approximation). Itis only moderately more computationally expensive than thediscrete-time collision cost of the previous section.

T

B

A(t)

A(t+1)

Friday, February 1, 13

Fig. 4. Illustration of swept volume, which we use in our continuous collisioncost.

Consider a moving object A and a static object B, for0 ≤ t ≤ 1. The motion is free of collision if the swept-out volume ∪tA(t) does not intersect B. First suppose thatA undergoes only translation, not rotation. (We will considerrotations below.) Then the swept-out volume is the convex hullof the initial and final volumes [29]⋃

t∈[0,1]

A(t) = convhull(A(t),A(t+ 1)) (17)

Thus we can use the same sort of collision cost we describedin Section IV, but now we calculate the signed distancebetween the swept-out volume of A and the obstacle B:

sd(convhull(A(t),A(t+ 1)),B) (18)

It turns out that we don’t have to calculate the convexhull of shapes A(t), A(t+ 1) to perform the necessary signeddistance computation, since (as noted in Section IV) the signeddistance cost can be calculated using the support mappings.In particular, the support mapping is given by

sconvhull(C,D)(v) =

{sC(v) if sC(v) · v > sD(v) · vsD(v) otherwise

(19)

Calculating the gradient of the swept-volume collision costis slightly more involved than discrete case described inEquations (15) and (16). Let’s consider the case where objectA is moving and object B is stationary, as in Figure 4. Let’ssuppose that A and B are polyhedral. Then the closest pointpswept ∈ convhull(A(t), A(t + 1)) lies in one of the facesof this polytope. convhull(A(t), A(t+ 1)) has three types offaces: (1) all the vertices are from A(t), (2) all of the verticesare from A(t+ 1), and (3) otherwise. Cases (1) and (2) occurwhen the deepest contact in the interval [t, t+1] occurs at oneof the endpoints, and the gradient is given by the discrete-timeformula. In case (3), we have to estimate how the closest pointvaries as a function of the poses of A at times t and t+ 1.

We use an approximation for case (3) that is compu-tationally efficient and empirically gives accurate gradient

estimates. It is correct to first order in non-degenerate 2Dcases, but it is not guaranteed to be accurate in 3D. Letpswept, pB , denote the closest points and normals betweenconvhull(A(t), A(t+1)) and B, respectively, and let n be thenormal pointing from B into A.

1) Find supporting vertices p0 ∈ A(t) and p1 ∈ A(t+ 1)by taking the support map of these sets in the normaldirection −n.

2) Our approximation assumes that the contact point pswept

is a fixed convex combination of p0 and p1. In somecases, p0, pswept, and p1 are collinear. To handle theother cases, we set

α =‖p1 − pswept‖

‖p1 − pswept‖+ ‖p0 − pswept‖(20)

We make the approximation

pswept(θ) ≈ αp0 + (1− α)p1 (21)

3) Calculate the Jacobians of those points

Jp0(θt

0) =d

dθtp0, Jp1

(θt+10 ) =

d

dθt+1p1 (22)

4) Similarly to Equation 16, linearize the signed distancearound the trajectory variables at timesteps t and t+ 1

sdAB(θt,θt+1) ≈ sdAB(θt0,θ

t+10 )

+αnTJp0(θt

0)(θt − θt0)

+(1− α)nTJp1(θt+10 )(θt+1 − θt+1

0 )

(23)

The preceding discussion assumed that the shapes undergotranslation only. However, the robot’s links also undergorotation, so the convex hull will underestimate the swept-outvolume. This phenomenon is illustrated in Figure 5. We cancalculate a simple upper-bound to the swept-out volume, basedon the amount of rotation. Consider a shape A undergoingtranslation T and rotation angle φ around axis k in localcoordinates. Let A(t) and A(t + 1) be the occupied spaceat the initial and final times, respectively. One can show thatif we expand the convex hull convhull(A(t), A(t + 1)) bydarc = rφ2/8, where r is the maximum distance from a pointon A to the local rotation axis, then the swept-out volume iscontained inside.

In summary, we can ensure continuous time safety byensuring that for each time interval [t, t+ 1]

sd(convhull(A(t),A(t+ 1)),O) > dsafe + darc (24)

One could relax this constraint into a penalty as describedin Section IV, by approximating φ(θt,θt+1). In practice, weignored the correction darc, since it was well under 1 cm in allof the problems we considered.

The method described in this section for continuous-timecollision detection only has a modest performance penaltyversus the discrete-time collision detection, where the slow-down is because we have to calculate the support mappingof a convex shape with twice as many vertices. As a result,the narrow-phase collision detection takes about twice aslong. The upshot is that the continuous collision cost solves

𝑑

𝑟

Fig. 5. Illustration of the difference between swept out shape and convexhull. The figure shows a triangle undergoing translation and uniform rotation.The swept-out area is enclosed by dotted lines, and the convex hull is shownby a thick gray line.

problems with thin obstacles where the discrete-time cost failsto get the trajectory out of collision. An added benefit is thatwe can ensure continuous-time safety while parametrizing thetrajectory with a small number of waypoints, reducing thecomputational cost of the optimization.

VI. MOTION PLANNING BENCHMARK

Fig. 6. Scenes in our benchmark tests. Left and center: two of the scenesused for the arm planning benchmark. Right: a third scene, showing the pathfound by our planner on an 18-DOF full-body planning problem.

We compared our algorithm to several other motion plan-ning algorithms on a collection of problems in simulatedenvironments. Our evaluation is based on four test scenesincluded with the MoveIt! distribution that is part of the ROSmotion planning libraries [5, 7]. We used the bookshelves,countertop, industrial, and tunnel scenes for the evaluationbecause they were the most complex. The set of planningproblems was created as follows. For each scene we set upthe robot in a number of diverse configurations. Each pair ofconfigurations yields a planning problem. We assume that theend configuration is fixed, as opposed to some other constraintlike the gripper pose.

Our tests include 198 arm planning problems and 96 full-body problems. We compared to the top-performing plan-ning algorithms from OMPL / MoveIt. They include a bi-directional RRT [14] and a variant of KPIECE [27]. All ofthese algorithms were run using default parameters and post-processed by the default smoother used by MoveIt. We alsocompared to the latest implementation of CHOMP on the armplanning problems. This version is not yet publicly availableat the time of publication, but it was made available to usby the authors [32]. We did not use CHOMP for the full-body

planning problems because we did not have the documentationor data files we would need to run these experiments on thePR2.

We tested both our algorithm and CHOMP under twoconditions: single initialization and multiple initializations. Forthe single initialization, we used a straight line in configurationspace from the start to the goal. For multiple initializations,we used the following methodology.• For the arm planning problems, prior to performing

these experiments we manually selected four waypointsW1,W2,W3,W4 in joint space. These waypoints werefixed for all scenes and problems. Let S and G denote thestart and goal states for a planning problem. Then we usedthe four initializations SW1G, SW2G, SW3G, SW4G,which linearly interpolate between S and Wi for the firstT/2 time-steps, and then linearly interpolate between Wi

and G for the next T/2 timesteps.• For the full-body planning problems, we randomly sam-

pled the environment for base positions (x, y, θ) with thearms tucked. After finding a collision-free configurationW of this sort, we initialized with the trajectory SWGas described above. We generated up to 5 initializationsthis way. Note that even though we initialize with tuckedarms, the optimization typically untucks the arms toimprove the cost.

A few more implementation details for our algorithm aregiven below:• Our current implementation of the continuous-time col-

lision cost does not consider self-collisions, but we pe-nalized self-collisions at discrete times as described inIV.

• For collision checking, we took the convex hull of everymesh of the robot. Each link is made of one or moremeshes. We used the Bullet collision checker [8].

• The termination conditions we used for the optimizationwere (i) maximum of 40 iterations, (ii) minimum meritfunction improvement ratio of 10−4, (iii) minimum trustregion size 10−4. Conditions (ii) and (iii) occurred in thevast majority of these cases, indicating good convergence.

• The arm trajectories have 11 timesteps, and the full-bodytrajectories have 41 timesteps.

CHOMP was run for 3 seconds for each initialization;this duration was chosen because the success rate on thisbenchmark sharply decreased for lower computation time.OMPL was limited to 30 seconds on full-body scenes.

The results for arm planning are shown in Table I. Theresults for full-body planning are shown in Table II. Our algo-rithm with multiple initializations substantially outperformedthe other approaches in both sets of problems. The path lengthswere normalized by dividing by the shortest path length forthat problem (across all planners).

VII. OTHER APPLICATIONS

Trajectory optimization is widely applicable to problemsinvolving a variety of interesting constraints, including non-holonomic and dynamic constraints.

Fig. 7. Atlas robot in simulation walking across the room and pressing abutton. Each footstep was planned separately. Five states out of a long motionare shown.

A. Humanoid walking: static stability

We have validated that our approach scales to a high-DOFsituation; planning a statically stable walking motion for theAtlas humanoid robot model. The degrees of freedom includeall 28 joints and the 6 DOF pose, where we used the axis–angle(exponential-map) representation for the orientation. Walkingis divided into four phases (1) left foot planted, (2) both feetplanted (3) right foot planted, (4) both feet planted. We imposethe constraint that the center of mass constantly lies above theconvex hull of the planted foot or feet. That is, the convexsupport polygon is represented as an intersection of k half-planes, yielding k inequality constraints

aixcm(θ) + biycm(θ) + ci ≤ 0, i ∈ {1, 2, . . . , k} (25)

where the ground-projection of the center of mass (xcm, ycm)is a nonlinear function of the robot’s configuration.

Using this approach, we plan a sequence of steps across aroom, as shown in figure 7. Each step is planned separatelyusing the phases described above. The robot is able to obeythese stability and footstep placement constraints while duck-ing under an obstacle.

B. Pose constraints

Our approach can readily handle kinematic constraints, forexample, the constraint that a redundant robot’s gripper is at acertain pose at the end of the trajectory. A pose constraint canbe formulated as follows. Let Ftarg denote the target poseof the gripper, and let Fcur(θ) be the current pose. ThenF−1targFcur(θ) gives the pose error, measured in the frame ofthe target pose. This pose error can be represented as the six-dimensional error vector

h(θ) = (tx, ty, tz, rx, ry, rz) (26)

where (tx, ty, tz) is the translation part, and (rx, ry, rz) is theaxis-angle representation of the rotation part.

One can also impose partial orientation constraints. Forexample, consider the constraint that the robot is holding abox that must remain upright. The orientation constraint is anequality constraint, namely that an error vector (vwx , v

wy )(θ)

vanishes. Here, v is a vector that is fixed in the box frameand should point upwards in the world frame.

Trajopt Trajopt-Multi ompl-RRTConnect ompl-LBKPIECE CHOMP CHOMP-Multisuccess fraction 0.84 0.99 0.97 0.96 0.66 0.85average time (s) 0.20 0.32 1.2 3.1 3.1 6.0

avg normed length 1.2 1.2 1.6 1.7 2.4 2.6

TABLE IResults on 198 arm planning problems for a PR2, involving 7 degrees of freedom. Trajopt refers to our algorithm.

Trajopt Trajopt-multi OMPL-RRTConnect OMPL-LBKPIECEsuccess fraction .63 0.84 0.53 0.50average time (s) 2.1 7.6 18.0 18.7

avg normed length 1.08 1.09 1.5 1.5

TABLE IIResults on 96 full-body planning problems for a PR2, involving 18 degrees of freedom (two arms, torso, and base).

Saturday, February 2, 13

Fig. 8. Several stages of a box picking procedure, in which boxes aretaken from the stack and moved to the side. The box is subject to orientationconstraints.

Figure 8 shows our algorithm planning a series of motionsthat pick boxes from a stack. Our algorithm typically planseach motion in 30− 50 ms.

VIII. REAL-WORLD EXPERIMENTS

A. Environment preprocessing

One of the main challenges in taking motion planning fromsimulation to reality is creating a useful representation ofthe environment’s geometry. Depending on the scenario, thegeometry data might be live data from a Kinect or laser rangefinder, or it might be a mesh produced by an offline mappingprocedure.

We have successfully used our algorithm with two differentrepresentations of environment geometry: (1) convex decom-position, and (2) meshes.

The process of going from a surface mesh to a union of con-vex shapes is called approximate convex decomposition [17].Convex decomposition is a popular approach for simplifyinggeometric models for collision checking and simulation, e.g.for video games [8]. We used the HACD software of KhaledMamou [18], which, in our experience, robustly producedgood decompositions, even on the open meshes we generatedfrom single depth images.

Our algorithm also can be used directly with mesh data.The mesh is viewed as a soup of triangles (which are convexshapes), and we penalize collision between each triangle andthe robot’s links. For best performance, the mesh should firstbe simplified to contain as few triangles as possible whilefaithfully representing the geometry, e.g. see [6].

Example code for generating meshes and convex decompo-sitions from Kinect data, and then planning using our softwarepackage Trajopt, is provided in a tutorial at [1].

B. Real-world experiments

We performed several real-world experiments involving amobile robot (PR2) to explore and validate two aspects ofour approach: (1) applying it to the “dirty” geometry datathat we get from 3D sensors, and (2) seeing if the full-body trajectories can be executed, in practice. Our end-to-end system successfully handled three full-body planningproblems:

1) Grasp a piece of trash on a table and place it in a garbagebin under a table (one arm + base)

2) Open a door, by following the appropriate pose trajec-tory to open the handle and push. (two arms + torso +base)

3) Drive through an obstacle course, where the PR2 mustadjust its torso height and arm position to fit throughoverhanging obstacles (two arms + torso + base).

The point clouds we used were obtained by mapping out theenvironment using SLAM and then preprocessing the map toobtain a convex decomposition. Videos of these experimentsare available at the website for this paper [2].

IX. DISCUSSION

While the motivation of this work is similar to CHOMP,our approach differs from CHOMP in several important di-mensions, most notably that (1) we use a different approachfor collision detection, and (2) we use a different numericaloptimization scheme.

1) Distance fields versus convex-convex collision checking:CHOMP uses the Euclidean distance transform—a precom-puted function on a voxel grid that specifies the distanceto the nearest obstacle, or the distance out of an obstacle.Typically each link of the robot is approximated as a union ofspheres, since the distance between a sphere and an obstaclecan be bounded based on the distance field. The advantageof distance fields is that checking a link for collision againstthe environment requires constant time and doesn’t dependon the complexity of the environment. On the other hand,

spheres and distance fields are arguably not very well suitedto situations where one needs to accurately model geometry,which is why collision-detection methods based on meshes andconvex primitives are more prevalent in applications like real-time physics simulation, which require speed and accuracy.

One important consideration for trajectory optimization ishow “well-shaped” the objective is. That is, given a trajectorythat contains collisions, how reliably does following the gra-dient get the trajectory out of collisions, rather than gettingstuck in a bad local optima? Whereas convex-convex collisiondetection takes two colliding shapes and computes the minimaltranslation to get them out of collision, the distance field (andits gradient) merely computes how to get each robot point (orsphere) out of collision; however, two points may disagreeon which way to go. Thus convex-convex collision detectionarguably provides a better local approximation of configurationspace, allowing us to formulate a better shaped objective.

The CHOMP objective is designed to be invariant toreparametrization of the trajectory. This invariance property ofthe objective makes it much better shaped, helping the gradientpull the trajectory out of an obstacle instead of encouraging itto jump through the obstacle faster. Our method of collisionchecking against the swept-out shape achieves this result in acompletely different way. We did not try scaling our collisionpenalty by speed as in the CHOMP objective, but that wouldbe interesting.

2) SQP versus projected gradient descent: CHOMP uses(preconditioned) projected gradient descent, i.e., it takes stepsx← Proj(x−A−1∇f(x)), whereas our method uses sequen-tial quadratic programming (SQP). Taking a projected gradientstep is cheaper than solving a QP. However, these projectedgradient steps don’t incorporate much second-derivative infor-mation, and projected gradient descent has linear, rather thanquadratic convergence. That said, although we use a second-order method, we don’t necessarily get quadratic convergencebecause we don’t calculate the full Hessian of all terms inthe objective. Another advantage of sequential quadratic pro-gramming is that it can handle deeply infeasible initializationsusing penalties and merit functions, as described in SectionIII. We’ll note that about half of the modern software forgeneric non-convex optimization uses an SQP variant (e.g.KNITRO, SNOPT). The remainder uses interior point methodsand augmented Lagrangian methods [16].

So in both numerical optimization and collision detection,our approach requires more computation per iteration butgenerates a subproblem that is a better approximation of thetrue problem. Thus our algorithm takes a small number ofiterations to converge (usually 15 or 20), but each iteration ismore expensive; whereas CHOMP has the opposite attributes.(Though in practice, CHOMP iterations are somewhat expen-sive because of the larger number of timesteps required tomake it work well.) One could potentially use a differentcombination of approaches: gradient descent with convexcollision detection, or SQP with distance fields. However, thechoices are not orthogonal, since if you choose one slowmethod and one fast method, you may end up with the worst

of both worlds: expensive iterations and slow convergence.

X. SOURCE CODE AND REPRODUCIBILITY

All of our source code is available as a BSD-licensed open-source package called Trajopt [1]. Optimization problems canbe constructed and solved using the underlying C++ API orthrough Python bindings. Trajectory optimization problemscan be specified in JSON string that specifies the costs,constraints, degrees of freedom, and number of timesteps. Weare also working on a MoveIt plugin [5] so our software canbe used along with ROS tools.

For robot and environment representation, we use Open-RAVE, and for collision checking we use Bullet, because ofthe high-performance GJK-EPA implementation and collisiondetection pipeline. Two different backends can be used forsolving the convex subproblems: (1) Gurobi, a commercialsolver, which is free for academic use; and (2) BPMPD [19],a free solver, which is included in our software distribution.

The benchmark results presented in this paper can bereproduced by running scripts provided at the paper’s webpage[2]. Various examples, including humanoid walking and armplanning with orientation constraints, are included with oursoftware distribution [1].

XI. CONCLUSION

We presented a novel algorithm that uses trajectory opti-mization for robotic motion planning. We benchmarked ouralgorithm against sampling-based planners from OMPL andCHOMP. Our algorithm was faster than the alternatives, solveda larger fraction of problems, and produced better paths. Asidefrom the benchmark, we validated our approach on planningstepping motions the Atlas humanoid robot, industrial boxpicking, and the real PR2 and its sensor data.

XII. ACKNOWLEDGEMENTS

We thank Jeff Trinkle, Sachin Patil, and Dmitry Berenson,and Nikita Kitaev for insightful discussions and commentson the paper. We thank Kurt Konolige and Ethan Rubleefrom Industrial Perception Inc. for supporting this work andproviding valuable feedback. We thank Ioan Sucan and SachinChitta for help with MoveIt, and we thank Anca Dragan,Chris Dellin, and Sidd Srinivasa for help with CHOMP. Thisresearch has been funded in part by the Intel Science andTechnology Center on Embedded Computing, by an AFOSRYIP grant, and by a Sloan Fellowship.

REFERENCES

[1] Webpage for Trajopt software package. URL http://rll.berkeley.edu/trajopt.

[2] Webpage for this paper. URL http://rll.berkeley.edu/trajopt/rss.

[3] J.T. Betts. Practical methods for optimal control andestimation using nonlinear programming, volume 19.Society for Industrial & Applied Mathematics, 2010.

[4] O. Brock and O. Khatib. Elastic strips: A framework formotion generation in human environments. The Interna-tional Journal of Robotics Research, 21(12):1031–1052,2002.

[5] S. Chitta, I. Sucan, and S. Cousins. Moveit![ROS topics].Robotics & Automation Magazine, IEEE, 19(1):18–19,2012.

[6] Paolo Cignoni, Claudio Montani, and Roberto Scopigno.A comparison of mesh simplification algorithms. Com-puters & Graphics, 22(1):37–54, 1998.

[7] B. Cohen, I.A. Sucan, and S. Chitta. A generic infras-tructure for benchmarking motion planners. In IntelligentRobots and Systems (IROS), 2012 IEEE/RSJ Interna-tional Conference on, pages 589–595. IEEE, 2012.

[8] Erwin Coumanns. Bullet physics library, 2012.www.bulletphysics.org.

[9] A.D. Dragan, N.D. Ratliff, and S.S. Srinivasa. Manipula-tion planning with goal sets using constrained trajectoryoptimization. In Robotics and Automation (ICRA), 2011IEEE International Conference on, pages 4582–4588.IEEE, 2011.

[10] C. Ericson. Real-time collision detection. MorganKaufmann, 2004.

[11] E. G. Gilberg, D. W. Johnson, and S. S. Keerthi. A fastprocedure for computing the distance between complexobjects in three-dimensional space. IEEE Journal ofRobotics and Automation, 1988.

[12] M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, andS. Schaal. STOMP: Stochastic trajectory optimization formotion planning. In Robotics and Automation (ICRA),2011 IEEE International Conference on, pages 4569–4574. IEEE, 2011.

[13] Oussama Khatib. Real-time obstacle avoidance for ma-nipulators and mobile robots. The international journalof robotics research, 5(1):90–98, 1986.

[14] James J Kuffner Jr and Steven M LaValle. Rrt-connect:An efficient approach to single-query path planning. InRobotics and Automation, 2000. Proceedings. ICRA’00.IEEE International Conference on, volume 2, pages 995–1001. IEEE, 2000.

[15] R. Lampariello, D. Nguyen-Tuong, C. Castellini,G. Hirzinger, and J. Peters. Trajectory planning foroptimal robot catching in real-time. In Robotics andAutomation (ICRA), 2011 IEEE International Conferenceon, pages 3719–3726. IEEE, 2011.

[16] S. Leyffer and A. Mahajan. Nonlinear constrainedoptimization: methods and software. Argonee NationalLaboratory, Argonne, Illinois, 60439, 2010.

[17] J.M. Lien and N.M. Amato. Approximate convex de-composition of polyhedra. In Proceedings of the 2007ACM symposium on Solid and physical modeling, pages121–131. ACM, 2007.

[18] K Mamou and F Ghorbel. A simple and efficientapproach for 3d mesh approximate convex decomposi-tion. In 16th IEEE International Conference on ImageProcessing (ICIP V9), pages 3501–3504, 2009.

[19] Csaba Meszaros. The bpmpd interior point solver forconvex quadratic problems. Optimization Methods andSoftware, 11(1-4):431–449, 1999.

[20] I. Mordatch, E. Todorov, and Z. Popovic. Discoveryof complex behaviors through contact-invariant optimiza-tion. In ACM SIGGRAPH, 2012.

[21] J. Nocedal and S.J. Wright. Numerical optimization.Springer Verlag, 1999.

[22] C. Park, J. Pan, and D. Manocha. Itomp: Incrementaltrajectory optimization for real-time replanning in dy-namic environments. In Proceedings of the InternationalConference on Automated Planning and Scheduling, toappear, 2012.

[23] Michael Posa and Russ Tedrake. Direct trajectoryoptimization of rigid body dynamical systems throughcontact. In Algorithmic Foundations of Robotics X, pages527–542. Springer, 2013.

[24] S. Quinlan and O. Khatib. Elastic bands: Connectingpath planning and control. In Robotics and Automation,1993. Proceedings., 1993 IEEE International Conferenceon, pages 802–807. IEEE, 1993.

[25] N. Ratliff, M. Zucker, J.A. Bagnell, and S. Srinivasa.CHOMP: Gradient optimization techniques for efficientmotion planning. In Robotics and Automation, 2009.ICRA’09. IEEE International Conference on, pages 489–494. IEEE, 2009.

[26] I.A. Sucan, M. Moll, and L.E. Kavraki. The open motionplanning library. Robotics & Automation Magazine,IEEE, 19(4):72–82, 2012.

[27] Ioan A Sucan and Lydia E Kavraki. Kinodynamicmotion planning by interior-exterior cell exploration. InAlgorithmic Foundation of Robotics VIII, pages 449–464.Springer, 2009.

[28] Y. Tassa, T. Erez, and E. Todorov. Synthesis and stabi-lization of complex behaviors through online trajectoryoptimization. In Proc. IROS, 2012.

[29] G. van den Bergen. A fast and robust GJK implementa-tion for collision detection of convex objects. Journal ofGraphics Tools, 4(2):7–25, 1999.

[30] C.W. Warren. Global path planning using artificialpotential fields. In Robotics and Automation, 1989.Proceedings., 1989 IEEE International Conference on,pages 316–321. IEEE, 1989.

[31] A. Werner, R. Lampariello, and C. Ott. Optimization-based generation and experimental validation of optimalwalking trajectories for biped robots. In IntelligentRobots and Systems (IROS), 2012 IEEE/RSJ Interna-tional Conference on, pages 4373–4379. IEEE, 2012.

[32] M. Zucker, N. Ratliff, A.D. Dragan, M. Pivtoraiko,M. Klingensmith, C.M. Dellin, J.A. Bagnell, and S.S.Srinivasa. CHOMP: Covariant hamiltonian optimizationfor motion planning. International Journal of RoboticsResearch, 2012.