SplineMotion collision Regression bug in fcl-0.5 vs 0.6.1 commit b611d50

[ddengster]

Hi, we've been using fcl's SplineMotion and collisions are being reported incorrectly between versions 0.5 and 0.6.1, commit b611d50. I've noted that fcl::Transform3f was switched to the Eigen version, but besides that I don't know what else changed that affects this.

Here's some unit tests to showcasing this bug:

fcl-0.5
https://github.com/ddengster/fcl/tree/fcl-0.5-splinemotion-bug

BOOST_AUTO_TEST_CASE(SplineMotion_test)
{
  fcl::Vec3f t[4];
  t[0] = fcl::Vec3f(7.550000, 8, 0);
  t[1] = fcl::Vec3f(4.183333, 8, 0);
  t[2] = fcl::Vec3f(0.816667, 8, 0);
  t[3] = fcl::Vec3f(-2.550000, 8, 0);

  fcl::Vec3f r[4];
  r[0] = fcl::Vec3f(0, 0, 3.141593);
  r[1] = fcl::Vec3f(0, 0, 3.141593);
  r[2] = fcl::Vec3f(0, 0, 3.141593);
  r[3] = fcl::Vec3f(0, 0, 3.141593);
  auto motion_a = std::make_shared<fcl::SplineMotion>(
    t[0], t[1], t[2], t[3],
    r[0], r[1], r[2], r[3]);
  
  t[0] = fcl::Vec3f(-0.032564, 8, 0);
  t[1] = fcl::Vec3f(1.257920, 8, 0);
  t[2] = fcl::Vec3f(3.051563, 8, 0);
  t[3] = fcl::Vec3f(4.627592, 8, 0);

  r[0] = fcl::Vec3f(0, 0, 0);
  r[1] = fcl::Vec3f(0, 0, 0);
  r[2] = fcl::Vec3f(0, 0, 0);
  r[3] = fcl::Vec3f(0, 0, 0);
  auto motion_b = std::make_shared<fcl::SplineMotion>(
    t[0], t[1], t[2], t[3],
    r[0], r[1], r[2], r[3]);

  // test collision with unit circles
  {
    auto shape_a = std::make_shared<fcl::Sphere>(1.0);
    const auto obj_a = fcl::ContinuousCollisionObject(
      shape_a,
      motion_a);

    auto shape_b = std::make_shared<fcl::Sphere>(1.0);
    const auto obj_b = fcl::ContinuousCollisionObject(
      shape_b,
      motion_b);

    fcl::ContinuousCollisionRequest request;
    request.ccd_solver_type = fcl::CCDC_CONSERVATIVE_ADVANCEMENT;
    request.gjk_solver_type = fcl::GST_LIBCCD;

    fcl::ContinuousCollisionResult result;
    fcl::collide(&obj_a, &obj_b, request, result);

    if (result.is_collide)
      std::cout << "collision! toi: " << result.time_of_contact << std::endl;
    else
      std::cout << "no collision" << std::endl;
  }

}

fcl-0.5 result:

collision! toi: 0.170645

fcl-0.6.1
https://github.com/ddengster/fcl/tree/0.6.1-splinemotion-bug

