Question: How best to leverage PhysX as a collision checker (No Physics)

[Levi-Armstrong]

I am interested in leveraging PhysX for collision checking only, to be used with in robotic motion planning algorithms. I currently have implementations for FCL and Bullet but I am interested in the GPU support provided by PhysX. I am new to PhysX and I have been going through the API and testing out different functionalities.

At a high level I would like to accomplish the following. Add RigidStatic objects to the environment along with RigidKinematic objects. Then I would like to move the RigidKinematics objects to different locations and at each location perform a collision/distance check. The information needed from the collision check is: distance/penetration depth, translation vector, contact point corresponding to the translation vector for each object in the contact pair.

I have been able to put a simple test where I add the RigidStatic objects and then use the scene overlap function to check contact with PxGeometry, but this only provide the depth and translation vector and not the contact points provided by GJK. I do see under geomutils that there are functions for getting this information but not sure what is the best approach this use case using PhysX.

How should I approach this use case using PhysX?

[shaneasd]

Have you considered using split simulation to perform collision detection without resolution then use a contact reporting callback to get the details of the collisions?

caveats:

• I haven't tried this so I'm not offering advice, just ideas
• the documentation linked is for physx 3 but you're presumably using physx 4. I'm assuming the interface for these features is fundamentally the same but haven't checked.
• The documentation for split simulation makes it seem like it would complain (or not work) if you keep detecting collisions without resolving them. You may need to keep recreating the scene to avoid this which may have significant performance implications.

[Levi-Armstrong]

@shaneasd Thank you for the recommendation. I recently setup the split simulation, but I cannot get the onContact event to trigger. I copy the code into a single file so maybe you or someone else can see what I am doing incorrectly because the contact event it not being called. Now the contactReportFilterShader is being called so it is detecting the collision.

#include <PxPhysicsAPI.h>
#include <istream>
#include <memory>

#define PX_RELEASE(x)   if(x)   { x->release(); x = nullptr;}
#define UNUSED(x) (void)(x)
#define CONSOLE_BRIDGE_logError(x) { std::printf("%s\n", (x)); }

class TesseractSimulationEventCallback : public physx::PxSimulationEventCallback
{
public:
  void onConstraintBreak(physx::PxConstraintInfo* /*constraints*/, physx::PxU32 /*count*/) override {std::printf("onConstraintBreak\n");}
  void onWake(physx::PxActor** /*actors*/, physx::PxU32 /*count*/) override {std::printf("onWake\n");}
  void onSleep(physx::PxActor** /*actors*/, physx::PxU32 /*count*/) override {std::printf("onSleep\n");}
  void onTrigger(physx::PxTriggerPair* /*pairs*/, physx::PxU32 /*count*/) override {std::printf("onTrigger\n");}
  void onAdvance(const physx::PxRigidBody*const* /*bodyBuffer*/, const physx::PxTransform* /*poseBuffer*/, const physx::PxU32 /*count*/) override {std::printf("onAdvance\n");}

