![Page 1: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/1.jpg)
FCL and
Motion Planning with Uncertainty
Jia Pan
Summer 2011
Mentor: Sachin Chitta
![Page 2: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/2.jpg)
Motivation
• Human environments • Clutter
• Dynamic obstacles
• Data from 3D sensors
– Large number of points • (~10,000 for laser scans, ~20,000 for stereo)
• Realtime computation important for fast online reactive grasping, motion planning
Fast, efficient collision checking is essential for any fast online robot operations in human environments
![Page 3: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/3.jpg)
Motivation
• Visual sensor range / coverage limited • E.g. PR2 cannot see top of bookshelf
• Object occlusion, sensor occlusion • Big issue in cluttered environment
• Parts of environment are unknown
• Imperfect, noisy sensing • Large uncertainty in environment representation
Motion planning should account for uncertainty in the environment
![Page 4: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/4.jpg)
My Contributions
• Collision checking
– FCL (Fast Collision Library)
• Motion planning with uncertainty
![Page 5: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/5.jpg)
Collision Checking
![Page 6: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/6.jpg)
Current Implementation in ROS
• Based on ODE
– about 10 years old
– state of the art in collision checking has advanced greatly
– no proximity (distance) computation
– cannot be parallelized easily (need thread lock)
– C style code without good documentation, difficult to extend
![Page 7: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/7.jpg)
Fast Collision Library (FCL)
• Fully templated code (C++)
• Designed to be easily extendable
– New state-of-the-art algorithms can be easily added
• Current features include
– Low level (object level) collision
– Broad phase collision checking
– Proximity
– Continuous collision detection (CCD)
– Deal with noisy point cloud data (experimental)
![Page 8: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/8.jpg)
Class Architecture
![Page 9: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/9.jpg)
Low level Collision Checking
• Mesh collision and mesh / shape collision
• Based on bounding volume hierarchy (BVH) to cull out the collision test of far-away parts
• Similar idea is used in distance computation
• Shape collision • Based on GJK / MPR algorithm to
provide collision/penetration depth information
![Page 10: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/10.jpg)
High Level Collision Checking
• Apply hierarchical culling technique to accelerate collision test among many objects – Especially useful for robot self
collision
• Called ‘collision manager’ in FCL – Four types of collision managers are provided
• Brute force
• ODE style
• Sweep and prune
• Interval tree
– Spatial subdivision methods will be added later.
![Page 11: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/11.jpg)
Continuous Collision Detection
• More rigorous guarantee for collision state of motion within a given time interval
– Return the contact time
![Page 12: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/12.jpg)
Results
• Implemented in two packages – fcl - an independent library with the core collision/proximity functions.
– collision_space_fcl
• Same API as collision_space
• Easy integration with motion planners, kinematics, etc.
• Integrated with ROS and the PR2 – Integrated with OMPL and the motion planning pipeline
– Tested in simulation and the PR2 robot
• Separate repository on kforge • (https://kforge.ros.org/project/fcl)
![Page 13: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/13.jpg)
Results
• Tested against ODE – 100 obstacles in random poses within robot workspace
– Robot model used is the PR2
– 1000 random configurations of the robot arms
• Average timings (in seconds)
– Shapes (basic primitives) are converted into meshes in FCL
– Shape collision checking needs further integration
Collision type Sphere/Mesh Cylinder/Mesh Box/Mesh Mesh/Mesh
ODE 0.27 0.31 0.34 6.19
FCL 0.34 0.37 0.37 0.72
![Page 14: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/14.jpg)
FCL
![Page 15: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/15.jpg)
Point Cloud Results
![Page 16: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/16.jpg)
Motion Planning with Uncertainty
![Page 17: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/17.jpg)
Environment Representation
• Octomap – Compact representation for
large scene
– Can model free space, occupied space and unknown space efficiently
– Encode the uncertainty in each cell
– Update when environment changes
![Page 18: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/18.jpg)
Planning with Costmap
• Transition-based RRT [Jaillet et al.]
– Compute a path with minimum overall variation
– Use annealing-alike method to balance between exploration and exploitation of random search process
![Page 19: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/19.jpg)
Planning with Costmap
• Temperature (T)
• low T: only low-cost RRT edges can be added more exploitation
• High T: high-cost RRT edges are allowed more exploration
• T update
• When one RRT edge is added, decrease T
• When RRT extend fails many times, increase T
![Page 20: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/20.jpg)
Planning with Uncertainty
• T-RRT does not meet our requirements – TRRT: path with minimum variations
– We need: path with minimum uncertainty
• Modification – Modify the cost of each RRT edge
cost(a, b) = f(b) + h(b) – f(a) – h(a)
= u(a, b) + h(b) – h(a)
• u: uncertainty cost from octomap intersection
• h: distance to goal
![Page 21: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/21.jpg)
Results
• Integrated with OMPL & ROS
• Tested on PR2
• Further testing needed
– Explore right set of parameters
– More scenarios
![Page 22: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/22.jpg)
Movie
![Page 23: FCL and Motion Planning with Uncertainty · Results • Implemented in two packages –fcl - an independent library with the core collision/proximity functions. –collision_space_fcl](https://reader033.vdocuments.mx/reader033/viewer/2022050108/5fab37c787ac237f8e35ec5a/html5/thumbnails/23.jpg)
Conclusion
• Collision checking
– Integrated into ROS and OMPL
– Released on kforge (separate repository called FCL)
– Should be ready for regular use
– Paper for ICRA 2012
• Planning with uncertainty
– Maybe for ICRA 2012
• Future work
– Combine RRT and A*