CHOMP-R: Covariant Optimization for Motion Replanning

Mihail Pivtoraiko1, Matt Klingensmith2, J. Andrew Bagnell2 and Alonzo Kelly2

Abstract— This paper presents a method of fast trajectoryreplanning: a process of repairing a previously planned trajec-tory that is subject to being invalidated due to the inevitableand frequent changes in the world model of autonomous robotsoperating in partially-known, cluttered environments. Whilethis procedure is widely applied to lower-dimensional systemsusing search-based planning methods, the proposed approachgeneralizes to arbitrary high-dimensional systems, includingrobotic manipulators. Our method is based on covariant gra-dient descent with dynamic cost function weighting, which wedemonstrate to lead to superior convergence with respect tothe state of the art in this domain. In addition, we note andexploit the fact that the environment representations basedon distance fields are compositional under the min operation:by pre-computing distance fields for known objects in therobot’s environment, we can effectively compute a very highresolution distance field for the entire scene by transformingeach object into the appropriate frame and using the minimumacross these component distance fields. Due to these two noveldesign aspects, our replanner exhibits superior performancewith respect to prior work in this domain, as demonstrated byboth simulated and real robot experiments.

I. INTRODUCTION

Many popular approaches to motion planning, especiallyfor high-dimensional systems, use the models of the en-vironment and the robot that are assumed to be staticand known with certainty. Unfortunately, in many realisticrobotics scenarios, this assumption is not always satisfied: theworld model is rarely fully known and is usually uncertain.Frequent and potentially significant changes to it are causedeither by sensor noise or by moving agents in the envi-ronment. This motivates the need for efficient replanning:“repairing” a trajectory should it become invalidated, in thespirit of the work by Stentz and Hebert [28]. CHOMP-R,the presented motion replanner based on covariant gradientdescent, effectively fills that need.

The proposed approach was inspired by the work ofRatliff et al. [26] in applying covariant optimization tomotion planning. We propose several modifications to theirapproach in order to apply this optimization technique totrajectory replanning. We demonstrate that the proposedalgorithmic improvements lead to as much as an order ofmagnitude improvement in runtime, on average, withoutlosses in planning quality in terms of convergence, obstacle

This work was supported by the DARPA Autonomous Robotic Manipu-lation Program.

1M. Pivtoraiko is with the Department of Mechanical Engineering andApplied Mechanics, University of Pennsylvania, Philadelphia, PA 19104,USA; mihailp@seas.upenn.edu

2M. Klingensmith, D. Bagnell and A. Kelly are with the RoboticsInstitute, Carnegie Mellon University, Pittsburgh, PA 15213, USA;{mklingen, dbagnell, alonzo}@cs.cmu.edu

Fig. 1. Trajectory replanning in manipulation. Robot manipulatorscan utilize the approach presented here for monitoring and replanning armtrajectories. We provide simulated and real robot results using the DARPAARM platform in the figure. The robot attempts to avoid an obstacle thatis unexpectedly introduced in its workspace.

avoidance or trajectory smoothness. In addition, we introduceworkspace pose constraint satisfaction in the context ofcovariant optimization – a useful feature for manipulatingcertain objects (e.g. carrying a mug without spilling), dual-arm manipulation and other advanced tasks.

Similar to most numerical optimization approaches, themethod performs “local” modifications to the trajectory,namely within a certain region around the original trajectory,while the size of the region is related to the local minimaof the cost function. However, this observation is in fact oneof the reasons for preferring the approach in replanning: thislocality reduces the disturbances to the trajectory followingcontroller when switching from the original to the replannedtrajectory. Reduced trajectory deviation is likely to simplifytrajectory following by the mechanism. Many standard plan-ning paradigms do not offer this locality feature.

In summary, CHOMP-R is attractive for motion replanningfor high-dimensional systems because it is well-founded inoptimization theory and easy to implement, while exhibitinggood convergence even in standard motion planning setting.It is the first covariant optimization technique that is fastenough to be effective as a motion replanner.

A. Motivation

The replanning capability, described above, is emerging asespecially useful as robot systems become increasingly moreadvanced, featuring perception and localization systems that

