collisions utils

[arocchi]

There is still some work to do wrt collisions utils:

• move loadACMFromSRDF into collision_utils - we could make multiple versions of this function, one accepting an srdf model and an acm, one accepting an srdf filename and an acm
• add a broad phase on the collision detection (using AABBs)
• tests for collision_utils:
  • check distance computation with ad-hoc distance computation for capsules
  • verify correctness of minimum distance points by verifying that the distance between them is the correct minimum distance
  • verify correctness of minimum distance points by verifying they lay on the surface of the capsule they belong to
  • check timings between ad-hoc and FCL distance computation
  • check timings for distance computation with and without closest points query
  • check timings of AABB vs capsule-capsule distance query (no closest points check)
  • compatibility with Ubuntu 12.04 and older FCL, or disabling for older FCL versions (conditional compilation), since tests don't pass with FCL version 2.8 (we need at least FCL 3)

[arocchi]

@fcdean what do you think?

[arocchi]

• generateLinksToUpdate can be tested by checking that it gives the same result of getAllEntryNames in cases where there is no link pairs with CONDITIONAL collision checking