  // https://gameworksdocs.nvidia.com/PhysX/4.1/documentation/physxguide/Manual/AdvancedCollisionDetection.html#extracting-contact-information
  void onContact(const physx::PxContactPairHeader& pairHeader, const physx::PxContactPair* pairs, physx::PxU32 nbPairs) override
  {
    UNUSED(pairHeader);
    std::printf("TesseractSimulationEventCallback::onContact\n");
    const physx::PxU32 bufferSize = 64;
    physx::PxContactPairPoint contacts[bufferSize];
    for(physx::PxU32 i=0; i < nbPairs; i++)
    {
      const physx::PxContactPair& cp = pairs[i];
      physx::PxU32 nbContacts = pairs[i].extractContacts(contacts, bufferSize);
      for(physx::PxU32 j=0; j < nbContacts; j++)
      {
          physx::PxVec3 point = contacts[j].position;
          physx::PxVec3 impulse = contacts[j].impulse;
          physx::PxU32 internalFaceIndex0 = contacts[j].internalFaceIndex0;
          physx::PxU32 internalFaceIndex1 = contacts[j].internalFaceIndex1;
          UNUSED(point);
          UNUSED(impulse);
          UNUSED(internalFaceIndex0);
          UNUSED(internalFaceIndex1);
          //...
      }

      if(cp.events & physx::PxPairFlag::eNOTIFY_TOUCH_FOUND)
      {
  //            if((pairHeader.actors[0] == mSubmarineActor) ||
  //                (pairHeader.actors[1] == mSubmarineActor))
  //            {
  //                PxActor* otherActor = (mSubmarineActor == pairHeader.actors[0]) ?
  //                    pairHeader.actors[1] : pairHeader.actors[0];
  //                Seamine* mine =  reinterpret_cast<Seamine*>(otherActor->userData);
  //                // insert only once
  //                if(std::find(mMinesToExplode.begin(), mMinesToExplode.end(), mine) ==
  //                    mMinesToExplode.end())
  //                    mMinesToExplode.push_back(mine);

  //                break;
  //            }
      }
    }
  }
};

enum class FilterGroup : physx::PxU32
{
    eSTATIC     = (1 << 0),
    eKINEMATIC  = (1 << 1)
};

physx::PxFilterFlags contactReportFilterShader(physx::PxFilterObjectAttributes attributes0,
                                               physx::PxFilterData filterData0,
                                               physx::PxFilterObjectAttributes attributes1,
                                               physx::PxFilterData filterData1,
                                               physx::PxPairFlags& pairFlags,
                                               const void* constantBlock,
                                               physx::PxU32 constantBlockSize)
{
  UNUSED(constantBlockSize);
  UNUSED(constantBlock);

  // let triggers through
  if(physx::PxFilterObjectIsTrigger(attributes0) || physx::PxFilterObjectIsTrigger(attributes1))
  {
      pairFlags = physx::PxPairFlag::eTRIGGER_DEFAULT;
      return physx::PxFilterFlag::eDEFAULT;
  }

  // generate contacts for all that were not filtered above
  pairFlags = physx::PxPairFlag::eNOTIFY_CONTACT_POINTS | physx::PxPairFlag::eDETECT_DISCRETE_CONTACT;

  // trigger the contact callback for pairs (A,B) where
  // the filtermask of A contains the ID of B and vice versa.
  if((filterData0.word0 & filterData1.word1) && (filterData1.word0 & filterData0.word1))
      pairFlags |=physx::PxPairFlag::eNOTIFY_TOUCH_FOUND;

  return physx::PxFilterFlag::eDEFAULT;

//  // all initial and persisting reports for everything, with per-point data
//  pairFlags = physx::PxPairFlag::eSOLVE_CONTACT | physx::PxPairFlag::eDETECT_DISCRETE_CONTACT
//        | physx::PxPairFlag::eNOTIFY_TOUCH_FOUND
//        | physx::PxPairFlag::eNOTIFY_TOUCH_PERSISTS
//        | physx::PxPairFlag::eNOTIFY_CONTACT_POINTS;
//  return physx::PxFilterFlag::eNOTIFY; //:eCALLBACK; //physx::PxFilterFlag::eDEFAULT;
}

class TesseractPhysx
{
public:
  using Ptr = std::shared_ptr<TesseractPhysx>;
  using ConstPtr = std::shared_ptr<const TesseractPhysx>;

  TesseractPhysx()
  {
    initialize();
  }

  virtual ~TesseractPhysx()
  {
    PX_RELEASE(scene_);
    PX_RELEASE(dispatcher_);
    PX_RELEASE(physics_);
    PX_RELEASE(cooking_);

    if(pvd_)
    {
      physx::PxPvdTransport* transport = pvd_->getTransport();
      PX_RELEASE(pvd_);
      PX_RELEASE(transport);
    }

    PX_RELEASE(foundation_);
  }

