Pregrasp Manipulation as Trajectory Optimization › pub_files › 2013 › 6 › RSS13-librarian.pdfpregrasp manipulation motion that allow nonprehensile reconfiguration motion.

Pregrasp Manipulation as Trajectory OptimizationJennifer King, Matthew Klingensmith, Christopher Dellin,

Mehmet Dogar, Prasanna Velagapudi, Nancy Pollard and Siddhartha SrinivasaThe Robotics Institute, Carnegie Mellon University

{jeking,mklingen,cdellin,mdogar,pkv,nsp,siddh}@cs.cmu.edu

Abstract—We explore the combined planning of pregraspmanipulation and transport tasks. We formulate this problemas a simultaneous optimization of pregrasp and transporttrajectories to minimize overall cost. Next, we reduce thissimultaneous optimization problem to an optimization of thetransport trajectory with start-point costs and demonstratehow to use physically realistic planners to compute the costof bringing the object to these start-points. We show howto solve this optimization problem by extending functionalgradient-descent methods and demonstrate our planner ontwo bimanual manipulation platforms.

I. Introduction

We address the problem of pregrasp manipulation,where an object must be manipulated to a feasibleor convenient location before it can be grasped. Suchsituations occur often in the real world where objectsare inaccessible or inconveniently placed: a thin bookthat must be slid to the edge of a table to be graspedand placed in a bookshelf, or a heavy drill that must bepulled close to be grasped and lifted (Fig.1). Pregraspmanipulation makes impossible manipulation tasks pos-sible and hard tasks easier.

In addition to the complexity of motion planningin high-dimensional manipulator configuration spaces,pregrasp manipulation is further complicated by thetight coupling between the two phases of reconfiguration,where the object is moved to a grasp, and transport,where the grasped object is moved to its goal.

As a consequence, prior work ([27], detailed in Sec-tion II) has focused on producing feasible solutions tothe pregrasp manipulation problem. Some work [4] doesexplore optimality, but only in a restricted setting of thesingle pose of the object just before it is being grasped.

Furthermore, these works all rely on the object beingrigidly grasped at all times. But, as we saw in the aboveexamples, pregrasp manipulation is often achieved notjust by grasping but by pulling, pushing, sliding, or othernonprehensile physics-based actions, using the supportsurface to provide stability.

In this paper, we strive to overcome both of theselimitations by developing algorithms to produce optimalpregrasp manipulation motion that allow nonprehensilereconfiguration motion.Optimality. Our first contribution is a formulation ofpregrasp manipulation as trajectory optimization (Sec-tion III). Through a series of reductions, we demonstratehow the coupled reconfiguration and transport problem

Fig. 1. The robot slides a book to the edge of a table, grasps the bookspine, and places it into a bookshelf (top). The robot reconfigures aheavy drill to pick it up from a table (bottom).

can be reduced to a single constrained trajectory opti-mization problem.

Our second contribution is to solve this constrainedtrajectory optimization problem by extending functionalgradient optimization techniques [8, 26] to address con-straints and costs on starting configurations (Section IV).Nonprehensile actions. Our third contribution is a for-mulation of reconfiguration with nonprehensile quasi-static pushing actions (Section V). This allows us toderive analytical bounds on the types of actions we canperform on the object. We exploit one such interestingbound: that the motion of a pushed or pulled object likea book by the fingertips can be modeled as a bounded-curvature Dubins car. This allows us to use extremelyefficient lattice-style planners [18] used on ground vehi-cles for pushing.

We demonstrate an implementation of our algorithmon two bimanual manipulation platforms for the two dif-ferent pregrasp manipulation tasks mentioned initially:pushing a book to the edge of a table, grasping it, andplacing it to a bookshelf; and reconfiguring a heavy drillto pick it up from a table. Our experiments (Section VI)show that our algorithm has a significantly greater suc-cess rate and lower cost compared to a planner that onlyoptimizes transport cost. This does come at the slightcomputational cost of solving the joint problem.

Our work takes a step towards incorporating physics-based reconfiguration actions into trajectory optimiza-

Fig. 2. An example manipulation problem involving a reconfigurationtrajectory, a grasp, and a transport trajectory.

tion techniques. We are encouraged by our results.