provide new information at high update rates. Under-utilizingthis information by a planner that does not react fast enoughputs the robot at a disadvantage.

Moreover, robot designs also become increasingly morecomplex, featuring more than one manipulator arm andother moving parts, operating in complex, cluttered envi-ronments. Classical motion planning techniques, includingsampling- and search-based ones, require runtime that isoverly extensive for high-dimensional replanning. In contrast,often trajectory repair may be accomplished with a relativelysmall, local modification to the trajectory, and many classicalplanners appear to be over-powered for this task. Instead,we propose a method based on numerical optimization thatoffers complexity advantages with respect to the conventionalmethods.

Lastly, by virtue of reasoning about the distance toobstacles in a continuous fashion, numerical optimizationapproaches yield maximum margin quality to obstacle avoid-ance: not only is the collision avoided, but the distance tothe nearest obstacle is also maximized. This quality is usefulin avoiding collision during trajectory following even in theevent that the robots models of the environment and of itsown motion are inaccurate.

In summary, using a re-planner that is too slow to react hasa disadvantage of causing the robot to operate more slowlythan it would otherwise be able to do. Trajectory locality,smoothness and maximing obstacle avoidance margin arealso attractive for increasing the speed of the mechanism– and both are naturally provided by the proposed approach.

B. Prior Work

Motion replanning has been a popular method of man-aging changing and uncertain environments in the domainof wheeled vehicle navigation for several decades, and anumber of approaches appear to be standard componentsof fielded systems [17, 22, 28]. However, the applicationof these methods to general, high dimensional systems hasbeen limited due to their susceptibility to the “curse ofdimensionality”: in large configuration spaces, the methodsbased on exhaustive search suffer from intractability.

To overcome these issues, numerical optimization has beenapplied in this domain. Khatib [19] pioneered the applicationof the artificial potential field for real-time obstacle avoid-ance. The method’s sensitivity to local minima has beenaddressed in a range of related work. Analytical navigationfunctions that are free of local minima have been proposedfor some specific environments by Rimon and Koditschek[27]. Numerical alternatives of a more general nature wereexplored by Barraquand and Latombe [1], however theyrequire an exponential amount of memory with respect tothe dimension of the configuration space. Quinlan and Khatib[24] further extended numerical methods in motion replan-ning by modeling the trajectory as a mass-spring system, anelastic band, and performing replanning by scanning backand forth along the elastic, while moving one mass particle ata time. An extensive effort is made to construct an explicit C-space model of the free space, and the cost function contains

terms that attempts to control the motion of the trajectoryparticles along the elastic. Brock and Khatib [6] furtherextend this technique. Much of the complexity of thesemethods is alleviated by the method proposed herein viacovariant gradient descent, introduced by Ratliff et al. [26].Obstacles are considered directly in the robot’s workspace,where the notions of distance and inner product are morenatural. While our method shares a number of design aspectswith [26], certain key components are structured differentlyso as to apply covariant optimization to motion replanning.As described in the following sections, these modificationsspan the core components of the algorithm, from the costfunction formulation to the world model representation.

Kalakrishnan et al. [15] addressed the motion planningproblem by applying motion sampling techniques. However,the method ignores gradient information that is, in fact,readily available and can be quite useful to improve con-vergence. In contrast, our approach exploits the availabilityof the gradient. Our assumption that it exists is frequentlysatisfied and does not hinder the application of the methodin a variety of relevant contexts, such as collision avoidanceand workspace constraint satisfaction, as we demonstrate inthe sequel.

Sampling-based [14, 16, 20] and search-based [7] methodshave been applied to motion planning for high-dimensionalsystems, including manipulators. workspace constraint sat-isfaction was also demonstrated [3]. However, as suggestedabove, the application of these methods to trajectory replan-ning has been limited, since these methods are most effectiveat motion planning at the “global” scale and do not attempt toleverage the proximity to the initial trajectory being repaired.In contrast, the locality of trajectory updates by the numericaloptimization methods is convenient, as motivated in SectionI-A.

