assertion failed in libccd for Box to Box signed distance

[k2shah]

I'm running into an issue with FCL box <-> box signed distance where distance queries can hang forever. I believe the issue stems from this line.

https://github.com/flexible-collision-library/fcl/blob/master/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h#L1779 while(1)

I think the break condition is never met. Also when we run this in debug mode, a particular assertion fails.

in line 1327 of gjk_solver_libccd-inl.h this line assert(isOutsidePolytopeFace(&polytope, &f, &query_point)); fails.

Some preliminary findings tell me that the two objects that are failing the EPA expansion are both boxes (). and the query_point is on the same plane of the face f. I was able to look at the face normal and the query_point location. When ShapeSignedDistanceLibccdImpl is called. Likely, the f or the query_point is incorrect due to numerical precision.

Below is a reproducible case as a gtest case. This is built with gcc 7 and against fcl 0.7.0. This is built in Debug mode, with flags

-U_FORTIFY_SOURCE -fstack-protector -fno-omit-frame-pointer -g '-std=c++0x' -MD -fPIC '-std=c++17' -fno-canonical-system-headers -c fcl_test.cc

/**
 * fcl_test.cpp
 */
#include "gtest/gtest.h"
#include "Eigen/Dense"
#include "fcl/geometry/shape/box.h"
#include "fcl/broadphase/broadphase_dynamic_AABB_tree.h"
#include "fcl/broadphase/default_broadphase_callbacks.h"

    TEST(FCLTest, BoxTest) {
      using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
      fcl::DynamicAABBTreeCollisionManagerd tree1, tree2;
      Eigen::Matrix3d R1, R2;
      Eigen::Vector3d T1, T2;
      fcl::Transform3d pose1, pose2;

      // box 1
      CollisionGeometryPtr_t box1_ptr(
              new fcl::Boxd(0.25466666666666882, 0.17999999999999994, 0.10000000000000001));
      // set pose
      R1 << -0.091180275877866812, -0.99583037809002062, -0.0028311418055858018,
      0.99583254371528329, -0.091185230269289819, 0.0016729190909176572,
      -0.0019241019682901123, -0.0026668059216439916, 0.99999459297427817;
      T1 << -0.51931042117852699, 8.3835690261541238, 2.2548289378504784;
      pose1.linear() = R1.transpose();
      pose1.translation() = T1;

      // box 2
      CollisionGeometryPtr_t box2_ptr(
              new fcl::Boxd(0.10000000000000001, 0.75, 0.29999999999999999));
      // set pose
      R2 << -1, 0, 0,
      0, -1, 0,
      0, 0, 1;
      T2 << -0.40791387728097306, 8.2271462567397808, 2.0615427567791107;
      pose2.linear() = R2.transpose();
      pose2.translation() = T2;

      // make objects
      fcl::CollisionObjectd obj1(box1_ptr, pose1);
      fcl::CollisionObjectd obj2(box2_ptr, pose2);
      // add register to trees
      tree1.registerObject(&obj1);
      tree2.registerObject(&obj2);
      // distance request
      fcl::DefaultDistanceData<double> distance_data;
      distance_data.request.abs_err = 1e-5;
      distance_data.request.enable_signed_distance = true;
      distance_data.request.enable_nearest_points = true;

      // get the distance
      std::cout << "calling distance" << std::endl;
      tree1.distance(&tree2, &distance_data,
                         fcl::DefaultDistanceFunction<double>);
      ASSERT_TRUE(distance_data.result.min_distance < 0);
    }

[hongkai-dai]

@k2shah I am very sorry for the belated reply. I think the problem comes from numerical round-off error. As you point out, the check assert(isOutsidePolytopeFace(&polytope, &f, &query_point)); fails. I think I put this sanity check there to catch the numerical precision problem.

Now I think if this isOutsidePolytopeFace check fails, then it indicates that the query point is on the face. In that case we should report that the two objects are touching each other.

@SeanCurtis-TRI would you like to help me fix this bug when you come back to TRI? Thanks! Maybe I can send the PR and you could review the code?

[k2shah]

closing since no update