II. Related Work

This work builds on trajectory-optimization ap-proaches for arm motion planning, such as those in-volving elastic bands [25], or covariant Hamiltonianoptimization [26]. General frameworks for trajectory op-timization with contact constraints also exist [10, 24].Our method takes advantage of the structure of pregraspmanipulation by identifying the points of regraspingand reducing the dimensionality of the search space.We use these points of regrasping as constraints duringoptimization [8].

Sampling-based approaches to arm planning, based onroadmaps [15], or trees [16] exist as well, for both freeand constrained [3] spaces. Simeon [27] uses roadmapsto study the problem of manipulating objects constrainedsuch that they require multiple pick and place operationsto move to desired goal configurations. The proposedmethod produces long chains of pick and place opera-tions, but does not consider optimality.

Several studies explore grasp metrics which considerboth the grasp itself [5, 11] and the resulting transporttrajectory [4, 14]. Such works attempt to solve the sameclass of problems as this work.

The general problem of nonprehensile manipulationhas also been explored at length, whether by pushing inthe plane [7, 19], tipping [22], pivoting [1], or caging [6].These works focus on producing a desired change in theobject of interest without considering potential subse-quent prehensile manipulations.

III. Pregrasp Manipulation as TrajectoryOptimization

Consider the problem presented in Fig.2, where a ma-nipulator must move an object which cannot be graspedin its initial configuration. Instead, the manipulator per-forms some reconfiguration to get the object to a graspablelocation, grasps it, then transports the object to its finallocation. In this work we consider this general classof problem, in which manipulation is decomposed intoreconfiguration and transport trajectories. Starting froma general formulation of trajectory optimization withpregrasp manipulation, we apply a series of reductionsto find a form that is easily accessible to functionalgradient optimization methods.

A. Functional gradient optimization

We define state x = (q, o) ∈ X = C × SE(3) asthe configuration of the robot q ∈ C and the pose ofthe object o ∈ SE(3). We control the motion of therobot, and the robot can move the object. It can do thisby grasping, pulling, pushing, or loosely caging it. Wedefine the mechanics of this manipulation in more detailin Section V.

Our objective is to get the object from some start osto some goal og. We seek to find the best trajectory ξ :[0, 1] → X that achieves this. To do this, we define acost functional (a function of a function) U : Ξ→ R+ tominimize, where Ξ is a Hilbert space of trajectories:

ξ∗ = argminξ

U[ξ] s.t. ξ(0) = (·, os); ξ(1) = (·, og) (1)

Note that here, and henceforth, we use · to mean anyfeasible q ∈ C, e.g. an element from the set of all inversekinematic solutions of the arm that grasp o. Also, whileour examples consider reconfiguration actions before thetransport, the approach is equally applicable to suchactions after transport.

B. Pregrasp manipulation

We consider the class of pregrasp manipulation prob-lems where the object is manipulated once before itis grasped. Thus, motion decomposes into two phases:reconfiguration (R), and transport (T):

ξ = ξR[0, τ]⊕ ξT [τ, 1] (2)

This allows us to rewrite (1) as:

ξ∗ = argminξ

U[ξR ⊕ ξT ] = argminξR ,ξT

(U[ξR] + U[ξT ])

s.t. ξR(0) = (·, os); ξT(1) = (·, og); ξR(τ) = ξT(τ)(3)

Unfortunately, the two optimizations are tightly cou-pled at ξ(τ), the state at which the two trajectories meet.

Our second reduction loosens this coupling by allow-ing the robot to release and grasp the object between thetwo phases for free. As a consequence, the only couplingbetween the two phases is the pose of the object, i.e.

ξR(τ) = (·, oτ) and ξT(τ) = (·, oτ), where oτ is the poseof the object when it is grasped.

This restricts oτ to the much smaller set S ⊂ SE(3)which are (1) a stable resting configuration for the object,(2) reachable by reconfiguration, and (3) accessible tograsping for transport.

This allows us to tease apart (3). Our next reduc-tion is driven by computational efficiency. We restrictreconfiguration cost to functions only of object pose:U[ξR] , U[oR], where the trajectory ξR = (qR, oR). Thisreduction enables us to compute reconfiguration costextremely quickly. We then use standard graph-searchtechniques (explained in detail in Section V) to find theminimum reconfiguration cost of bringing the object toa particular oτ :

