Getting assert failure on isOutsidePolytopeFace box to convex hull mesh (sphere)

[Levi-Armstrong]

Setup:

• Have a 1m x 1m x 1m box located at <0.0, 0.0, 0.0>
  • defined using fcl::boxd type
• Convex hull of meshed sphere sphere128.stl.txt located at <0.2, 0.0, 0.0>
  • defined using fcl::convexd type

When checking these two shapes I am recieving an assert failure in gjk_libccd-inl.h

Assert

assert(isOutsidePolytopeFace(&polytope, &f, &query_point));

Function

static void ComputeVisiblePatch(
    const ccd_pt_t& polytope, ccd_pt_face_t& f,
    const ccd_vec3_t& query_point,
    std::unordered_set<ccd_pt_edge_t*>* border_edges,
    std::unordered_set<ccd_pt_face_t*>* visible_faces,
    std::unordered_set<ccd_pt_edge_t*>* internal_edges) {
  assert(border_edges);
  assert(visible_faces);
  assert(internal_edges);
  assert(border_edges->empty());
  assert(visible_faces->empty());
  assert(internal_edges->empty());
  assert(isOutsidePolytopeFace(&polytope, &f, &query_point));
  visible_faces->insert(&f);
  for (int edge_index = 0; edge_index < 3; ++edge_index) {
    ComputeVisiblePatchRecursive(polytope, f, edge_index, query_point,
                                 border_edges, visible_faces, internal_edges);
  }
  assert(ComputeVisiblePatchRecursiveSanityCheck(
      polytope, *border_edges, *visible_faces, *internal_edges));
}

This was found when testing PR #288. I am still learning my way around fcl so does anyone have any insight into where I should start looking?

[Levi-Armstrong]

Also, I just tested building in release with debug info and as you expect it does not stop at the assert but I would have expected it to return bad results but the results were close to right answer. I think there may be an issue with the EPA implementation. Does fcl have a debug tool for visualizing the expanded polygon?

Correct Answer:

• distance: -0.55
• nearest point on box <0.5, 0.0, 0.0>
• nearest point on meshed sphere <-0.05, 0.0, 0.0>

Calculated Results:

• distance: -0.5525004
• nearest point on box <0.5, 0.025599, -0.005324>
• nearest point on meshed sphere <-0.0525004, 0.025599, -0.005324>

[Levi-Armstrong]

The previous example was ran against a convex hull sphere with 128 triangles where I just ran it against a convex hull sphere 1028 triangles. In debug it still fails at the assert but it does produce the correct answer. Now thinking back I did some looking into this a while back I found that when two objects are in collision the nearest point returned is the closest vertex which explains why I am getting the correct answer with a higher resolution mesh. @SeanCurtis-TRI and @hongkai-dai is the assert a valid check?

Calculated Results:

distance: -0.5499999
nearest point on box <0.5, -9e-8, 1e-7>
nearest point on meshed sphere <-0.049999, -9e-8, 1e-7>

[curds01]

We'll have to take a look into this. Right here and now, I have no easy means of looking at the sphere. So, without being able to examine it, I have to rely on questions:

1. If I infer things correctly from the data you've given, the sphere has radius 0.25, right? So, the sphere is completely contained in the box, yes?
2. Does the 128-triangle version actually have the vertex that most deeply penetrates (It would be the [-0.25, 0, 0] vertex in its local frame)? Or how about the 1028-triangle version? I would assume that the answers are no and yes, respectively. Or if not "no" and "yes", then "no" and "almost".

[curds01]

Ok, I've cheated and cracked open the linked stl file. The 128 triangles are based on 66 vertices. As geodesic spheres go, it doesn't seem overly spherical. I would expect all vertices to be equidistant from the origin. However, what I observe is that the vertices are bound by a bounding box from [-0.2525 -0.252393 -0.246258] to [ 0.252721 0.253174 0.244274]. Furthermore, the radius suggested by the various radii range from 0.252230423938 to 0.257859246604.

And since the left most point is located [-0.2525 0.0255992 -0.00532469], I presume that's exactly why you're getting a penetration depth of 0.5525 (and you'll note the sphere's left-most point is your nearest point on meshed sphere). So, that's why with the lower tesselation you're not getting what you'd expect from a sphere primitive with radius 0.25.

I presume the higher resolution sphere more closely approximates the sphere ideal. So, this all suggests that, in release mode, you're getting the right answer (for the data provided), but that the assertion in debug mode is being fooled.

[SeanCurtis-TRI]

All of those comments are actually mine -- I can't keep my github accounts straight when I'm home.

[SeanCurtis-TRI]

@hongkai-dai This is related to your EPA changes -- an assertion is failing and we'll want to investigate why. Do you want to do this or shall I put it on my plate?

[hongkai-dai]

@SeanCurtis-TRI if you have time, could you look into it? I don't think I have time to look into it until Saturday.

[Levi-Armstrong]

@SeanCurtis-TRI When testing you may need PR #288 which is what I have been using. Also I have been using the ROS package geometric_shapes to import the stl file and it has a function which will convert to convex hull code below.

      shapes::ShapePtr mesh(shapes::createMeshFromResource("package://tesseract_collision/test/sphere.stl"));
      bodies::ConvexMesh convex(mesh.get());
      convex.correctVertexOrderFromPlanes();
      const std::vector<unsigned int>& triangles = convex.getTriangles();
      const EigenSTL::vector_Vector3d& vertices = convex.getVertices();
      const EigenSTL::vector_Vector4d& planes = convex.getPlanes();

      int triangle_count = triangles.size()/3;
      Eigen::Vector3d* fcl_vertices = new Eigen::Vector3d[vertices.size()];
      Eigen::Vector3d* fcl_plane_normals = new Eigen::Vector3d[planes.size()];
      double* fcl_plane_dis = new double[planes.size()];
      int* polygons = new int[4 * triangle_count];

      for (unsigned i = 0; i < vertices.size(); ++i)
      {
        fcl_vertices[i] = vertices[i];
      }

      for (unsigned i = 0; i < planes.size(); ++i)
      {
        fcl_plane_normals[i] = planes[i].head<3>();
        fcl_plane_dis[i] = planes[i][3];
      }

      for(auto i = 0; i < triangle_count; ++i)
      {
        int i1 = triangles[3*i + 0];
        int i2 = triangles[3*i + 1];
        int i3 = triangles[3*i + 2];

        polygons[4*i + 0] = 3;
        polygons[4*i + 1] = i1;
        polygons[4*i + 2] = i2;
        polygons[4*i + 3] = i3;
      }

      fcl::Convexd fcl_convex(fcl_plane_normals, fcl_plane_dis, planes.size(), fcl_vertices, vertices.size(), polygons);

[SeanCurtis-TRI]

#288 seems to be crazy. It's so out of date that it isn't compatible with master. I did a local rebase and find I have failing tests. I'm going to address that first and then I'll look at this particular assertion error.

[SeanCurtis-TRI]

@Levi-Armstrong, I've submitted a quick PR to help diagnose your problem. There was a TODO in the code that I felt had a reasonable probability of being your problem. This will help confirm the diagnosis. If you (at least temporarily) merge this into your branch and run in debug mode, let me know what error message you get. I hope/expect you to get a different failure complaining about zero-area triangles. If you don't, well, that'll be informative as well.