template <typename S>
void test_SplineMotion_test()
{
  fcl::Vector3<S> t[4];
  t[0] = fcl::Vector3<S>(7.550000, 8, 0);
  t[1] = fcl::Vector3<S>(4.183333, 8, 0);
  t[2] = fcl::Vector3<S>(0.816667, 8, 0);
  t[3] = fcl::Vector3<S>(-2.550000, 8, 0);

  fcl::Vector3<S> r[4];
  r[0] = fcl::Vector3<S>(0, 0, 3.141593);
  r[1] = fcl::Vector3<S>(0, 0, 3.141593);
  r[2] = fcl::Vector3<S>(0, 0, 3.141593);
  r[3] = fcl::Vector3<S>(0, 0, 3.141593);
  auto motion_a = std::make_shared<fcl::SplineMotion<S>>(
    t[0], t[1], t[2], t[3],
    r[0], r[1], r[2], r[3]);
  
  t[0] = fcl::Vector3<S>(-0.032564, 8, 0);
  t[1] = fcl::Vector3<S>(1.257920, 8, 0);
  t[2] = fcl::Vector3<S>(3.051563, 8, 0);
  t[3] = fcl::Vector3<S>(4.627592, 8, 0);

  r[0] = fcl::Vector3<S>(0, 0, 0);
  r[1] = fcl::Vector3<S>(0, 0, 0);
  r[2] = fcl::Vector3<S>(0, 0, 0);
  r[3] = fcl::Vector3<S>(0, 0, 0);
  auto motion_b = std::make_shared<fcl::SplineMotion<S>>(
    t[0], t[1], t[2], t[3],
    r[0], r[1], r[2], r[3]);

  // test collision with unit circles
  {
    auto shape_a = std::make_shared<fcl::Sphere<S>>(1.0);
    const auto obj_a = fcl::ContinuousCollisionObject<S>(
      shape_a,
      motion_a);

    auto shape_b = std::make_shared<fcl::Sphere<S>>(1.0);
    const auto obj_b = fcl::ContinuousCollisionObject<S>(
      shape_b,
      motion_b);

    fcl::ContinuousCollisionRequest<S> request;
    request.ccd_solver_type = fcl::CCDC_CONSERVATIVE_ADVANCEMENT;
    request.gjk_solver_type = fcl::GST_LIBCCD;

    fcl::ContinuousCollisionResult<S> result;
    fcl::collide(&obj_a, &obj_b, request, result);

    if (result.is_collide)
      std::cout << "collision! toi: " << result.time_of_contact << std::endl;
    else
      std::cout << "no collision" << std::endl;
  }

}

GTEST_TEST(FCL_COLLISION, SplineMotion_test)
{
  test_SplineMotion_test<float>();
}

fcl-0.6.1 result:

[ RUN      ] FCL_COLLISION.SplineMotion_test
no collision

If anyone could provide some insight into this it would be much appreciated. Thanks!

[SeanCurtis-TRI]

Thanks for calling this out and sorry for the regression. The logical thing would be to introduce the test into FCL and do a little bisection. I don't know when we'll have some free cycles to address this, but if you end up discovering the cause, we'd love to help PR it into FCL.

[ddengster]

Ok, we've tracked this down to changes in the sphereSphereDistance function. The change in 0.6.1 tweaked the point computations.

0.6.1
https://github.com/flexible-collision-library/fcl/blob/master/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h#L102

0.5
https://github.com/flexible-collision-library/fcl/blob/fcl-0.5/src/narrowphase/narrowphase.cpp#L300

[mxgrey]

I'm a little suspicious of the change from the local to global coordinate frame for the returned points. If the conservative advancement algorithm implementation wasn't updated to expect a global coordinate frame, then that could explain a regression being introduced in 0.6.

[SeanCurtis-TRI]

This is some really good sleuthing! Thanks for tracking it down.

Changing the frame was definitely the right thing to do (see PR #288), but clearly code in FCL that was dependent on that frame was not caught. Lack of unit tests, presumably. The right thing is to make sure that anything calling distance queries recognizes the frame in which the results are reported.

So, looking at your test case, the problem is not in the SplineMotion, but in the continuous collision code. Would you agree with that? I haven't worked with FCL's CCD code yet so this gave me the chance to do a bit of perusal. Given the code you're exercising, this line would be the problematic line, right? This code is assuming that shapeDistance returns "closest points", each measured and expressed in the object's frame rather than the world frame. So, if you removed the transform multiplications, your test is happy?

[ddengster]

Yup, @SeanCurtis-TRI removing the transform multiplication fixes it! Thanks a lot!

We've also added a PR for the fix:
#505