u∗R(oτ) = minξR

U[ξR] s.t.

{ξR(0) = (·, os)ξR(τ) = (·, oτ)

(4)

This allows us to rewrite (3) in its final form as anoptimization of the transport trajectory which also takesinto account the reconfiguration cost:

ξ∗T = arg minξT

(U[ξT ] + u∗R(oτ)) s.t. ξT(τ) = (·, oτ ∈ S)(5)

We have finally reduced our problem to a solitary tra-jectory optimization problem with a constraint on thestarting configuration of the trajectory. In the followingsection, we describe an extension of CHOMP, a func-tional gradient optimization technique used often in armmotion planning [26], to solve this problem.

IV. CHOMP with Start Costs

A. CHOMP

CHOMP [26] solves the unconstrained functional op-timization problem (1). Given a norm A in the Hilbertspace Ξ, CHOMP minimizes the regularized Taylor se-ries approximation of U about the current trajectory ξi:

ξi+1 = argminξ{U[ξi] + ∇̄UT(ξ − ξi) +

λ

2||ξ − ξi||2A} (6)

This can be minimized exactly to give the update rule:

ξi+1 = ξi −1λ

A−1∇̄U (7)

with the functional gradient operator:

∇̄ = ∂∂ξ− d

dt∂

∂ξ ′(8)

CHOMP models the cost function, U [ξ], as a trade-off between a “prior” smoothness cost, fprior, and anobstacle cost, fobs, bending the trajectory away fromobstacles:

U [ξ] = fprior [ξ] + fobs [ξ] (9)

A variant of CHOMP that addresses trajectory-wideconstraints, called GSCHOMP [8] solves the problem:

ξ∗ = argminξ

U [ξ] s.t. hξ = 0 (10)

By additionally linearizing the constraint about thecurrent trajectory ξi as h(ξ) = C(ξ − ξi) + b, GSCHOMPobtains a simple Newton’s method-style update:

ξi+1 = ξi −1λ

A−1∇̄U (11a)

+1λ

A−1CT(CA−1CT)−1CA−1∇̄U (11b)

− A−1CT(CA−1CT)b (11c)

where (11a) is the unconstrained update (7), (11b) is theprojection onto the zero set C(ξ− ξi) = 0 and (11c) is theoffset correction to further project onto C(ξ− ξi)+ b = 0.

B. Start Costs

Our final reduction (5) posed the reconfiguration prob-lem as optimizing a functional with a start constraint,which GSCHOMP addresses, but also a start cost, whichwe address now.

A key issue with start costs u∗R(oτ) is that they areonly defined on the configuration space submanifoldwhere the object is in S. As a result, there is no gradientinformation available when the optimizer is not on themanifold, which happens very often due to linearizationand numerical precision.

We address this by lifting the function and its gradientinto C by projecting any configuration onto the subman-ifold and using that cost u∗R(q) = u

∗R(proj(q)). Further-

more, we use the workspace projection for efficiency.Once lifted, we can define the start cost functional as:

U∗R[ξ] = u∗R(ξ(τ)) (12)

This cost functional is then added to U[ξ] (9).A ‘good’ projection would ideally lift the cost onto the

tangent space of the linearized constraint, and therebyresult in no component along (11b). However, the com-putational cost of finding this projection overwhelms thesimplicity of just eliminating the component with (11b).

C. Representing the Constraint

While the above formulation is general and will workfor a wide variety of pregrasp manipulation actions, inthis study we focus on the class of problems wherereconfiguration is accomplished via nonprehensile ac-tions such as pushing and rotating the object on a table.Therefore, we have the constraint, hξT , that the transporttrajectory starts by grasping the object off the planedescribed by the table.

At the starting point, ξT(τ) = (q, oτ), the robot con-figuration and the object pose is related by:

oτ = FK(q) Tg (13)

Fig. 3. Pushing model. (a) Friction cones at the contacts. (b) The limit surface. (c) The cone of all possible forces that can be applied by thehand. (d) A subset of the forces which create linear velocity along a single direction and a bounded range of angular velocities. (e) The Dubinscar model.

where Tg is the grasp transform representing the relativepose of the object in the robot end-effector, and FK is theforward-kinematics of the robot giving the end-effectorpose in the world.

We describe the table using the xy-plane of the coor-dinate frame Tt. Then, using (13), we can represent theconstraint that the object must be on this plane:

h(ξT) = S T−1t FK(q)Tg

where S is a selector matrix

S =

0 0 1 0 0 00 0 0 1 0 00 0 0 0 1 0

(14)which selects the z-axis, the rotation around x, and therotation around y. The result is a 3-dimensional vectorwhich is 0 on the constraint.

We can find C through straightforward differentiation.Since S, Tt and Tg are constant, we have:

C = S T−1t J(q) Tg (15)

where J(q) is the Jacobian of the end effector motionevaluated at q.

V. The Mechanics of Pregrasp Manipulation

We build our reconfiguration planner based on theanalysis of the quasi-static interactions between the robothand and a pushed object. We illustrate the robot handand an object in Fig.3-(a). Using the coefficient of frictionbetween the hand and the object, µ, we can draw frictioncone [23] edges at an angle tan−1(µ) with the contactnormal at the contact points. Coulomb’s law dictates thatthe possible pushing forces that the hand can apply arelimited by the friction cones.

The limit surface [12, 13] relates the generalized forcesapplied on an object to the resulting generalized velocity.We assume that the object applies finite pressure to theunderlying surface and the friction between the two isalso finite. Then the limit surface is smooth and strictlyconvex, similar in shape to a three-dimensional ellipsoid(Fig.3-(b)) where the dimensions are fx, fy, and m, theforce along x-direction, the force along y-direction, andthe moment respectively. During quasi-static pushing the

generalized forces applied to the object are exactly on thelimit surface. Given such a generalized force we can findthe object’s generalized velocity using the normal to thelimit surface at that point, and replacing the dimensionsto be vx, vy, and ω, the linear velocity along x, the linearvelocity along y, and the angular velocity, respectively.

In Fig.3-(c) we illustrate the three-dimensional conewhich corresponds to all the combined forces that canbe applied by the forces in the friction cones. Thecorresponding cone of generalized velocities gives thepossible velocities the object can move with [9]. Severalstudies use this analysis to study the controllability andplanning of pushing [2, 20, 21]. In this study we usea subset of these velocities, shown in Fig.3-(d), whichcorrespond to a single linear velocity and a boundedrange of angular velocities. This simplifies our pushingsystem to a Dubins car [17] for planning purposes (Fig.3-(e)). This model allows us to take advantage of a widerange of research done in motion planning for car-likevehicles. In particular, we choose to perform a graphsearch over a lattice state space [18].

Our action space includes the pushing actions andadditionally switching actions where the robot changesthe side of the object it is pushing on. In this sense, wemodel each object as a Dubins car that can move alongthe four primary directions. During planning, we checkeach action for collision between the pushed object andobstacles. We also check for collision between the end-effector and obstacles.

We define the cost of reconfiguration, u∗R(oτ), as thedistance the object must travel from its starting pose,os, to the grasp pose, oτ . Using such a planner inthe low-dimensional state space enables us to generatethe reconfiguration cost for an intermediate pose, oτ ,very quickly. This formulation associates zero cost withswitching pushing sides, as the object does not moveduring this action.

While this planner is very fast and physically realistic,it produces only the trajectory of the end-effector andthe object. We find the corresponding arm trajectoryseparately. This step can fail due to the arm’s kinematicinfeasibility, in which case we re-run our planner remov-ing the offending action edges from our search graph.

Fig. 4. The 9 scenes used to test the algorithm. In each scene, thegreen object represents the book to be reconfigured. The blue objectsrepresent obstacles.

Fig. 5. Valid push paths for two initial configurations of the book.In the left configuration, the book is first pushed around the obstacle.The hand must then change sides of the book to continue the push tothe edge of the table. In the right configuration, the book is pusheddirectly to the edge of the table.

VI. Experiments

A. Book Manipulation

We compare our algorithm Reconfiguration CHOMPwith two alternatives, each of which optimizes thetwo costs sequentially instead of jointly: (A) Pre-GraspCHOMP, which first computes the object pose that min-imizes reconfiguration cost, and then fixes that poseand optimizes the transport cost via CHOMP; (B) Start-Set CHOMP, which optimizes transport cost by settingreconfiguration cost, u∗R(oτ) to zero in (5) but is seededat the optimum of the reconfiguration cost.

We validate two key hypotheses:(H1) Planning time vs. solution cost: ReconfigurationCHOMP significantly lowers path cost and increasessuccess rate, but at a significant increase in planningtime. However, the positive effect on path cost andsuccess rate persists even when other planners are giventhis extra time to improve their solutions.(H2) Reconfiguration vs. transport cost tradeoff: Recon-figuration CHOMP behavior is sensitive to the relativeweighting between reconfiguration and transport costs.Problem setup. We first consider a book manipulationscenario, where we task our robot to place a book lyingin the middle of a table into a bookshelf (Fig.4). The bookmust first be reconfigured to the table edge, where it canthen be stably grasped and transported to the bookshelf.The reconfiguration path of the book must avoid contactwith any obstacles on the table.

0!

1!

2!

3!

4!

5!

6!

7!

8!

!

Seco

nds (

s)!

Computation Time!

Start-Set!Full Reconf!

(a)! (b)! (c)!

0!

100!

200!

300!

400!

500!

600!

700!

!

Cost!

0!10!20!30!40!50!60!70!80!90!

100!

!

Perc

enta

ge (%

)!

Success Rate!606.67!

531.11!64.28!

83.33! 6.643!

3.91!

Fig. 6. A comparison between success rate, cost and computationtime between Start-Set CHOMP and Full Reconfiguration CHOMP. In(c), the light green shows the extra cost required to generate the fullreconfiguration cost function.

Fig.5 shows examples of reconfiguration plans auto-matically computed by our planner, with object pathsas well as pushing directions. Our constraint surface isdefined by reachable poses of the book which lie onedges of the table. To construct the lattice graph, wediscretize the table’s surface into 1 cm by 1 cm axis-aligned grid cells. We then use numerical differentiationto compute the reconfiguration cost gradient. DuringCHOMP iterations, we use linear interpolation to com-pute the continuous gradient.

We construct 9 test scenes, shown in Fig.4, which varyin number of obstacles and start location of the book. Foreach test scene, we define 14 initialization trajectories,resulting in 126 test cases for each of the 3 algorithms.The seed trajectory is a straight line trajectory in config-uration space with start point uniformly sampled fromour constraint surface. All 14 seed trajectories share acommon goal.Dependent measures. We compare end-to-end planningtime, solution cost, and success rate. We define successas a feasible collision-free trajectory.

1) (H1) Planning time vs. solution cost: Our results areshown in Fig.6. In every case, we allow all algorithmsto run multiple times (with random restarts) until theslowest algorithm has completed.