Much of CHOMP-R’s complexity advantages come froma pre-computed, compositional representation of the envi-ronment. Boundary representations of the environment havebeen previously developed using geometric primitives byBobrow [4], Gilbert et al. [13] and Lin and Canny [23]. Effi-cient hierarchical representations have also been proposed byFaverjon [11], Quinlan [25] and others. In contrast, we utilizethe models of the actual objects in the environment, whichallows pre-computation of very accurate, high-resolutiondistance field representations of the objects. The assumptionthat object shape information is known a-priori is based onthe observation that these data are becoming increasinglyavailable for many common objects, facilitated by the recentadvances in RGB-D and similar sensing technology [21].In this regard, the proposed method seeks to explore thesymbiotic relationship between perception and planning byleveraging the recent advances in the former to attain newgains in the latter.

C. Problem Formulation

The trajectory replanning problem requires five param-eters: Wobst, Q, J, ur,F . We assume the robot workspaceto be W ⊂ SE(3), and Wobst ⊂ W is a subset that is

occupied by the obstacles or other regions that the robotmust avoid. Environment collisions, manipulator end-effectorpose and similar constraints are most naturally specified inthe workspace. The subset of the workspace that is free ofobstacles is Wfree = W \ Wobst. The robot configurationspace or C-space, Q ⊂ Rn, is an n-dimensional compactdifferentiable manifold, equipped with a metric ρ. The Jaco-bian J captures the relationship between W and Q.

We define the control space, U , a Hilbert space consistingof trajectories, or controls, u : R → Q. A common way toparameterize the controls is w.r.t. time, however by virtueof covariant gradient that we will use, this parameterizationis not important here; instead, we assume the domain of acontrol to be an interval [0, 1] ∈ R. The argument to u willbe omitted frequently for brevity.

The given reference trajectory, ur, is a control that hasbeen invalidated due to a change in the world model andmust be replanned. It encodes the boundary conditions forthe replanning problem: qI = u(0) and qF = u(1). Lastly, agiven time-independent cost function F : U → R assignsa scalar cost to steering the system with a control u ∈ U .In practice, cost may be related to time duration of theassociated control, energy consumed by the system duringthe motion, risk experienced, etc. We define the cost functionas a weighted sum of component functions, representing anumber of the desired performance criteria:

F(u) =

k∑i

wifi(u) (1)

for u ∈ U and weight vector w ∈ Rk. We also define aworkspace path of a point b, an element of the robot’s bodyB, that is in motion while the robot executes a trajectory uas the projection of this trajectory on the position subspaceof W , πb : [0, 1]→ R3.

Then, the problem of replanning is computing a newcontrol, u∗, that minimizes the cost function, given problemconstraints:

u∗ = argminu∈U

F(u)

s.t. u(0) = ur(0)

u(1) = ur(1)

πb(t) ∈Wfree,∀t ∈ [0, 1] ,∀b ∈ B

(2)

II. COMPOSITIONAL DISTANCE FIELD

In the numerical optimization context, obstacle avoidanceis performed by transforming a model of the environmentinto a continuous and differentiable cost function. One ap-proach to obtaining such a cost function is to use a signeddistance field d : R3 → R which gives the distance from apoint x ∈ R3 to the boundary of the nearest obstacle. Valuesof d(·) are negative inside obstacles, positive outside, andzero at the boundary.

A number of fast techniques for computing discrete dis-tance fields exist; for example, a method due to Felzenszwalband Huttenlocher [12] has linear complexity in the number of

(a) (b)

(c) (d)

Fig. 2. Pre-computed object distance primitives. The figures shows twoexamples of objects with complex, non-convex shapes that would be difficultto represent accurately with simple geometric primitives. The correspondingobject distance primitives are shown on the right in the figure.

voxels in the grid. Unfortunately, this computation still takesseveral seconds on modern hardware for typical manipulatorreachability regions, even at the minimal admissible voxelresolution. Moreover, it is beneficial to increase this resolu-tion as much as possible, as that yields a more informedrepresentation and tends to improve convergence of theoptimizer.

