Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post Reply
wbthomason
Posts: 9
Joined: Fri May 10, 2019 8:18 pm

Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post by wbthomason »

I'm using Bullet just for collision detection (as part of a motion planner; I have no dynamics). I set up a btCollisionWorld with the default collision dispatcher and collision config, and a btDbvtBroadphase as my broadphase interface. I then load btCollisionObjects into the collision world.

I am using btBvhTriangleMeshShapes for my collision shapes. I know from other forum results that concave shapes can result in zero contact points if one object is completely inside another, but (a) it appears that faces of my meshes are in collision, and (b) it does not appear that all pairs which should collide (but don't) have one object completely inside the other. I'm also aware that btBvhTriangleMeshShapes is only to be used for static objects - I thought this was OK for my use since without dynamics, everything is static (right?) I have also tried using btGImpactMeshShape, with no change in results.

At run-time, I update the transforms for the collision objects I've added to the collision world based on the motion planner state, and call updateAabbs() and then performDiscreteCollisionDetection() on the collision world. I always get a non-zero number of contact manifolds, but every single manifold always has zero contact points.

I know from other posts on this forum that a contact manifold is created in the broadphase based on AABB intersection and may reasonably have zero contact points. However, when I visualize some of the states I'm trying to collision check (using the debug drawer interface), there are a number of collision geometries clearly in contact with each other - things like a robot geometry stuck in the middle of a table geometry. As such, I'm baffled by getting no contact points.

I do make large movements of the objects between calls to performDiscreteCollisionDetection() - even though I don't have dynamics, per se, could this cause Bullet to miss some of my collisions?

I can also post a more complete code example, though I'll have to extract it from my broader system for it to make sense.
steven
Posts: 83
Joined: Mon Nov 05, 2018 8:16 am
Location: China

Re: Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post by steven »

i would like to work with you to find the rootcause if you can provide the sample code.
wbthomason
Posts: 9
Joined: Fri May 10, 2019 8:18 pm

Re: Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post by wbthomason »

Sure, thank you! The below is not the exact code I'm using, because that includes too many parts of the system I'm using Bullet in to be a clear example. However, it should be equivalent in terms of Bullet usage - it uses the same structures, constructed in the same way, and through the same methods. Please let me know if anything isn't clear, and thank you for your help!

Code: Select all

#include <array>
#include <iostream>
#include <memory>
#include <vector>

#include <bullet/btBulletCollisionCommon.h>

// This is tinyobjloader from: https://github.com/syoyo/tinyobjloader
#define TINYOBJLOADER_IMPLEMENTATION
#include "tiny_obj_loader.h"

constexpr double PENETRATION_EPSILON = 0.001;

void load_collision_objects(std::vector<std::unique_ptr<btCollisionObject>>& collision_objects);
void update_object_positions(std::vector<std::unique_ptr<btCollisionObject>>& collision_objects);

int main() {
  // Create the collision objects
  std::vector<std::unique_ptr<btCollisionObject>> collision_objects;
  load_collision_objects(collision_objects);

  // Create the objects necessary for the collision world
  auto collision_config     = std::make_unique<btDefaultCollisionConfiguration>();
  auto collision_dispatch   = std::make_unique<btCollisionDispatcher>(collision_config.get());
  auto broadphase_interface = std::make_unique<btDbvtBroadphase>();
  auto collision_world      = std::make_unique<btCollisionWorld>(collision_dispatch.get(),
                                                            broadphase_interface.get(),
                                                            collision_config.get());

  // Load the collision objects into the world
  for (const auto& collision_object : collision_objects) {
    collision_world->addCollisionObject(collision_object.get());
  }

  // Main collision checking logic. In the actual system, this isn't a loop, but a method on a
  // class that's called repeatedly by an external loop
  for (int i = 0; i < 500; ++i) {
    update_object_positions(collision_objects);
    collision_world->updateAabbs();
    collision_world->performDiscreteCollisionDetection();
    // This is always positive, and changes as objects move across iterations, as expected
    const auto num_manifolds = collision_dispatch->getNumManifolds();

    for (int j = 0; j < num_manifolds; ++j) {
      auto* manifold = collision_dispatch->getManifoldByIndexInternal(j);
      // This is *always* zero, which is incorrect
      const auto num_contacts = manifold->getNumContacts();
      for (int k = 0; k < num_contacts; ++k) {
        const auto& point = manifold->getContactPoint(k);
        // Neither of the messages in the below branches ever print
        if (point.getDistance() <= -PENETRATION_EPSILON) {
          std::cout << "Got a collision!\n";
        } else {
          std::cout << "Contact point isn't a collision\n";
        }
      }

      manifold->clearManifold();
    }
  }

  return 0;
}

void load_collision_objects(std::vector<std::unique_ptr<btCollisionObject>>& collision_objects) {
  std::vector<tinyobj::attrib_t> obj_attribs;
  std::vector<std::vector<tinyobj::shape_t>> obj_shapes;
  std::vector<btVector3> obj_scales;
  std::vector<btTransform> obj_poses;

  // In the real system, this is loading meshes from URDF and other data files. It's just stubbed
  // here, as the logic is unimportant to this example
  load_tinyobj_structures(obj_attribs, obj_shapes, obj_scales, obj_poses);

  for (size_t i = 0; i < obj_attribs.size(); ++i) {
    // Make the collision shape
    const auto& mesh_attrib = obj_attribs[i];
    const auto& mesh_shapes = obj_shapes[i];
    // All of my .obj files are known to contain a single shape
    const auto& shape   = mesh_shapes[0];
    size_t index_offset = 0;
    // This is a memory leak, but it's ok because these meshes need to live the full lifetime of
    // the program
    btTriangleMesh* trimesh = new btTriangleMesh();
    std::array<btVector3, 3> vertices;
    // This loading code modified from the example given in the tinyobjloader repository
    for (size_t f = 0; f < shape.mesh.num_face_vertices.size(); ++f) {
      // All of the faces in my .obj files are known to be triangles
      for (size_t v = 0; v < 3; ++v) {
        const auto idx = shape.mesh.indices[index_offset + v];
        const auto vx  = mesh_attrib.vertices[3 * idx.vertex_index + 0];
        const auto vy  = mesh_attrib.vertices[3 * idx.vertex_index + 1];
        const auto vz  = mesh_attrib.vertices[3 * idx.vertex_index + 2];
        btVector3 point(vx, vy, vz);
        vertices[v] = point * obj_scales[i];
      }

      trimesh->addTriangle(vertices[0], vertices[1], vertices[2]);
      index_offset += 3;
    }

    // Again, this is a leak that doesn't matter
    auto model = new btBvhTriangleMeshShape(trimesh, true, true);

    // Make the collision object
    const auto& collision_object =
    collision_objects.emplace_back(std::make_unique<btCollisionObject>());
    collision_object->setCollisionShape(model);
    collision_object->setWorldTransform(obj_poses[i]);
  }
}

void update_object_positions(std::vector<std::unique_ptr<btCollisionObject>>& collision_objects) {
  std::vector<btTransform> new_poses;

  // In the real system, the new poses for objects are provided by sampling a state and using FK,
  // but that's not relevant here. The below is a stub function that finds arbitrary new poses for
  // the objects
  get_new_poses(new_poses);

  for (size_t i = 0; i < new_poses.size(); ++i) {
    const auto& new_pose = new_poses[i];
    auto& object_pose    = collision_objects[i]->getWorldTransform();
    object_pose.setOrigin(new_pose.getOrigin());
    object_pose.setRotation(new_pose.getRotation());
  }
}
steven
Posts: 83
Joined: Mon Nov 05, 2018 8:16 am
Location: China

Re: Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post by steven »

i just write a similar test but use two boxes instead of the concave and don't find the issue you said. so next i need the data to create the concave shape. thanks.

Code: Select all

I know from other posts on this forum that a contact manifold is created in the broadphase based on AABB intersection and may reasonably have zero contact points.
overlappingPairArray is created in the broadphase while manifold is created in the narrowphase.
wbthomason
Posts: 9
Joined: Fri May 10, 2019 8:18 pm

Re: Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post by wbthomason »

Hmm, ok. I've attached an archive with geometries for three of the objects geometries I'm using (table.obj, arena.obj, and torso_lift.obj) as well as the URDF file (a modified version of pr2.urdf from the ROS pr2_description package) I load in the real code. Note that torso_lift.obj comes from the URDF; it's meant as an example of a robot part which should collide.

Posing, for example, table.obj at the world origin with rotation (as a x-y-z-w quaternion) (0, 0, 0.7071067690849304, 0.7071067690849304) and torso_lift.obj at translation (0, 0, 0.790675) with no rotation should cause the two to be in collision (I've visualized these poses for the objects, and they appear to be in collision, visually).
steven wrote: Sat May 11, 2019 9:08 am overlappingPairArray is created in the broadphase while manifold is created in the narrowphase.
Oh, ok. I guess I misunderstood - I was referring to this post: viewtopic.php?t=5689
Attachments
meshes.tar.gz
(33.11 KiB) Downloaded 293 times
steven
Posts: 83
Joined: Mon Nov 05, 2018 8:16 am
Location: China

Re: Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post by steven »

your two collison objects all are the concave but now the engine doesn't support the concave vs. convave collision in the narrowphase. you can refer to this function btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc() in file btdefaultcollisionconfiguration.cpp for details.

i think you should perform convex decomposition to one of these two concaves by HACD(Hierarchical Approximate Convex Decomposition).
hope this is helpful for you! good luck!
thanks.
wbthomason
Posts: 9
Joined: Fri May 10, 2019 8:18 pm

Re: Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post by wbthomason »

Oh, it doesn't support concave/concave? I thought from the collision matrix on page 16 of https://github.com/bulletphysics/bullet ... Manual.pdf that the gimpact algorithm would handle that case.

Regardless, I've actually already started using V-HACD to decompose my robot shapes. I'm now getting a bunch of self-collisions in the robot that I wasn't before, but I can probably solve that by adding a collision filter to account for the convex models, I guess?

Thank you for the help!
steven
Posts: 83
Joined: Mon Nov 05, 2018 8:16 am
Location: China

Re: Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post by steven »

Code: Select all

I have also tried using btGImpactMeshShape, with no change in results.
Could you post some code of how did you use the btGImpactMeshShape? i will try it later.

i find the algorithm for the gimpact need to be regiestered externally like below.

Code: Select all

btCollisionDispatcher * dispatcher = static_cast<btCollisionDispatcher *>(m_dynamicsWorld ->getDispatcher());
btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher);
steven
Posts: 83
Joined: Mon Nov 05, 2018 8:16 am
Location: China

Re: Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post by steven »

add a picture to clarify this case. any ideas about the gimpact mesh is helpful, thanks in advance.
collisionConaves.png
collisionConaves.png (29.69 KiB) Viewed 8714 times
wbthomason
Posts: 9
Joined: Fri May 10, 2019 8:18 pm

Re: Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post by wbthomason »

Oh, I see - I didn't know that GImpact needed to be explicitly registered. My GImpact usage was just substituting btGImpactShape for btBvhTriangleMeshShape in loading my collision geometries (and ofc. the corresponding changes in construction). I did not register the collision algorithm. From reading issues on the main bullet3 repo, it looks like I would also need to use the GImpact to compound shape utility to get things working with GImpact. Since I already have V-HACD working, I may just see if I can get rid of the self-collisions by adding a collision filter. Thanks!
steven
Posts: 83
Joined: Mon Nov 05, 2018 8:16 am
Location: China

Re: Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post by steven »

after changing to tag 2.82(git checkout 2.82), i find the GimpactTestDemo. i don't know why it is deleted in the later version. hope it is useful for future user.
wbthomason
Posts: 9
Joined: Fri May 10, 2019 8:18 pm

Re: Incorrect collision results with concave shapes (contact manifolds, but 0 contact points)

Post by wbthomason »

For what it's worth, convex decomposition did the trick. I'm now getting far more collisions than I should, but I think that's an issue with my particular system and not Bullet. Thanks @steven for the help!
Post Reply