Our planner shows a 29% improvement in success rate.The most common cause of failure is selection of anintermediate pose that is infeasible to achieve becauseit is occupied by an obstacle.

We compare total path cost across all tests where bothalgorithms were successful. Our algorithm demonstratesa 12% improvement over Start-Set CHOMP. A t-testshows this difference is in fact statistically significant(t(50) = 16.58, p < 0.001). We note that the size ofthe cost improvement is sensitive to the scale of thereconfiguration cost relative to the transport cost. Thiswill be further discussed in Section VI-A2.

Next, we consider single-run performance time. Theincreased difference between planning time is statisti-cally significant (t(50) = 36.03, p < 0.001). Much of thisincrease can be attributed to calculating the reconfigura-tion cost function.

0  

20  

40  

60  

80  

100  

120  

140  

160  

180  

Total Cost: !157.81!

(a)!

0  

20  

40  

60  

80  

100  

120  

140  

160  

180  

Total Cost: !109.84!

(b)!

0"

20"

40"

60"

80"

100"

120"

140"

160"

180"

""

Total Cost: !108.9!

(c)!Reconfiguration!Transport!

0  

200  

400  

600  

800  

1000  

1200  

Total Cost: !764!

(a)!

0  

200  

400  

600  

800  

1000  

1200  