However, many of the objects with which the robots nowa-days must interact cannot be easily represented as collectionsof such geometric primitives. The objects are almost neverconvex and typically have complex shape, as illustrated inFigure 2. In addition, we note that the efficiency of manymodern robot perception systems is due to leveraging certaindomain knowledge of the problem, frequently involvingdetailed models of the known objects in the environment.In an effort to leverage a symbiotic relationship of planningand perception, we propose utilizing the same object modelsfor efficient distance field computation.

In particular, we note and exploit the fact that the environ-ment representations based on distance fields are composi-tional under the min operation. For every available objectmodel, a high-resolution distance field and its gradientsare computed via an extensive, but off-line computation infree space and with respect to a certain frame of referenceof the object, FO. During planning, a perception processgenerates a model of the environment which includes a setO of objects and their poses, expressed with homogeneoustransforms TFW

FOin world frame FW . Then the distance field

computation is reduced to a minimization across a set ofdistance field primitives pre-computed for each object in O.

d(x) = minO∈O

(TFW

FO

)−1x (3)

Hierarchical representations, such as the k-d tree [2], may

be utilized to speed up this computation.

III. COST FUNCTIONS

Similar to most optimization methods, the basis of op-timality of trajectories computed by CHOMP-R is a costfunction. We require that this function be continuous anddifferentiable with respect to trajectory parameterization inthe relevant subset of the domain of the function.

A. Smoothness Cost

Besides computing obstacle avoiding trajectories, wewould like to obtain trajectories that are smooth in termsof C-space dynamics, namely those with time derivativesof small magnitude. This is beneficial for the accuracy oftrajectory tracking and for increasing the speed of execution.

To this end, we construct a smoothness cost function,fs(u) as a sum of squared C-space derivatives of the trajec-tory. For each derivative d = 1, . . . , D, we assume a finitedifferencing matrix Kd and represent smoothness cost as asum:

fs(u) =1

2

D∑d=1

wd‖Kdu+ ed‖2 (4)

where constant ed encapsulates the boundary values qI andqF , and wd is the weight of each of the cost components.The smoothness cost can be re-written as quadratic form:

fs(u) =1

2uTAu+ uT b+ c (5)

for the appropriate constants, including a positive definite A.This definition is similar to Ratliff et al. [26], however we

represent u as an n × m matrix and avoid the Kronecker(tensor) product, thereby reducing the complexity of thecovariant update rule (Section IV-A) from O(m3n3) downto O(n), since we also replace the matrix multiply witha tri-diagonal solve using the Thomas algorithm [8]. Thismodification contributes to the reduction in planner runtimereported in Figure 4-E1 in experimental results.

B. Obstacle Cost

The process of converting an environment model to acontinuous and differentiable function was given in SectionII. Here this function is used to formulate a cost functionthat results in obstacle avoidance. We first review the designof the obstacle cost function by Ratliff et al. [26]. Then wepropose an alternative, minimax, formulation of this function.

First, we define an obstacle potential function based onthe distance field:

c (x) =

−d(x) + 1

2ε, if d(x) < 012ε (d(x)− ε)2, if 0 ≤ d(x) ≤ ε0 otherwise

(6)

Integration of the obstacle potential over the entire robot,∀b ∈ B, provides an estimate of the collision cost at a givenconfiguration.

1) Integral Formulation: Ratliff et al. [26] estimate thecollision cost of the entire trajectory via a line integral alongthe trajectory:

fo(u) =

∫ 1

0

∫Bc (πb(t)) ‖

d

dtπb(t)‖dbdt (7)

Because (7) does not depend on higher order derivativesbeyond velocity, we can write functional gradient as ∇̄fo =∂v∂q −

ddt

∂v∂q′ , where v denotes everything inside the time

integral [9, 25]. Application of this formula to (7) yields:

∇̄fo =

∫BJT ‖π′b‖ (P∇c− cκ) du (8)

where P = I−π̂b′π̂b′T is a projection matrix, κ = 1‖π′

b‖2Pπ′′b

is the curvature vector [10], normalized vectors are denotedwith π̂b, prime notation denotes time derivatives, and timedependence of J , πb and c is suppressed for brevity.