  TesseractPhysx(const TesseractPhysx&) = delete;
  TesseractPhysx& operator=(const TesseractPhysx&) = delete;
  TesseractPhysx(TesseractPhysx&&) = delete;
  TesseractPhysx& operator=(TesseractPhysx&&) = delete;

  physx::PxFoundation* getFoundation() const { return foundation_; }
  physx::PxPhysics* getPhysics() const { return physics_; }
  physx::PxCooking* getCooking() const { return cooking_; }
  physx::PxScene* getScene() const { return scene_; }
  physx::PxMaterial* getMaterial() const { return material_; }

  physx::PxDefaultAllocator& getAllocator() { return default_allocator_; }
  const physx::PxDefaultErrorCallback& getErrorCallback() { return error_callback_; }

private:
  physx::PxDefaultAllocator     default_allocator_;
  physx::PxDefaultErrorCallback error_callback_;

  physx::PxFoundation*           foundation_ {nullptr};
  physx::PxPhysics*              physics_ {nullptr};
  physx::PxCooking*              cooking_ {nullptr};
  physx::PxScene*                scene_ {nullptr};
  physx::PxDefaultCpuDispatcher* dispatcher_{nullptr};
  physx::PxMaterial*                     material_{nullptr};
  physx::PxPvd*                  pvd_{nullptr};
  std::string                    pvd_host_ {"127.0.0.1"};

  TesseractSimulationEventCallback event_cb_;

  void initialize()
  {
    foundation_ = PxCreateFoundation(PX_PHYSICS_VERSION, default_allocator_, error_callback_);
    if(!foundation_)
        CONSOLE_BRIDGE_logError("PxCreateFoundation failed!");

    pvd_ = physx::PxCreatePvd(*foundation_);
    physx::PxPvdTransport* transport = physx::PxDefaultPvdSocketTransportCreate(pvd_host_.c_str(), 5425, 10);
    pvd_->connect(*transport,physx::PxPvdInstrumentationFlag::eALL);

    physics_ = PxCreatePhysics(PX_PHYSICS_VERSION, *foundation_, physx::PxTolerancesScale(), true, pvd_);
    if(!physics_)
      CONSOLE_BRIDGE_logError("PxCreatePhysics failed!");

    // The PhysX cooking library provides utilities for creating, converting, and serializing bulk data. Depending on
    // your application, you may wish to link to the cooking library in order to process such data at runtime.
    // Alternatively you may be able to process all such data in advance and just load it into memory as required.
    // Initialize the cooking library as follows:
    cooking_ = PxCreateCooking(PX_PHYSICS_VERSION, *foundation_, physx::PxCookingParams(physx::PxTolerancesScale()));
    if (!cooking_)
        CONSOLE_BRIDGE_logError("PxCreateCooking failed!");

    // The extensions library contains many functions that may be useful to a large class of users, but which some users
    // may prefer to omit from their application either for code size reasons or to avoid use of certain subsystems,
    // such as those pertaining to networking. Initializing the extensions library requires the PxPhysics object:
    if (!PxInitExtensions(*physics_, pvd_))
      CONSOLE_BRIDGE_logError("PxInitExtensions failed!");

    physx::PxSceneDesc scene_desc(physics_->getTolerancesScale());
    scene_desc.gravity = physx::PxVec3(0.0f, -9.81f, 0.0f);
    dispatcher_ = physx::PxDefaultCpuDispatcherCreate(2);
    scene_desc.cpuDispatcher    = dispatcher_;
    scene_desc.filterShader = physx::PxDefaultSimulationFilterShader;
    scene_desc.kineKineFilteringMode = physx::PxPairFilteringMode::eKEEP; // So kin-kin contacts with be reported
    scene_desc.staticKineFilteringMode = physx::PxPairFilteringMode::eKEEP; // So static-kin constacts will be reported
    scene_desc.simulationEventCallback = &event_cb_;
    scene_desc.filterShader = contactReportFilterShader;
    scene_ = physics_->createScene(scene_desc);

    physx::PxPvdSceneClient* pvdClient = scene_->getScenePvdClient();
    if(pvdClient)
    {
      pvdClient->setScenePvdFlag(physx::PxPvdSceneFlag::eTRANSMIT_CONSTRAINTS, true);
      pvdClient->setScenePvdFlag(physx::PxPvdSceneFlag::eTRANSMIT_CONTACTS, true);
      pvdClient->setScenePvdFlag(physx::PxPvdSceneFlag::eTRANSMIT_SCENEQUERIES, true);
    }

    material_ = physics_->createMaterial(0.5f, 0.5f, 0.6f);
  }
};