Total Cost: !1041.7!

(b)!

0  

200  

400  

600  

800  

1000  

1200  

Total Cost: !804.9!

(c)!Reconfiguration!Transport!(c)!

Fig. 7. Comparison of reconfiguration and transport trajectories for a single scene between three algorithms: (a) Pre-Grasp CHOMP (b) Start-SetCHOMP (c) Reconfiguration CHOMP. The top images show the results when transport cost is weighted high. The bottom images show resultswhen reconfiguration cost is weighted high.

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1101

102

103

Cos

t!

Reconfiguration Cost Weighting (α)!

Start-Set!Full Reconf!

Pre-Grasp!

0 1

102

101

Fig. 8. Comparative performance of algorithms as cost is weightedbetween pure reconfiguration (α = 1) and pure transport cost (α = 0).

2) (H2) Reconfiguration vs. transport cost tradeoff: To testthis hypothesis we use a subset of a our 126 test cases.In particular, we examine only the test cases where theseed trajectory starts at a configuration correspondingto the optimal reconfiguration pose. Using this subsetallows us to create a reconfiguration-informed Start-SetCHOMP algorithm. It now starts from a point of optimalreconfiguration, but still optimizes only over transportcost.

Fig.7 shows the selected intermediate location andassociated object path and transport trajectory for each ofthe three algorithms for a single scenario. The top threeimages show a scenario where transport cost is large rel-ative to reconfiguration cost. The bottom row of imagesshow a scenario with reconfiguration cost increased. Ascan be seen, Reconfiguration CHOMP selects a shorterreconfiguration trajectory when reconfiguration cost has