2) Minimax Formulation: On certain systems, such aslow-power embedded controllers, floating point matrix arith-metic is an expensive operation. In such applications, we notepotential value of redefining the obstacle cost as a minimaxformulation allows minimizing this computation, while stillattaining much of CHOMP-R performance. In particular, wereplace the line integral in (7) with a max operator:

fo(u) = maxi

(∫Bc (πu(u(t))) ‖ d

dtπu(u(t))‖du

)(9)

for trajectory samples i ∈ {0, . . . , 1}. As a result, thecost function will seek to minimize the worst violationof the obstacle constraint and will not be influenced byother trajectory points. Once the maximum value of costis alleviated, the planner will shift focus to bring anotherbody element out of collision, eventually leading to obstacleavoidance by the entire trajectory. While (7) repeats thecomputation of the body integral over a large number oftrajectory samples, this formulation performs it only onceand is, therefore, less demanding computationally.

This formulation, however, is likely to require a greaternumber of optimization iterations, since each iteration issomewhat less informed than the integral formulation whereall trajectory points are used. This trade-off may be managedby utilizing several high-cost trajectories instead of the singlemaximizing one in (9). The choice of this trade-off largelydepends on the specifics of the computing platform at handand the runtime cost of matrix arithmetic on it.

C. Workspace Cost

In many settings, it is beneficial to impose a cost oncertain workspace motion of the mechanism. For example, inthe context of manipulation, visual servoing, etc. one mightdesire to constrain the end-effector of the manipulator to bein a certain, maybe small, subset of the workspace.

Given a certain element of the robot body, b ∈ B andits workspace position (x, y, z) and orientation (φ, γ, θ),we propose setting up a per-dimension cost function thatevaluates to 0 if b satisfies the desired constraint and to

quadratic error if it does not. Using the x- dimension asan example, and supposing we desire to limit b’s pose suchthat its x-coordinate is greater than xmin, we can define aworkspace cost potential similar to (6):

c (x) =

