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

Path Planning and Execution For Deformable Objects Using a Voxel-Based Representation Calder Phillips-Grafflin and Dmitry Berenson Worcester Polytechnic Institute 1

Motivation – Motion Planning • 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 Winer et al, 2012 2 Lakshmanan et al, 2012

Motivation - Execution • 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? 3

Outline • Background • Voxel-based representation • Deformation cost function • Cost-space motion planning • Intelligent path execution • Results • Conclusions 4

Prior work – Representation • 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) 5 Faure et al, 2011

Background – Motion Planning • 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) 6 Frank et al, 2011

Background – Execution “Elastic Bands: connecting path planning and control” (Quinlan et al, 1993) Quinlan et al, 1993 7

Methods – Representation • 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 8

Methods – Deformation Cost Function • Sum of costs for all intersecting voxels • Per-voxel weighted combination of costs from both objects 𝐸 𝑘 𝐶 𝐸 𝑗 𝐵 C ij A,B = 𝑇 𝑗 𝐵 + 𝑇 𝑘 𝐶 𝐸 𝑗 𝐵 + 𝐸 𝑘 𝐶 𝐸 𝑗 𝐵 + 𝐸 𝑘 𝐶 9

Methods – Discrete Planning • A* – suitable for 2D and 3D problems • Pareto-optimal combination of path length and deformation cost 𝑔(𝑦) = 1 − 𝑞 ∗ ℎ 𝑦 + 𝑕 𝑦 + 𝑞 ∗ 𝑒𝑓𝑔𝑝𝑠𝑛𝑏𝑢𝑗𝑝𝑜𝐷𝑝𝑡𝑢(𝑦) A* state value • Low p values result in shorter path • High p values result in lower deformation 10

Methods – Sampling-Based Planning • 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 • nFail Max controls temperature • Lower: faster planning • Higher: lower cost solutions Jaillet et al, 2010 11

Methods – Sampling-Based Planning • GradienT-RRT (Berenson et al, 2011) • Designed for narrow cost-space valleys • Derived from T-RRT • Project nodes using gradient Berenson et al, 2011 𝛼𝑟 = 𝐊(𝑟, 𝑦 1 , 𝑦 2 , … ) 𝑈 𝐷 1 𝛼𝑦 1 𝑈 , 𝐷 2 𝛼𝑦 2 𝑈 , … 𝑈 12

Methods – Execution • 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 13

Methods – Path Preprocessor • 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 14

Methods – Reactive Controller • Use cost gradient to locally improve path • Reject the cost gradient onto vector Q cur → Q n • “Correct” next state Q n with rejected gradient to form Q n * • All corrected states fall on “correction hyperplane” 15

Methods – Controller Constraints • Ensure that controller follows path within some bound • Ensure that controller never goes backwards • Ensure all Q n * are valid w.r.t. later states • If Q n * violates constraints, pull it back to the intersection of correction hyperplanes at Q n ’ 16

Results Outline • Discrete motion planning with PR2 and physical test environment • Sampling-based motion planning with simulation environment • Path preprocessor standalone testing • Reactive controller performance 17

Results – Discrete Planning • Paths executed by PR2 in foam test environment • Deformation tracked by camera • Calibrate planner with tracked deformation 18

Results – Discrete Planning 19

Results – Discrete Planning • 3D tests with P = [0,1] in 0.01 increments Robot P = 0.7 P = 0.01 P = 0.0 Length: 94 65 61 Deformation: 0 683 1062 Length: 73 58 57 Deformation: 81 159 310 20

Results – Sampling-based Planning • Motion planning in OpenRAVE using T-RRT and GradienT-RRT • Simulator validation in Bullet 21

Results – Sampling-based Planning 22

Results – Sampling-based Planning • GradienT-RRT finds solutions faster • T-RRT finds solutions with lower cost 23

Results – Path Preprocessor • 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 24

Results – Reactive Controller • 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 25

Conclusions • Efficient to compute – 50x to 200x faster than equivalent using Bullet • Suitable for discrete and sampling-based planners • Planners produce paths that minimize deformation 26

Conclusions • 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 27

Questions? 28