large weight. Conversly, the trajectories selected by theother two algorithms are unaffected.

We further explore the performance of the three al-gorithms as we trade importance between reconfigu-ration and transport cost in Fig.8. Here we show thechange in total path cost as the relative weight of thereconfiguration cost is increased. When relative weightis very small, our algorithm reduces to Start-Set CHOMP,optimizing only transport cost. As reconfiguration costbecomes more important, our algorithm tends towardperformance of Pre-Grasp CHOMP, optimizing only re-configuration cost.

The best performing algorithm varies with α. Predict-ing which algorithm will perform best for a given α isdifficult, as this varies by task. One obvious alternativeto Reconfiguration CHOMP is to run Pre-Grasp CHOMPand Start-Set CHOMP for a given α and use the mini-mum cost trajectory. We note that while in some casesthis technique will provide a lower cost solution, it comeswith the time penalty incurred by running two planners.

B. Lifting a Drill

Problem Setup We next examine a scenario where therobot is tasked with using a small impact drill (Fig.1-bottom) to remove lugnuts from a wheel hub. In orderto use the drill, the robot must first lift it off the table.Often the pose and weight of the drill make it impossibleto find a valid grasp due to joint torque limits. Instead,the robot must first reconfigure the drill by sliding it to apose where a feasible lift trajectory can be found.

In these experiments, our constraint surface is definedby reachable poses of the drill on the table. We assumethe drill can slide and rotate in the plane of the table.Like with the book task described previously, we wishto define a reconfiguration cost function that penalizes

a trajectory based on the length of the reconfiguration.In addition, we wish to penalize poses which requireslarge joint torques when lifting the drill. Thus we definea two part reconfiguration cost function:

u∗R(oτ) = β f u f (oτ) + βdud(oτ) (16)

where u f (oτ) represents the joint torque penalty, ud(oτ)represents the cost based on the length of the reconfigu-ration trajectory and β f , βd ∈ R are weighting constants.

We define ud(oτ) as the translational distance betweenoτ (the pose of the object at the start of the lift) andthe initial pose, os. The gradient of this cost functionis straightforwardly computed as the vector from oτ toos passed through the pseudo-inverse of the kinematicJacobian of the robot.

For the second part of our reconfiguration cost func-tion, we consider joint torques. If we assume that thedrill, when grasped by the end-effector, produces a forceF due to gravity, the joint torque can be calculated byJ(q)T F, where J(q) is the kinematic translational Jacobianat configuration q. Using this information, we can definea cost function which represents the sum squared jointtorques:

u f (oτ) = FT J(qτ)T J(qτ)F (17)

