Potentially upgrade to newer version of FCL

[wxmerkt]

• Indigo currently uses FCL 0.3.x (0.3.4 as of writing)
• Kinetic uses FCL 0.5, with the requirement for Octomap 1.8
• Upstream FCL is on track to release 0.6 which has narrow phase signed distance support.

There are a ton of fixes regarding computing distances including signed distances in the current head and I consider it worth using the new FCL version.

Of particular note:

• Exact negative distance computation using additional collision checking flexible-collision-library/fcl#172.
• Issues and fixes discussed in TRI is considering investing in FCL development flexible-collision-library/fcl#97
• Capsule support in FCL (MoveIt tracking issue: Add support for capsule collision checking moveit/moveit#158)

[wxmerkt]

This may be extended to potentially remove the reliance on the MoveIt Planning Scene in its entirety (or encapsulating the limited functionality we use) as this blocks using a fixed version of FCL. We can continue to use ACM without the planning scene.

After fixing some of the collision distance computation in Exotica, this returns 3.7cm penetration because the Mesh-to-Mesh triangle penetration computation is incorrect:

[VladimirIvan]

Can the FCL 0.6 with any dependencies be put in a debian?

Less dependency on MoveIt Planning Scene is good.
I want to eventually put the collision checking into a separate task map (outside of the Scene). This can also be a separate package in a separate repo, so we can make it depend on non-standard libraries without affecting compatibility of core exotica.

[wxmerkt]

I tried wrapping FCL 0.6 in a catkin package but there were conflicts between the FCL versions and namespaces brought in by MoveIt and the new catkinized FCL. I assume it could be catkinized and used exclusively if we got rid of MoveIt. So it's probably a 1-2-week-or-so project to do so (encapsulate CollisionScene as a class/library that has no MoveIt but allows external updates of transforms and objects in the FCL world; have Scene call these whenever there's an update - and move the ownership of the MoveIt planning scene element to Scene).

For now I've made quite some progress on fixing the distance and penetration depth calculation, just needs tidying up. Of course the collision points and depths aren't that accurate (visually just slightly beyond touching vs what is being returned (3.6cm) is quite off) but I hope it's good enough for now.