void setupFiltering(TesseractPhysx& phy,
                    physx::PxRigidActor* actor,
                    physx::PxU32 filterGroup,
                    physx::PxU32 filterMask)
{
    physx::PxFilterData filterData;
    filterData.word0 = filterGroup; // word0 = own ID
    filterData.word1 = filterMask;  // word1 = ID mask to filter pairs that trigger a
                                    // contact callback;
    const physx::PxU32 numShapes = actor->getNbShapes();
    physx::PxShape** shapes = (physx::PxShape**)phy.getAllocator().allocate(sizeof(physx::PxShape*)*numShapes, nullptr, __FILE__, __LINE__);
    actor->getShapes(shapes, numShapes);
    for(physx::PxU32 i = 0; i < numShapes; i++)
    {
        physx::PxShape* shape = shapes[i];
        shape->setSimulationFilterData(filterData);
    }
    phy.getAllocator().deallocate(shapes);
}

int main(int, const char*const*)
{
  TesseractPhysx phy;

  std::string link_name = "static_link";
  physx::PxTransform global_tf;
  global_tf.p = physx::PxVec3(0, 0, 0);
  global_tf.q = physx::PxQuat(0, 0, 1, 0);

  physx::PxRigidStatic* sphere = physx::PxCreateStatic(*phy.getPhysics(), global_tf, physx::PxSphereGeometry(physx::PxReal(0.5)), *phy.getMaterial());
  sphere->setName(link_name.c_str());
  std::printf("%s\n", sphere->getName());
  setupFiltering(phy, sphere, static_cast<physx::PxU32>(FilterGroup::eSTATIC), static_cast<physx::PxU32>(FilterGroup::eKINEMATIC));
  phy.getScene()->addActor(*sphere);

  physx::PxTransform kin_global_tf, kin_shape_tf;
  kin_global_tf.p = physx::PxVec3(0, 0, 0.7f);
  kin_global_tf.q = physx::PxQuat(0, 0, 1, 0);
  kin_shape_tf.p = physx::PxVec3(0, 0, 0);
  kin_shape_tf.q = physx::PxQuat(0, 0, 1, 0);
  physx::PxRigidDynamic* kinematic_sphere = physx::PxCreateKinematic(*phy.getPhysics(),
                                                                     kin_global_tf,
                                                                     physx::PxSphereGeometry(physx::PxReal(0.25)),
                                                                     *phy.getMaterial(),
                                                                     physx::PxReal(0.1),
                                                                     kin_shape_tf);
  kinematic_sphere->setName("kinematic_sphere");

  setupFiltering(phy, kinematic_sphere, static_cast<physx::PxU32>(FilterGroup::eKINEMATIC), static_cast<physx::PxU32>(FilterGroup::eSTATIC));
  phy.getScene()->addActor(*kinematic_sphere);

  for (int i = 0; i < 5; ++i)
  {
//    phy.getScene()->simulate(0.0001f);
    phy.getScene()->collide(0.0001f);
    phy.getScene()->fetchCollision(true);
    phy.getScene()->advance(); // Can this be skipped
    phy.getScene()->fetchResults(true);

    physx::PxTransform update_tf;
    update_tf.p = physx::PxVec3(0, 0, 1.0f - ((physx::PxReal(i) + 1.f) * 0.1f));
    update_tf.q = physx::PxQuat(0, 0, 1, 0);
    kinematic_sphere->setGlobalPose(update_tf);
  }
  std::printf("Done!\n");
  return 0;
}