where qτ is a valid configuration of the manipulatorwhen grasping the drill at pose oτ . Here poses whichrequire high torque for the drill to be lifted are penalizedwith higher reconfiguration cost. The gradient of thisfunction contains the kinematic Hessian, and is difficultto compute directly. Instead, we use finite differencingto approximate the gradient.

For each experiment, we randomly select the initialpose for the drill from the set of reachable poses. Therobot is given a goal of lifting the object and thenextending its arm outward. Other than the table, no otherobstacles exist in the environment. Fig.9 shows an exam-ple solution. As can be seen, the optimal reconfigurationpose, oτ is closer to the robot than the initial pose, os.

Also shown in the figure is the effect of varying theparameters β f (the weight on the torque cost function),and βd (the weight on the reconfiguration cost function)given some initial starting pose (green). The resultingsteps of optimization (orange) push the object toward alocal minimum of the feasibility cost function given theweights on each component. As β f increases relative toβd, the drill is pushed closer to the robot so that the jointsuse the minimal energy to lift the drill. In contrast, as βdincreases, the drill’s configuration is pushed closer andcloser to its initial configuration. At very high valuesof β f , the resulting reconfiguration motion is a purerotation.

In each of these experiments, the performance ofCHOMP was not very adversely limited by incorporat-ing constraints and additional loss functions. Averageper-iteration time increased from 30 ms to 38 ms from

single-goal CHOMP [26], and total planning time in-creased from 3.1 seconds to 3.9 seconds (for 100 itera-tions).

VII. Discussion and Future Work

Trajectory optimization is a powerful tool to generateintelligent behavior from simple costs. Pregrasp manip-ulation, however, requires planning for multiple modesof prehensile and nonprehensile interaction, e.g. pick-and-place, pushing, pulling, toppling. These modes oftenhave very different system dynamics, meaning optimiza-tion over the complete manipulation trajectory requiresa very high-dimensional state space and a complicatedjoint description of the system dynamics. Instead, weobserve that the complete manipulation trajectory can bedivided into steps at the regrasp points. This enables usto run specialized, lower-dimensional planners for eachdifferent mode of interaction, while still sharing the costof each mode with neighboring modes at the regrasppoints.

In this paper we present results on tasks which requireonly two modes of interactions: pushing and pick-and-place. However, our general formulation can easily beextended to work with other modes of interactions, e.g.toppling. Similarly, it can be extended for tasks whichrequire more than two steps of interaction.

Planning multiple steps of open-loop manipulationactions makes them vulnerable to growing uncertaintyduring the execution of these actions. For example, aftermultiple pushing actions, an object might end up in adifferent spot than the original goal, breaking the con-nection to subsequent transport trajectories which havealready been planned. An interesting extension wouldbe to integrate a cost for uncertainty-inducing actionsinto the optimization process to increase the robustnessof the resulting plans.

Our optimization process uses the gradient of thereconfiguration costs computed for possible object graspposes at a high resolution. This computation creates anoverhead in computation time. In future work we aimfor a tighter integration between the transport trajectoryoptimization and the reconfiguration planning, such thatreconfiguration planning can be dynamically invoked asneeded by the optimizer.

Acknowledgments

This material is based upon work supported by NSFGrant No. 1208388, NSF-EEC-0540865, DARPA-BAA-10-28 and the Toyota Motor Corporation. We thank themembers of the Personal Robotics Lab for very helpfuldiscussion and advice.

References[1] Y. Aiyama, M. Inaba, and H. Inoue. Pivoting: A new method of graspless

manipulation of object by robot fingers. In IEEE/RSJ IROS, 1993.[2] S. Akella and M. T. Mason. Posing polygonal objects in the plane by

pushing. International Journal of Robotics Research, 17(1):70–88, January 1998.

L ifting Tra jectory

Low TorqueSolution

No Feasability Cost

Low Distance Solution

Fig. 9. Comparison of solutions as torque and distance cost weighting is varied.

[3] D. Berenson, S. Srinivasa, and J. Kuffner. Task Space Regions: A frameworkfor pose-constrained manipulation planning. IJRR, 30(12):1435–1460, 2011.

[4] L. Chang, S. Srinivasa, and N. Pollard. Planning pre-grasp manipulationfor transport tasks. In IEEE ICRA, 2010.

