search

flexible-collision-library / fcl

Flexible Collision Library
https://flexible-collision-library.github.io/
Other
1.38k stars 417 forks source link

segfault when using a Plane in collision detection #12

Closed isucan closed 9 years ago

isucan commented 11 years ago

When a plane is added to a collision world in moveit (where an octree also exists), fcl segfaults. 

(gdb) 
#12 0x00007fffefb2e710 in collision_detection::CollisionWorldFCL::checkRobotCollisionHelper(collision_detection::CollisionRequest const&, collision_detection::CollisionResult&, collision_detection::CollisionRobot const&, robot_state::RobotState const&, collision_detection::AllowedCollisionMatrix const*) const ()
   from /u/moveit/catkin/devel/lib/libmoveit_collision_detection_fcl.so
(gdb) 
#11 0x00007fffebf66ec9 in fcl::DynamicAABBTreeCollisionManager::collide(fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) const () from /opt/ros/groovy/lib/libfcl.so
(gdb) 
#10 0x00007fffebf6385f in fcl::details::dynamic_AABB_tree::collisionRecurse(fcl::NodeBase<fcl::AABB>*, fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) () from /opt/ros/groovy/lib/libfcl.so
(gdb) 
#9  0x00007fffefb1f723 in collision_detection::collisionCallback(fcl::CollisionObject*, fcl::CollisionObject*, void*) ()
   from /u/moveit/catkin/devel/lib/libmoveit_collision_detection_fcl.so
(gdb) 
#8  0x00007fffec1ac835 in fcl::collide(fcl::CollisionObject const*, fcl::CollisionObject const*, fcl::CollisionRequest const&, fcl::CollisionResult&) ()
   from /opt/ros/groovy/lib/libfcl.so
(gdb) 
#7  0x00007fffec1acbb4 in unsigned long fcl::collide<fcl::GJKSolver_libccd>(fcl::CollisionObject const*, fcl::CollisionObject const*, fcl::GJKSolver_libccd const*, fcl::CollisionRequest const&, fcl::CollisionResult&) ()
   from /opt/ros/groovy/lib/libfcl.so
(gdb) 
#6  0x00007fffec1aca0e in unsigned long fcl::collide<fcl::GJKSolver_libccd>(fcl::CollisionGeometry const*, fcl::Transform3f const&, fcl::CollisionGeometry const*, fcl::Transform3f const&, fcl::GJKSolver_libccd const*, fcl::CollisionRequest const&, fcl::CollisionResult&) () from /opt/ros/groovy/lib/libfcl.so
(gdb) 
#5  0x00007fffec033001 in unsigned long fcl::ShapeShapeCollide<fcl::Plane, fcl::Cylinder, fcl::GJKSolver_libccd>(fcl::CollisionGeometry const*, fcl::Transform3f const&, fcl::CollisionGeometry const*, fcl::Transform3f const&, fcl::GJKSolver_libccd const*, fcl::CollisionRequest const&, fcl::CollisionResult&) ()
   from /opt/ros/groovy/lib/libfcl.so
(gdb) 
#4  0x00007fffec09b91f in fcl::ShapeCollisionTraversalNode<fcl::Plane, fcl::Cylinder, fcl::GJKSolver_libccd>::leafTesting(int, int) const ()
   from /opt/ros/groovy/lib/libfcl.so
(gdb) 
#3  0x00007fffec1a7eeb in fcl::details::GJKCollide(void*, void (*)(void const*, _ccd_vec3_t const*, _ccd_vec3_t*), void (*)(void const*, _ccd_vec3_t*), void*, void (*)(void const*, _ccd_vec3_t const*, _ccd_vec3_t*), void (*)(void const*, _ccd_vec3_t*), unsigned int, double, fcl::Vec3fX<fcl::details::Vec3Data<double> >*, double*, fcl::Vec3fX<fcl::details::Vec3Data<double> >*) ()
   from /opt/ros/groovy/lib/libfcl.so
(gdb) 
#2  0x00007fffea289b87 in ccdMPRIntersect ()
   from /opt/ros/groovy/lib/libccd.so.2
(gdb)
tobiaskunz commented 11 years ago

Ran into the same issue. The problem is that there is no template specialization of the GJKInitializer class for a Plane. Thus, the nonspezialized version is used, which returns a NULL pointer instead of a GJK object.

jslee02 commented 9 years ago

First, FCL calls GJK solver when there is no template specialization of the GJKSolver_libccd::shapeIntersect() for shape collision. However, there is GJKSolver_libccd::shapeIntersect<Cylinder, Plane>() that calls specific collision algorithm for cylinder-plane instead of GJK solver for cylinder-plane so it shouldn't returns NULL pointer.

The problem arises when the order of collision object pair is reversed (i.e., plane-cylinder). For the case, FCL attempts to call GJKSolver_libccd::shapeIntersect<Plane, Cylinder>() that doesn't exist. Then, FCL attempts to call GJK solver for plane-cylinder, which returns NULL pointer as @tobiaskunz explained.

I believe this issue fixed by #53. I haven't tried to test with moveit so if it's still a problem with it let me know.