[kstorey-nvidia]

Does it work if you call setKinematicTarget rather than setGlobalPose()? PhysX only generates contacts between active actors, and setting globalPose (i.e. teleporting) a kinematic actor does not activate it.

[Levi-Armstrong]

Great! That was the issue. One other question is it possible to omit the advance() call if the only object in the environment are static and kinematic?

[kstorey-nvidia]

No, the advance call should do very minimal work, but it is still required even if you just have kinematic actors.

[Levi-Armstrong]

Thank you.

So it runs now but after initial contact it does not call the onContact event if it remains in contact. Is it possible to set it up to always report contact even if it was previously in contact?

[kstorey-nvidia]

This is because you asked it to do that with this flag:

pairFlags |=physx::PxPairFlag::eNOTIFY_TOUCH_FOUND;

you need to additionall ask for eNOTIFY_TOUCH_PERSIST if you want contact reports every frame for that pair

[Levi-Armstrong]

That was it. Thank you for you help. I will keep testing and integrating it open source software package.

[shaneasd]

If you're forced to call advance anyway, I wonder if it would be simpler to forget about the split simulation and just use regular stepping. Presumably you get the same contact notifications at the same point in the physics logic.

[Levi-Armstrong]

I will test just using the simulation method.

[Levi-Armstrong]

The simulation seems to works as well.

[Levi-Armstrong]

@kstorey-nvidia I have noticed if I add an Actor then call its setKinematicTarget(new_tf) followed by the call simulate(0.0001f) it reports the contact for the location of the actor prior to setKinematicTarget. Is there something I should call prior to simulate or do I just need to call simulate twice?

[Levi-Armstrong]

Base on this documentation should you always call wakeUp() and setWakeCounter() after you call setKinematicTarget()?

[shaneasd]

If you mean this bit https://gameworksdocs.nvidia.com/PhysX/4.1/documentation/physxguide/Manual/Simulation.html?highlight=setkinematictarget#split-sim , I think that's just listing a bunch of write-through properties in no particular order. It's not intended to be a sequence of calls you should make. From my understanding I don't think it makes sense to call wake up on an actor you're moving kinematically. I believe wakeup is for dynamic objects whose dynamic objects are no longer being simulated because their energy and applied forces became negligibly low. This doesn't apply to kinematically moving objects because they essentially don't have dynamics, their energy is undefined because they have infinite inertia and they don't respond to forces.

[Levi-Armstrong]

@shaneasd Thank you for the information. I wanting to use setGlobalPose() instead of setKinemticPose(), but when using setGlobalPose() it does not trigger a simulation onContact event. Do you know if it is possible to trigger the simulation onContact event when using setGlobalPose()?

[shaneasd]

No I don't know.