[5] L. Y. Chang and N. Pollard. Posture optimization for pre-grasp interactionplanning. In IEEE ICRA, Workshop on Manipulation Under Uncertainty, 2011.

[6] R. Diankov, S. Srinivasa, D. Ferguson, and J. Kuffner. Manipulation planningwith caging grasps. In IEEE-RAS Humanoids, 2008.

[7] M. Dogar and S. Srinivasa. A planning framework for non-prehensilemanipulation under clutter and uncertainty. Autonomous Robots, 33(3):217–236, 2012.

[8] A. Dragan, N. Ratliff, and S. Srinivasa. Manipulation planning with goalsets using constrained trajectory optimization. In IEEE ICRA, 2011.

[9] M. Erdmann. On a representation of friction in configuration space.International Journal of Robotics Research, 13(3), 1994.

[10] T. Erez and E. Todorov. Trajectory optimization for domains with contactsusing inverse dynamics. In IEEE/RSJ IROS, pages 4914–4919. IEEE, 2012.

[11] M. Gienger, M. Toussaint, and C. Goerick. Task maps in humanoid robotmanipulation. In IEEE/RSJ IROS, 2008.

[12] S. Goyal, A. Ruina, and J. Papadopoulos. Planar sliding with dry friction.Part 1. Limit surface and moment function. Wear, (143):307–330, 1991.

[13] R. D. Howe and M. R. Cutkosky. Practical Force-Motion Models for SlidingManipulation. IJRR, 15(6):557–572, 1996.

[14] D. Kappler, L. Y. Chang, M. Przybylski, N. Pollard, T. Asfour, and R. Dill-mann. Representation of pre-grasp strategies for object manipulation. InIEEE-RAS Humanoids, December 2010.

[15] L. Kavraki, P. Svestka, J.-C. Latombe, and M. Overmars. Probabilistic

roadmaps for path planning in high-dimensional configuration spaces. InIEEE ICRA, 1996.

[16] S. LaValle and J. Kuffner. Randomized kinodynamic planning. IJRR,20(5):378–400, 2001.

[17] S. M. LaValle. Planning Algorithms. Cambridge University Press, New York,NY, USA, 2006.

[18] M. Likhachev and D. Ferguson. Planning long dynamically-feasible ma-neuvers for autonomous vehicles. In RSS, 2008.

[19] K. Lynch and M. T. Mason. Controllability of pushing. In IEEE ICRA, 1995.[20] K. M. Lynch. Locally controllable manipulation by stable pushing. 15(2):318

– 327, April 1999.[21] K. M. Lynch and M. T. Mason. Stable Pushing: Mechanics, Controllability,

and Planning. 15(6):533–556, 1996.[22] Y. Maeda, H. Kijimoto, Y. Aiyama, and T. Arai. Planning of graspless

manipulation by multiple robot fingers. In IEEE ICRA, 2001.[23] M. T. Mason. Mechanics and Planning of Manipulator Pushing Operations.

IJRR, 5(3):53–71, 1986.[24] M. Posa and R. Tedrake. Direct trajectory optimization of rigid body

dynamical systems through contact. In WAFR, pages 527–542. Springer,2013.

[25] S. Quinlan and O. Khatib. Elastic bands: Connecting path planning andcontrol. In IEEE ICRA, 1993.

[26] N. Ratliff, M. Zucker, J. Bagnell, and S. Srinivasa. CHOMP: Gradientoptimization techniques for efficient motion planning. In IEEE ICRA, 2009.

[27] T. Simeon, J.-P. Laumond, J. Cortes, and A. Sahbani. Manipulation planningwith probabilistic roadmaps. IJRR, 23(7–8):729–746, 2004.

I IntroductionII Related WorkIII Pregrasp Manipulation as Trajectory OptimizationIII-A Functional gradient optimizationIII-B Pregrasp manipulation

IV CHOMP with Start CostsIV-A CHOMPIV-B Start CostsIV-C Representing the Constraint

V The Mechanics of Pregrasp ManipulationVI ExperimentsVI-A Book ManipulationVI-A1 (H1) Planning time vs. solution costVI-A2 (H2) Reconfiguration vs. transport cost tradeoff

VI-B Lifting a Drill

VII Discussion and Future Work