Add hpp-fcl as a collision checker

[rjoomen]

This adds HPP-FCL, the improved fork of FCL, as a collision checker.

Notable features:

• (Claimed) Improved performance
• State of the art GJK collision checking
• Safety margins, lower bound distance calculation, GJK early stopping (can we use any of these?)
• Actively developed

Most of this work was simply renaming, though a few types and methods differed slightly. By directly comparing the fcl_\*.\* and hpp_fcl_\*.\* source files one can easily check the required modifications.

Preliminary benchmarking looks promising.

Status: currently these unit tests fail:

Tests are run using the master and devel branches of hpp-fcl.

In the table below, master 1 uses the current workaround for distance contact normals, and master 2 uses fcl_result.normal. For devel there is no difference in test success between the two approaches for normal calculation. This suggests that this closed issue, regarding normals and nearest points solves the workaround as currently used in the FCL plugin.

Test	master 1	master 2	devel
HPP_FCLDiscreteBVHCollisionBoxSphereUnit	x	x
HPP_FCLDiscreteBVHCollisionBoxSphereConvexHullUnit	x	x
HPP_FCLDiscreteBVHCollisionBoxCylinderUnit		x
HPP_FCLDiscreteBVHCollisionBoxConeUnit		x
HPP_FCLDiscreteBVHCollisionBoxBoxUnit	x	x	x
HPP_FCLDiscreteBVHCollisionBoxBoxConvexHullUnit	x	x
HPP_FCLDiscreteBVHCollisionBoxCapsuleUnit	x	x	x
HPP_FCLDiscreteBVHCollisionLargeDataSetConvexHullUnit	x	x
HPP_FCLDiscreteBVHCollisionSphereSphereUnit	x	x
HPP_FCLDiscreteBVHCollisionSphereSphereConvexHullUnit	x
HPP_FCLDiscreteBVHCollisionMeshMeshUnit		x	x
HPP_FCLDiscreteBVHCollisionMultiThreadedConvexHullUnit	x	x
HPP_FCLDiscreteBVHCollisionOctomapSphereUnit			x
HPP_FCLDiscreteBVHCollisionOctomapSphereConvexHullUnit	x	x	x
HPP_FCLDiscreteBVHCollisionOctomapSphereMeshUnit*	x	x	x
HPP_FCLDiscreteBVHCollisionCloneUnit		x
HPP_FCLDiscreteBVHContactManagerConfigUnit	x	x
HPP_FCLDiscreteBVHCollisionCompoundCompoundUnit			x
Sum	12	16	7

* This test was disabled for FCL already

Interesting to note is that multiple runs of the tests results in a varying number of failed tests (the table lists the maximum number found). Perhaps some of the tests are at the edge of collision and the numerical optimization sometimes finds a collision and sometimes not.

[rjoomen]

There seems to be a CI problem with rosdep:

ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies (ROS distro is not set. Make sure `ROS_DISTRO` environment variable is set, or use `--rosdistro` option to specify the distro, e.g. `--rosdistro indigo`):
tesseract_collision: Cannot locate rosdep definition for [hpp-fcl]

If I test this locally hpp-fcl is indeed not found if I unset the ROS_DISTRO environment variable, but otherwise ros-$ROS_DISTRO-hpp-fcl. But as the CI is not ros-enabled, this is maybe not what we want.

[Levi-Armstrong]

Can we update this to point to the latest hash to see if the unit tests will pass?

[rjoomen]

Yes, but I wanted to test this locally first. Will do this week.

[rjoomen]

I've done more extensive testing, see the table in the first post. I've also updated the PR to use the devel branch.

[Levi-Armstrong]

Safety margins, lower bound distance calculation, GJK early stopping (can we use any of these?)

Depending on how it is implemented. In had to hack the safety margin piece in by creating the FCLCollisionObjectWrapper which inherits from fcl::CollisionObject<double> to expand the AABB for BVH.

What does this mean GJK early stopping (can we use any of these?)?

[rjoomen]

What does this mean GJK early stopping?

See here, here, and here.

(And (can we use any of these?) referred to whether any of the listed functions (safety margins, lower bound, early stopping) might be of use for Tesseract.)

[Levi-Armstrong]

Yes, we would set this to the contact threshold for normal collision checking and in trajopts case it would be set to safety_margin + safety_margin_buffer.

[rjoomen]

Tested with master: same result (as expected, v2.4.1 is very recent).

[Levi-Armstrong]

Not sure what to do here with so many tests failing. I know between FCL and Bullet there were slight numerical difference; have you looked at the tests to see if that is the case here?

[rjoomen]

Not sure what to do here with so many tests failing. I know between FCL and Bullet there were slight numerical difference; have you looked at the tests to see if that is the case here?

I've not looked in detail yet. But the hpp-fcl devel branch is under heavy development (some 100 commits since I started this PR), so I'd like to wait for v3 to be released before continuing this PR. I'm also mostly using cast collision checking, so for me it's not urgent anyway.

[Levi-Armstrong]

Do you want me to create a separate repository for this similar to the newly created tesseract_collision_physx?

[rjoomen]

Do you want me to create a separate repository for this similar to the newly created tesseract_collision_physx?

That's a good idea, please do.

[Levi-Armstrong]

@rjoomen What name do you prefer for the repository: tesseract_collision_hpp_fcl, tesseract_collision_hppfcl or ... ?

[rjoomen]

I don't really mind, but as the subproject is called hpp_fcl, maybe tesseract_collision_hpp_fcl is the most logical.

[Levi-Armstrong]

@rjoomen The repository has been created. Let me know if you run into any access issues.

https://github.com/tesseract-robotics/tesseract_collision_hpp_fcl