{(x− xmin)2 if x < xmin

0 otherwise(10)

If the desired interval is closed, another instance of (10)can be used for the other boundary, e.g. xmax. This potentialcan be used in a manner similar to Section III-B, e.g. from(7) onwards. This cost function is well-defined for any b ∈ B,and is continuous and differentiable.

IV. TRAJECTORY REPLANNING VIA COVARIANTOPTIMIZATION

This Section combines the components presented previ-ously: the compositional distance fields in Section II and thecost functions in Section III, and applies them to motionreplanning in the covariant optimization framework.

A. Covariant Gradient Descent

We obtain a trajectory update rule by computing a gradientof the cost function with respect to trajectory parameteri-zation. A first-order Taylor expansion of the cost functionduring an update step is:

F(u) ≈ F(uk) +∇F(uk)T (∆u) (11)

where ∆u = u − uk. This allows us to write the updatesimilar to [26]:

uk+1 = argminu∈U

(F(uk) +∇F(uk)T∆u+

λ

2‖∆u‖2M

)(12)

where ‖ · ‖2M is the norm with respect to the Riemannianmetric M . Setting the gradient of the right hand side of (12)to zero and solving for the minimizer leads to:

uk+1 = uk −1

λM−1∇F(uk) (13)

This update rule ensures that the change in the trajectoryis small with respect to the metric M . As given in Boyd andVandenberghe [5], solving a regularized problem of the formin (12) is equivalent to minimizing the linear approximationin (11) within a ball around uk whose radius is relatedto the regularization constant λ. At each trajectory update,only those modifications that keep the trajectory smooth areconsidered. We use A in (5) as the metric, and therefore theaction of the M−1 operator in (13) is, in basic terms, tospread the gradient across the trajectory in order to maintainsmoothness.

B. Weight Scheduling

Ratliff et al. [26] and Quinlan [25] use the weight vectorw in (1) to specify the relative importance of the componentsof the overall cost function being minimized. However, wefound value in modifying these weights during optimizationvia a process referred to as weight scheduling, similar to gainscheduling in non-linear control [18]. Early in optimization,it is beneficial to give most of the weight to collisionavoidance and similar costs and nearly remove the influenceof the smoothness cost. As optimization progresses andtrajectory obstacle cost significantly reduces, the weight isshifted to the smoothness cost to ascertain that the trajectoryremains attractive with respect to dynamics measures. Notethat, even while the optimizer focuses on obstacle avoidance,the covariant trajectory updates still maintain smoothness, asdiscussed in Section IV-A.

This measure makes the overall cost function more robustto local minima. For example, if the reference trajectory urbeing repaired (due to a collision along it) is already smooth,then the action of the collision cost will be to modify thetrajectory. If this modification would cause the trajectory tobe less smooth, it will force the smoothness cost to restoresmoothness and bring the trajectory back into collision.Indeed, the optimization may be at a local minimum of thissort right at the outset. Experimental results in Figure 4-E3suggest that weight scheduling indeed improves performancein practice – both in terms of better planner convergence andthe reduced number of required iterations.

V. EXPERIMENTAL RESULTS

Simulated and real-robot experimental results are pre-sented in the context of manipulation planning and demon-strate the efficacy of the approach. This presentation is di-vided into two parts. First, we show that, while the proposedsolution is best suited for replanning, it still outperforms theCHOMP algorithm [26] in general motion planning setting.Second, we apply the approach to replanning on a physicalrobot and demonstrate fast, nearly 10Hz, replan rates inrealistic scenarios.

In comparing to the standard CHOMP, we utilized a freelyavailable implementation based on ROS [29]. A simulatedmanipulator robot operates in a set of scenes that includedense clutter due to a collection of 50 randomly placed boxobstacles of random size, as illustrated in Figure 3. Tendifferent scenes were generated and 100 planning querieswere performed by selecting random initial and final armconfigurations.

The ROS implementation was executed on these planningqueries, and the results were considered the control group,i.e. baseline. The methods described in this paper werethen attempted also, and the results were compared to thiscontrol group. Four different configurations of CHOMP-Rwere compared to CHOMP:• E1: CHOMP algorithm, implemented verbatim to [26].

This experiment attempts to calibrate our implementa-tion of the algorithm to ROS [29].

Fig. 3. Simulated clutter scene. Comparison with CHOMP planner [26]was performed in randomized simulated clutter scenes similar to this one.

• E2: Same implementation as above, but the standardsigned distance field was replaced with the composi-tional distance field in Section II.

• E3: Same implementation as E1, but cost functionweights were scheduled as described in Section IV-B.

• E4: CHOMP-R: both object distance primitives andweight scheduling (as in E2 and E3) were combined.

Figure 4 demonstrates how the four variants of the algo-rithm compare to ROS CHOMP [29]. It uses three measuresfor the comparison: the ratio of the planning queries thatwere solved by a planner (failure means exceeding 500iterations), the number of iterations a planner took and theamount of time (in seconds) that was required to find asolution. These measures are color-coded in the Figure asblack, gray and white, respectively, and presented as ratios:each of the quantities was measured with each of the variantsE1-4 and divided by the respective quantity measured usingROS CHOMP.

In particular, the three bars corresponding to E1 demon-strate that our implementation of standard CHOMP was ableto solve a similar number of motion planning queries in thesimulated clutter environment (the black bar is marginallyabove the 1.0 ratio mark), but required a slightly greaternumber of iterations, on average across all trials. The im-plementation exhibited somewhat lower runtime in seconds,however, as Section III-A attempted to explain.

The experiment E2 suggests that replacing the standarddistance field with the compositional one does not affect per-formance, except by requiring far less runtime (in seconds)for each planning query. A reduction of over an order ofmagnitude is attained by virtue of eliminating the need tocompute the distance field on-line during planning.

The advantage of the cost function weight scheduling isdemonstrated in the experiment E3. The improvement istwo-fold: both in the greater ratio of queries solved andin reducing the number of iterations – with an advantageof about 20% in each category. Runtime in seconds is alsolower as a consequence of the reduction in iterations.

Lastly, E4 compares CHOMP-R, the combination of thethe features exhibited in E2 and E3, to ROS CHOMP. Thisinstance of the design is the one that we apply to the realrobot replanning experiments that follow.

The ARM-S manipulator robot, featuring a 7-DOF Bar-

Fig. 4. Planning performance comparison. The algorithm proposed inthis paper is evaluated in standard manipulator motion planning setting.Several aspects of the algorithm are compared to the CHOMP algorithmimplementation in ROS [29].

Fig. 5. Physical robot experiments. CHOMP and CHOMP-R arecompared on the ARM-S robot platform. The figure shows that the constraintclaimed greater runtime in seconds. The average CHOMP replan runtimewas 2.03 sec, while the average CHOMP-R runtime was 0.28 and 0.17sec. with and without the end-effector (e.e.) constraint, respectively. Thisresult is significant in demonstrating that CHOMP-R is sufficiently fast tobe responsible for the robot’s reactive behavior. In addition, it demonstratesthat satisfying workspace pose constraint maintains sub-second replan rate.

rett arm, was used for physical experimentation. The armrepeatedly executes a pre-defined trajectory, which becomesinvalidated due to an unexpected obstacle. Once the percep-tion system localizes the obstacle, the replanner reacts to itby modifying the trajectory to avoid the obstacle. 100 trialsof the experiment were performed in two configurations:CHOMP-R with and without the end-effector workspacepose constraint, compared to standard CHOMP. As Figure5 shows, CHOMP-R resulted in notable replanning speedupon this platform.

VI. CONCLUSIONS

We presented an approach to trajectory modification mo-tivated by the inevitable and frequent changes in the worldmodel of autonomous robots operating in partially-known,cluttered environments. The motion replanner is well suited

for general, high-dimensional systems, including manipu-lators. The method is based on covariant gradient descentwhich leads to significantly faster convergence in this contextthan standard descent methods. The resulting replanner is ef-fective at modifying planned trajectories should they becomein collision due to changes in the environment model.

Future work includes efforts to increase search spaceexploration capacity of the replanner by applying it in aparallel computation setting by providing a range of spatiallydistinct initial trajectories. By leveraging parallel computinghardware, a large number of such parallel processes canbe executed, resulting in exploration capacity that, in thiscontext, may potentially rival the standard, global motionplanning approaches.

ACKNOWLEDGMENTThe authors gratefully acknowledge the contribution of

the DARPA Autonomous Robotic Manipulation Program.Special thanks to Gil Jones and others at Willow Garagefor their support in performing ROS experiments.

REFERENCES

[1] J. Barraquand and J. C. Latombe. Robot motionplanning: a distributed representation approach. Inter-national Journal of Robotics Research, 10(6), 1991.

[2] J. L. Bentley. Multidimensional binary search trees usedfor associative searching. Communications of the ACM,18(9):509–517, 1975.

[3] Dmitry Berenson, Siddhartha S. Srinivasa, Dave Fergu-son, Alvaro Collet, and James J. Kuffner. Manipulationplanning with workspace goal regions. In Proc. ofthe IEEE International Conference on Robotics andAutomation, 2009.

[4] J. E. Bobrow. A direct minimization approach forobtaining the distance between convex polyhedra. In-ternational Journal of Robotics Research, 8(3):65–76,1989.

[5] S. Boyd and L. Vandenberghe. Convex Optimization.Cambridge University Press, 2004.

[6] Oliver Brock and Oussama Khatib. Elastic Strips: Aframework for motion generation in human environ-ments. International Journal of Robotics Research, 21(12):1031–1052, 12 2002.

[7] B. J. Cohen, S. Chitta, and M. Likhachev. Search-basedplanning for manipulation with motion primitives. InProc. of the IEEE International Conference on Roboticsand Automation, pages 2902–2908, 2010. doi: 10.1109/ROBOT.2010.5509685.

[8] S.D. Conte and C. deBoor. Elementary NumericalAnalysis. McGraw-Hill, New York, 1972.

[9] Richard Courant and David Hilbert. Methods of Math-ematical Physics, volume 1. Interscience Publishers,Inc., 1953.

[10] Manfredo P. do Carmo. Differential Geometry ofCurves and Surfaces. Prentice-Hall, 1976.

[11] B. Faverjon. Hierarchical object models for efficientanti-collision algorithms. In Proc. of the IEEE Interna-

tional Conference on Robotics and Automation, pages333–340, 1989. doi: 10.1109/ROBOT.1989.100010.

[12] P. Felzenszwalb and D. Huttenlocher. Distance trans-forms of sampled functions. Technical Report TR2004-1963, Cornell University, 2004.

[13] E. G. Gilbert, D. W. Johnson, and S. S. Keerthi. A fastprocedure for computing the distance between complexobjects in three-dimensional space. IEEE Journal ofRobotics and Automation, 4(2):193–203, 1988. doi: 10.1109/56.2083.

[14] D. Hsu, J.-C. Latombe, and R. Motwani. Path planningin expansive configuration spaces. In Proc. Conf. IEEEInt Robotics and Automation, volume 3, pages 2719–2726, 1997. doi: 10.1109/ROBOT.1997.619371.

[15] M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, andS. Schaal. Stomp: Stochastic trajectory optimizationfor motion planning. In Proc. IEEE Int Robotics andAutomation (ICRA) Conf, pages 4569–4574, 2011. doi:10.1109/ICRA.2011.5980280.

[16] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H.Overmars. Probabilistic roadmaps for path planningin high-dimensional configuration spaces. IEEE Trans-actions on Robotics and Automation, 12(4):566–580,1996. ISSN 1042-296X. doi: 10.1109/70.508439.

[17] A. Kelly, O. Amidi, M. Happold, H. Herman, T. Pi-larski, P. Rander, A. Stentz, N. Vallidis, and R. Warner.Toward reliable off-road autonomous vehicle operatingin challenging environments. In Proc. of the Interna-tional Symposium on Experimental Robotics, 2004.

[18] Hassan K. Khalil. Nonlinear Systems. Prentice Hall,2001.

[19] O. Khatib. Real-time obstacle avoidance for manip-ulators and mobile robots. International Journal ofRobotics Research, 5(1):90–98, 1986.

[20] Jr. Kuffner, J. J. and S. M. LaValle. Rrt-connect: Anefficient approach to single-query path planning. InProc. IEEE Int. Conf. Robotics and Automation ICRA’00, volume 2, pages 995–1001, 2000. doi: 10.1109/ROBOT.2000.844730.

[21] K. Lai, Liefeng Bo, Xiaofeng Ren, and D. Fox. A large-scale hierarchical multi-view RGB-D object dataset. InProc. of the IEEE International Conference on Roboticsand Automation, pages 1817–1824, 2011. doi: 10.1109/ICRA.2011.5980382.

[22] Maxim Likhachev and Dave Ferguson. Planninglong dynamically feasible maneuvers for autonomousvehicles. Int. J. Rob. Res., 28(8):933–945, 2009.ISSN 0278-3649. doi: http://dx.doi.org/10.1177/0278364909340445.

[23] M. C. Lin and J. F. Canny. A fast algorithm forincremental distance calculation. In Proc. of the IEEEInternational Conference on Robotics and Automation,pages 1008–1014, 1991. doi: 10.1109/ROBOT.1991.131723.

[24] S. Quinlan and O. Khatib. Elastic Bands: connectingpath planning and control. In Proc. Conf. IEEE Int

Robotics and Automation, pages 802–807, 1993. doi:10.1109/ROBOT.1993.291936.

[25] Sean Quinlan. Real-time modification of collision-freepaths. PhD thesis, Stanford University, 12 1994.

[26] Nathan Ratliff, Matt Zucker, J. Andrew Bagnell, andSiddhartha Srinivasa. CHOMP: Gradient optimizationtechniques for efficient motion planning. In Proc. ofthe IEEE International Conference on Robotics andAutomation, 2009.

[27] E. Rimon and D. E. Koditschek. Exact robot navigation

using cost functions: the case of distinct sphericalboundaries in en. In Proc. of the IEEE InternationalConference on Robotics and Automation, pages 1791–1796, 1988. doi: 10.1109/ROBOT.1988.12325.

[28] A. Stentz and M. Hebert. A complete navigationsystem for goal acquisition in unknown environments.Autonomous Robots, 2(2):127–145, 1995.

[29] Willow Garage. ROS CHOMP website, 2010. URLhttp://www.ros.org/wiki/chomp_motion_planner.