However I'd be careful using setGlobalPose for this purpose. My understanding is setGlobalPose just teleports the object to the new transform. It has no record of where it's come from so no velocity. This can make collisions act in an unintuitive way because usually you want collisions to conserve momentum. This is probably why onContact isn't triggered because something like the contact velocity isn't obviously defined. setKinematicPose on the other hand basically says that over the course of a tick the object will have the required velocity to end up at the target pose so collisions are a bit easier to understand (but it does mean you don't want to use it to teleport things).

Given your context where you're only wanting collision checking (not resolution) I'm not sure which of the properties of the two methods are important but I would guess that at setKinematicPose is t least acceptable for a robot part moving what is presumably a small distance in a tick. Is there a good reason not to use it.

[Levi-Armstrong]

The particular case I am using this for is a collision checker for a robotic motion planner. In this case the contact forces and velocities are not importance, but the contact points, distance or penetration at the end state are what I am after. Right now I am using the setKinematicPose which requires two simulation calls to get the contact information at the end location. I was hopping by using the setGlobalPose I could avoid making two calls to the simulate.

[kstorey-nvidia]

You could potentially call setGlobalPose and setKinematicTarget. SetGlobalPose will move the actor to the location (so collision is performed in that pose) and setKinematicTarget will wake the kinematic actor up so that it actually generates contacts.

[Levi-Armstrong]

I will tray that and see if it works.

[shaneasd]

Presumably you could call wakeUp instead of setKinematicTarget if the issue is just that it's sleeping?

[kstorey-nvidia]

Kinematic actors are a special-case in which sleeping state is based on whether they had a target set. It is illegal to explicitly call wakeUp() on a kinematic actor and will produce an error.

[Levi-Armstrong]

@kstorey-nvidia your recommendation worked where I call setGlobalPose() followed by setKinematicPose() with the same transform eliminates the need to call simulation twice. Thank you.

[Levi-Armstrong]

@kstorey-nvidia I have one more challenge to work through related to Octomaps. In our applications we leverage 3D perceptions for generating collision objects within the environment. This is usually represented by translating the octomap into several thousand boxed in the collision scene.

I originally tried to add all of the boxes to a single actor but had segfault around 6,000 and not sure if there is a limit on subshape for actors.

Then I switched to creating a single actor for each box and added it to an aggregate which seems to work but not sure what the best approach is.

Any recommendations on how best to represent 3D perception data within PhysX?

[kstorey-nvidia]

There is a 16-bit index involved in CmPtrTable, so the limit should be at 65,536 shapes per actor.

Do you have a callstack for the segfault with ~6000 shapes? Are you certain you didn't exceed 65,536?

I don't have any profile evidence to back-up what would be the best solution for this. However, my expectation would be that you can save some memory by using multiple shapes per actor.

Having said that, I would not recommend bundling 1000s of shapes under a single actor. It's not a use-case we've actively optimized for.

Perhaps using some middle-ground of, say, ~128 shapes per actor combined with aggregates might be a good way to go.

[Levi-Armstrong]

Do you have a callstack for the segfault with ~6000 shapes? Are you certain you didn't exceed 65,536?

Sorry, typo it was around the 65,536. I have an octomap with around 400,000 occupied cells.

Perhaps using some middle-ground of, say, ~128 shapes per actor combined with aggregates might be a good way to go.

I just want to make sure I understand correctly before implementing, so my planned approach is to break up the octomap into a set of actors with around 128 shapes. Then add all of the actors to a single aggregate, which ~400,000 shapes this would come to ~3125 actors. I have a few question below related to this approach.

• Do you see any issues with adding this many actors to a single aggregate?

• When adding the actors with 128 shapes to the aggregate, do you recommend provided a BVH structure?

[peiyongxian]

@Levi-Armstrong I am new to Physx, use physx as a collsion checker like you do, but i can not find the way to get distance/depth information in scene's overlap method. I would like to know how to get those informations? Can overlap method do? Or the only way is to call simulate() and processing contacts in callback function?

[kstorey-nvidia]

The overlap method is just a yes/no overlap test. It does not provide penetration information.

You have a few options available that do not require calling simulate. You can either compute the MTD between a pair of shapes using PxGeometryQuery::computePenetration or you can use immediate mode to generate contacts using PxGenerateContacts.

Hope this helps

[Levi-Armstrong]

I am currently using the simulate to process the contacts but I have used PxGeometryQuery::computePenetration in testing and plan to test the immediate mode. The reason I have stayed with using the simulate approach is I believe there are performance benefits that @kstorey-nvidia maybe can provided more details. If I understand correctly at the time simulate approach provided access to GPU acceleration and the other two approaches only ran on the CPU, but I am not sure this is true or not. @kstorey-nvidia are there any trade offs with performance between the three approaches?

[peiyongxian]

Thank you for your reply. I found that if there are not many geometries in the environment (less than 100 in my test), collision checking using simulate() method with callback to check contacts on cpu is faster than that on gpu.

[peiyongxian]

@kstorey-nvidia Really helps, thank you. I guess simulate() would be the better choice in the consideration of the broad phase algorithmn it includes which PxGenerateContacts does not have. Is that right?