Calder Phillips-Grafflin and Dmitry Berenson

Worcester Polytechnic Institute

Path Planning and Execution For Deformable

Objects Using a Voxel-Based Representation

1

Motivation – Motion Planning

2

• Motion planning for deformable objects as an optimal motion

planning problem

• We want to minimize deformation

• Reduce risk of injury or damage

• Need a cost function for deformation that is fast to compute

Lakshmanan et al, 2012

Winer et al, 2012

Motivation - Execution

3

• Sensor and actuation error cause higher cost-as-executed

• Optimal paths are particularly vulnerable

• “Smarter” control strategies can improve execution

• Sensing local environment takes time

• Can we identify when to use smarter control in advance?

Outline

4

• Background

• Voxel-based representation

• Deformation cost function

• Cost-space motion planning

• Intelligent path execution

• Results

• Conclusions

Prior work – Representation

5

• Accurate models are expensive to compute

• Mass-spring (Gibson et al, 1997)

• FEM (Müller et al, 2002; Irving et al, 2004)

• Efficient discretized models

“Sparse Meshless Models of Complex Deformable Solids”

(Faure et al, 2011)

Faure et al, 2011

Background – Motion Planning

6

• Feasible deformations

(Bayazit et al, 2002; Gayle et al, 2005; Rodriguez et al, 2006)

• Minimizing deformation

• Trajectory optimization (Maris et al, 2010)

“Efficient Motion Planning for Manipulation Robots in

Environments with Deformable Objects” (Frank et al, 2011)

Frank et al, 2011

Background – Execution

7

“Elastic Bands: connecting path planning and control”

(Quinlan et al, 1993)

Quinlan et al, 1993

Methods – Representation

8

• Voxel-based representation of elastic objects

• Similar to Faure et al, 2011

• Two parameters per voxel

• Deformability [0,1]

• Sensitivity [0,∞)

• Deformability is the rigidity of the voxel

• Sensitivity is cost of completely deforming the voxel

Methods – Deformation Cost Function

9

• Sum of costs for all intersecting voxels

• Per-voxel weighted combination of costs from both objects

Cij A,B =𝐷𝑖 𝐴

𝐷𝑖 𝐴 + 𝐷𝑗 𝐵𝑆𝑖 𝐴 +

𝐷𝑗 𝐵

𝐷𝑖 𝐴 + 𝐷𝑗 𝐵𝑆𝑗 𝐵

Methods – Discrete Planning

10

• A* – suitable for 2D and 3D problems

• Pareto-optimal combination of path length and deformation

cost

𝑓(𝑥) = 1 − 𝑝 ∗ ℎ 𝑥 + 𝑔 𝑥 + 𝑝 ∗ 𝑑𝑒𝑓𝑜𝑟𝑚𝑎𝑡𝑖𝑜𝑛𝐶𝑜𝑠𝑡(𝑥)

• Low p values result in shorter path

• High p values result in lower deformation

A* state value

Methods – Sampling-Based Planning

11

• T-RRT (Jaillet et al, 2010)

• Tree growth controlled by cost

• Lower cost nodes added

automatically

• Higher cost nodes added

based on cost increase and

“temperature” T

• nFailMax controls temperature

• Lower: faster planning

• Higher: lower cost solutions Jaillet et al, 2010

Methods – Sampling-Based Planning

12

• GradienT-RRT (Berenson et al, 2011)

• Designed for narrow cost-space valleys

• Derived from T-RRT

• Project nodes using gradient

𝛻𝑞 = 𝐉(𝑞, 𝑥1, 𝑥2, … )𝑇 𝐶1𝛻𝑥1𝑇 , 𝐶2𝛻𝑥2

𝑇 , … 𝑇

Berenson et al, 2011

Methods – Execution

13

• Path preprocessor determines when to use reactive control

• Reactive controller adapts path during execution

• Execution process

• Motion planner generates new path

• Preprocessor labels new path

• Controller executes path, switching between control

modes

Methods – Path Preprocessor

14

• Identify need for reactive control at each state in path

• Per-state features

• Cost & derivative

• Curvature & derivative

• “Brittleness” – increase

in cost of worst neighbor

• Logistic regression classifier with L1 penalty

• Classify states

• Identify important features

Methods – Reactive Controller

15

• Use cost gradient to locally

improve path

• Reject the cost gradient onto

vector Qcur→Qn

• “Correct” next state Qn with

rejected gradient to form Qn*

• All corrected states fall on

“correction hyperplane”

Methods – Controller Constraints

16

• Ensure that controller follows

path within some bound

• Ensure that controller never

goes backwards

• Ensure all Qn* are valid w.r.t.

later states

• If Qn* violates constraints, pull

it back to the intersection of

correction hyperplanes at Qn’

Results Outline

17

• Discrete motion planning with PR2

and physical test environment

• Sampling-based motion planning with

simulation environment

• Path preprocessor standalone testing

• Reactive controller performance

Results – Discrete Planning

18

• Paths executed by PR2 in

foam test environment

• Deformation tracked by

camera

• Calibrate planner with tracked

deformation

Results – Discrete Planning

19

Results – Discrete Planning

20

Robot P = 0.7 P = 0.01 P = 0.0

• 3D tests with P = [0,1] in 0.01 increments

Length: 94 65 61

Deformation: 0 683 1062

Length: 73 58 57

Deformation: 81 159 310

Results – Sampling-based Planning

21

• Motion planning in OpenRAVE using T-RRT and GradienT-RRT

• Simulator validation in Bullet

Results – Sampling-based Planning

22

Results – Sampling-based Planning

23

• GradienT-RRT finds solutions faster

• T-RRT finds solutions with lower cost

Results – Path Preprocessor

24

• Training data

• 100 random 2D environments with narrow passages

• Optimal path planned with A*

• ~100,000 labelled states

• Train classifier with 90%

• 96% correctly classified

• Feature identification

• Cost at state

• Brittleness

Results – Reactive Controller

25

• Tested with 30 random environments

• Plan path

• Apply offset to environment

• Execute w/ open-loop control

• Preprocess path

• Execute w/ reactive control

• 7.7% reduction in total path cost as executed

• Oscillation in narrow passages can cause higher cost

Conclusions

26

• Efficient to compute – 50x to 200x faster than equivalent

using Bullet

• Suitable for discrete and sampling-based planners

• Planners produce paths that minimize deformation

Conclusions

27

• Preprocessor effective at identifying when to use reactive

control

• Specific path features are key to using reactive control

• Reactive controller can reduce cost-as-executed

Questions?

28