path planning and execution for deformable objects using a...
TRANSCRIPT
![Page 1: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/1.jpg)
Calder Phillips-Grafflin and Dmitry Berenson
Worcester Polytechnic Institute
Path Planning and Execution For Deformable
Objects Using a Voxel-Based Representation
1
![Page 2: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/2.jpg)
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
![Page 3: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/3.jpg)
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?
![Page 4: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/4.jpg)
Outline
4
• Background
• Voxel-based representation
• Deformation cost function
• Cost-space motion planning
• Intelligent path execution
• Results
• Conclusions
![Page 5: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/5.jpg)
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
![Page 6: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/6.jpg)
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
![Page 7: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/7.jpg)
Background – Execution
7
“Elastic Bands: connecting path planning and control”
(Quinlan et al, 1993)
Quinlan et al, 1993
![Page 8: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/8.jpg)
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
![Page 9: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/9.jpg)
Methods – Deformation Cost Function
9
• Sum of costs for all intersecting voxels
• Per-voxel weighted combination of costs from both objects
Cij A,B =𝐷𝑖 𝐴
𝐷𝑖 𝐴 + 𝐷𝑗 𝐵𝑆𝑖 𝐴 +
𝐷𝑗 𝐵
𝐷𝑖 𝐴 + 𝐷𝑗 𝐵𝑆𝑗 𝐵
![Page 10: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/10.jpg)
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
![Page 11: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/11.jpg)
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
![Page 12: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/12.jpg)
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
![Page 13: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/13.jpg)
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
![Page 14: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/14.jpg)
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
![Page 15: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/15.jpg)
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”
![Page 16: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/16.jpg)
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’
![Page 17: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/17.jpg)
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
![Page 18: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/18.jpg)
Results – Discrete Planning
18
• Paths executed by PR2 in
foam test environment
• Deformation tracked by
camera
• Calibrate planner with tracked
deformation
![Page 19: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/19.jpg)
Results – Discrete Planning
19
![Page 20: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/20.jpg)
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
![Page 21: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/21.jpg)
Results – Sampling-based Planning
21
• Motion planning in OpenRAVE using T-RRT and GradienT-RRT
• Simulator validation in Bullet
![Page 22: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/22.jpg)
Results – Sampling-based Planning
22
![Page 23: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/23.jpg)
Results – Sampling-based Planning
23
• GradienT-RRT finds solutions faster
• T-RRT finds solutions with lower cost
![Page 24: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/24.jpg)
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
![Page 25: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/25.jpg)
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
![Page 26: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/26.jpg)
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
![Page 27: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/27.jpg)
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
![Page 28: Path Planning and Execution For Deformable Objects Using a ...ewdom.irccyn.ec-nantes.fr/material/presentation/session2/Phillips... · • Calibrate planner with tracked deformation](https://reader034.vdocument.in/reader034/viewer/2022050311/5f73d80dd3df304129739d24/html5/thumbnails/28.jpg)
Questions?
28