Path Planning and Execution For Deformable Objects Using a - - PowerPoint PPT Presentation

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: 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