diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index 83e512280..9edde18d4 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -25,10 +25,94 @@ #include +namespace +{ + +bool doCollide(uint32_t _categoryBitmask0, uint32_t _collideBitmask0, + uint32_t _categoryBitmask1, uint32_t _collideBitmask1) +{ + return (_categoryBitmask0 & _collideBitmask1) | + (_categoryBitmask1 & _collideBitmask0); +} + +} // namespace + namespace gz { namespace physics { namespace bullet_featherstone { +///////////////////////////////////////////////// +bool GzCollisionFilterCallback::needBroadphaseCollision( + btBroadphaseProxy *_proxy0, btBroadphaseProxy *_proxy1) const +{ + GzMultiBodyLinkCollider *col0 = + static_cast( + _proxy0->m_clientObject); + GzMultiBodyLinkCollider *col1 = + static_cast( + _proxy1->m_clientObject); + + if (col0 && col1) + { + // For backward compatibility, if category bitmask is not set, it + // defaults to the same value as collide bitmask. + uint32_t col0CategoryBitmask = col0->categoryBitmask.has_value() ? + col0->categoryBitmask.value() : col0->collideBitmask; + uint32_t col1CategoryBitmask = col1->categoryBitmask.has_value() ? + col1->categoryBitmask.value() : col1->collideBitmask; + // Early out if collide bitmask test fails + if (!doCollide(col0CategoryBitmask, col0->collideBitmask, + col1CategoryBitmask, col1->collideBitmask)) + { + return false; + } + } + + // Continue filtering collision based on logic in + // btOverlappingPairCache::needsBroadphaseCollision + bool collides = (_proxy0->m_collisionFilterGroup & + _proxy1->m_collisionFilterMask) != 0; + collides = collides && (_proxy1->m_collisionFilterGroup & + _proxy0->m_collisionFilterMask); + return collides; +} + +///////////////////////////////////////////////// +GzCollisionDispatcher::GzCollisionDispatcher( + btCollisionConfiguration *_collisionConfiguration) + : btCollisionDispatcher(_collisionConfiguration) +{ +} + +///////////////////////////////////////////////// +bool GzCollisionDispatcher::needsCollision(const btCollisionObject *_body0, + const btCollisionObject *_body1) +{ + const GzMultiBodyLinkCollider *col0 = + static_cast(_body0); + const GzMultiBodyLinkCollider *col1 = + static_cast(_body1); + + // Collision filtering in narrow phase. + if (col0 && col1) + { + // For backward compatibility, if category bitmask is not set, it + // defaults to the same value as collide bitmask. + uint32_t col0CategoryBitmask = col0->categoryBitmask.has_value() ? + col0->categoryBitmask.value() : col0->collideBitmask; + uint32_t col1CategoryBitmask = col1->categoryBitmask.has_value() ? + col1->categoryBitmask.value() : col1->collideBitmask; + // Early out if collide bitmask test fails + if (!doCollide(col0CategoryBitmask, col0->collideBitmask, + col1CategoryBitmask, col1->collideBitmask)) + { + return false; + } + } + + return btCollisionDispatcher::needsCollision(_body0, _body1); +} + ///////////////////////////////////////////////// WorldInfo::WorldInfo(std::string name_) : name(std::move(name_)) @@ -36,13 +120,19 @@ WorldInfo::WorldInfo(std::string name_) this->collisionConfiguration = std::make_unique(); this->dispatcher = - std::make_unique(collisionConfiguration.get()); + std::make_unique(collisionConfiguration.get()); this->broadphase = std::make_unique(); this->solver = std::make_unique(); this->world = std::make_unique( dispatcher.get(), broadphase.get(), solver.get(), collisionConfiguration.get()); + // Set custom collision filter callback for filtering based on + // surface contact parameters + this->collisionFilterCallback = std::make_unique(); + btOverlappingPairCache* pairCache = this->world->getPairCache(); + pairCache->setOverlapFilterCallback(this->collisionFilterCallback.get()); + btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher.get()); // Needed for force-torque sensor diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 53937bc1a..bc29018e4 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -53,6 +54,29 @@ namespace gz { namespace physics { namespace bullet_featherstone { +/// \brief Custom collision filter callback struct for handling +/// collisions based on surface contact parameters +struct GzCollisionFilterCallback : public btOverlapFilterCallback +{ + /// \brief Overrides base struct's function for additional collision + /// filtering based on surface contact parameters + bool needBroadphaseCollision(btBroadphaseProxy *_proxy0, + btBroadphaseProxy *_proxy1) const override; +}; + +/// \brief Custom gz collision dispatcher +class GzCollisionDispatcher : public btCollisionDispatcher +{ + /// \brief Constructor + public: explicit GzCollisionDispatcher( + btCollisionConfiguration *_collisionConfiguration); + + /// \brief Overrides base struct's function for additional collision + /// filtering based on surface contact parameters + public: bool needsCollision(const btCollisionObject *_body0, + const btCollisionObject *_body1) override; +}; + /// \brief The Info structs are used for three reasons: /// 1) Holding extra information such as the name /// that will be different from the underlying engine @@ -67,16 +91,18 @@ struct WorldInfo { std::string name; std::unique_ptr collisionConfiguration; - std::unique_ptr dispatcher; + std::unique_ptr dispatcher; std::unique_ptr broadphase; std::unique_ptr solver; std::unique_ptr world; + std::unique_ptr collisionFilterCallback; std::unordered_map modelIndexToEntityId; std::unordered_map modelNameToEntityId; int nextModelIndex = 0; double stepSize = 0.001; + bool collisionMasksDirty = false; explicit WorldInfo(std::string name); }; @@ -187,6 +213,12 @@ class GzMultiBodyLinkCollider: public btMultiBodyLinkCollider { return btMultiBodyLinkCollider::checkCollideWithOverride(_co) && btCollisionObject::checkCollideWithOverride(_co); } + + /// \brief Collision contact surface collide bitmask parameter + public: uint16_t collideBitmask = std::numeric_limits::max(); + + /// \brief Collision contact surface category bitmask parameter + public: std::optional categoryBitmask; }; /// Link information is embedded inside the model, so all we need to store here diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index 99e40b827..b0e1573b4 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -17,6 +17,7 @@ #include +#include #include #include #include @@ -194,6 +195,87 @@ Identity EntityManagementFeatures::GetLinkOfShape( return this->ReferenceInterface(_shapeID)->link; } +///////////////////////////////////////////////// +void EntityManagementFeatures::SetCollisionFilterMask( + const Identity &_shapeID, uint16_t _mask) +{ + auto *colInfo = this->ReferenceInterface(_shapeID); + auto *linkInfo = this->ReferenceInterface(colInfo->link); + + if (_mask != linkInfo->collider->collideBitmask) + { + linkInfo->collider->collideBitmask = _mask; + + // Mark dirty if new collision flags are set so that new contacts are + // generated with up-to-date collision flags. + auto *modelInfo = this->ReferenceInterface(linkInfo->model); + auto *world = this->ReferenceInterface(modelInfo->world); + world->collisionMasksDirty = true; + } +} + +///////////////////////////////////////////////// +uint16_t EntityManagementFeatures::GetCollisionFilterMask( + const Identity &_shapeID) const +{ + auto *colInfo = this->ReferenceInterface(_shapeID); + auto *linkInfo = this->ReferenceInterface(colInfo->link); + return linkInfo->collider->collideBitmask; +} + +///////////////////////////////////////////////// +void EntityManagementFeatures::RemoveCollisionFilterMask( + const Identity &_shapeID) +{ + // Reset to default value + this->SetCollisionFilterMask(_shapeID, std::numeric_limits::max()); +} + +///////////////////////////////////////////////// +void EntityManagementFeatures::SetCategoryFilterMask( + const Identity &_shapeID, uint16_t _mask) +{ + auto *colInfo = this->ReferenceInterface(_shapeID); + auto *linkInfo = this->ReferenceInterface(colInfo->link); + + if (_mask != linkInfo->collider->categoryBitmask) + { + linkInfo->collider->categoryBitmask = _mask; + + // Mark dirty if new collision flags are set so that new contacts are + // generated with up-to-date collision flags. + auto *modelInfo = this->ReferenceInterface(linkInfo->model); + auto *world = this->ReferenceInterface(modelInfo->world); + world->collisionMasksDirty = true; + } +} + +///////////////////////////////////////////////// +uint16_t EntityManagementFeatures::GetCategoryFilterMask( + const Identity &_shapeID) const +{ + auto *colInfo = this->ReferenceInterface(_shapeID); + auto *linkInfo = this->ReferenceInterface(colInfo->link); + // For backward compatibility, if category bitmask is not set, it + // defaults to the same value as collide bitmask. + return linkInfo->collider->categoryBitmask.has_value() ? + linkInfo->collider->categoryBitmask.value() : + linkInfo->collider->collideBitmask; +} + +///////////////////////////////////////////////// +void EntityManagementFeatures::RemoveCategoryFilterMask( + const Identity &_shapeID) +{ + auto *colInfo = this->ReferenceInterface(_shapeID); + auto *linkInfo = this->ReferenceInterface(colInfo->link); + linkInfo->collider->categoryBitmask.reset(); + + auto *modelInfo = this->ReferenceInterface(linkInfo->model); + auto *world = this->ReferenceInterface(modelInfo->world); + world->collisionMasksDirty = true; +} + ///////////////////////////////////////////////// Identity EntityManagementFeatures::ConstructEmptyWorld( const Identity &/*_engineID*/, const std::string &_name) diff --git a/bullet-featherstone/src/EntityManagementFeatures.hh b/bullet-featherstone/src/EntityManagementFeatures.hh index 1cbb47813..4d1c14685 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.hh +++ b/bullet-featherstone/src/EntityManagementFeatures.hh @@ -24,6 +24,7 @@ #include #include #include +#include #include "Base.hh" @@ -32,6 +33,8 @@ namespace physics { namespace bullet_featherstone { struct EntityManagementFeatureList : gz::physics::FeatureList< + CategoryFilterMaskFeature, + CollisionFilterMaskFeature, ConstructEmptyWorldFeature, GetEngineInfo, GetJointFromModel, @@ -163,6 +166,24 @@ class EntityManagementFeatures : public: bool RemoveNestedModelByName( const Identity &_modelID, const std::string &_modelName) override; + // ----- Manage collision filter masks ----- + public: void SetCollisionFilterMask( + const Identity &_shapeID, uint16_t _mask) override; + + public: uint16_t GetCollisionFilterMask( + const Identity &_shapeID) const override; + + public: void RemoveCollisionFilterMask(const Identity &_shapeID) override; + + // ----- Manage category filter masks ----- + public: void SetCategoryFilterMask( + const Identity &_shapeID, uint16_t _mask) override; + + public: uint16_t GetCategoryFilterMask( + const Identity &_shapeID) const override; + + public: void RemoveCategoryFilterMask(const Identity &_shapeID) override; + // ----- Construct empty entities ----- public: Identity ConstructEmptyWorld( const Identity &_engineID, const std::string & _name) override; diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 7db2ceb77..62905bc52 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -43,6 +43,7 @@ #include +#include #include #include #include @@ -1257,6 +1258,8 @@ bool SDFFeatures::AddSdfCollision( double restitution = 0.0; double torsionalCoefficient = 1.0; double rollingFriction = 0.0; + uint16_t collideBitmask = std::numeric_limits::max(); + std::optional categoryBitmask; if (const auto *surface = _collision.Surface()) { if (const auto *friction = surface->Friction()) @@ -1291,6 +1294,19 @@ bool SDFFeatures::AddSdfCollision( if (const auto r = bounce->FindElement("restitution_coefficient")) restitution = r->Get(); } + + if (const auto contact = surfaceElement->FindElement("contact")) + { + if (const auto bitmask = contact->FindElement("collide_bitmask")) + { + // Get only supports uint32_t so cast back to uint16_t + collideBitmask = static_cast(bitmask->Get()); + } + if (const auto bitmask = contact->FindElement("category_bitmask")) + { + categoryBitmask = static_cast(bitmask->Get()); + } + } } } @@ -1322,7 +1338,9 @@ bool SDFFeatures::AddSdfCollision( if (!linkInfo->collider) { - this->CreateLinkCollider(_linkID, _isStatic, shape.get(), + + this->CreateLinkCollider(_linkID, _isStatic, collideBitmask, + categoryBitmask, shape.get(), btInertialToCollision); linkInfo->collider->setRestitution(static_cast(restitution)); @@ -1472,6 +1490,7 @@ Identity SDFFeatures::ConstructSdfJoint( ///////////////////////////////////////////////// void SDFFeatures::CreateLinkCollider(const Identity &_linkID, bool _isStatic, + uint16_t _collideBitmask, std::optional _categoryBitmask, btCollisionShape *_shape, const btTransform &_shapeTF) { auto *linkInfo = this->ReferenceInterface(_linkID); @@ -1526,6 +1545,16 @@ void SDFFeatures::CreateLinkCollider(const Identity &_linkID, bool _isStatic, isFixed = totalLinkDofs == 0; } } + + // Set the collideBimask variable in the GzMultiBodyLinkCollider class + // instead of calling setCollisionFlags so we don't override bullet's + // internal collision flags which are used to indicate whether a collision + // is static, dynamic, kinematic, etc + // Set these masks before calling addCollisionObject so that + // the masks are available during the needBroadPhaseCollision check + linkInfo->collider->collideBitmask = _collideBitmask; + linkInfo->collider->categoryBitmask = _categoryBitmask; + if (_isStatic || isFixed) { worldInfo->world->addCollisionObject( diff --git a/bullet-featherstone/src/SDFFeatures.hh b/bullet-featherstone/src/SDFFeatures.hh index 2e8d3c9df..dd3d8f83c 100644 --- a/bullet-featherstone/src/SDFFeatures.hh +++ b/bullet-featherstone/src/SDFFeatures.hh @@ -18,6 +18,7 @@ #ifndef GZ_PHYSICS_BULLET_FEATHERSTONE_SRC_SDFFEATURES_HH_ #define GZ_PHYSICS_BULLET_FEATHERSTONE_SRC_SDFFEATURES_HH_ +#include #include #include @@ -82,9 +83,13 @@ class SDFFeatures : /// \brief Create and initialze the link collider in link info /// \param[in] _linkID ID of link to create the collider for /// \param[in] _isStatic True if the link is static + /// \param[in] _collideBitmask Collide bitmask + /// \param[in] _categoryBitmask Category bitmask /// \param[in] _shape Collision shape to attach to link - private: void CreateLinkCollider(const Identity &_linkID, - bool _isStatic, btCollisionShape *_shape = nullptr, + private: void CreateLinkCollider(const Identity &_linkID, bool _isStatic, + uint16_t _collideBitmask = std::numeric_limits::max(), + std::optional _categoryBitmask = std::nullopt, + btCollisionShape *_shape = nullptr, const btTransform &_shapeTF = btTransform::getIdentity()); }; diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index 15899dfa8..dd5cb0dd8 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -161,6 +161,19 @@ void enforceFixedConstraint( child->SetBaseWorldTransform(newChildBaseTf); } +void clearCollisionCache(btMultiBodyDynamicsWorld *_world) +{ + btDispatcher* dispatcher = _world->getDispatcher(); + btOverlappingPairCache* pairCache = + _world->getBroadphase()->getOverlappingPairCache(); + btBroadphasePairArray &pairArray = pairCache->getOverlappingPairArray(); + // Iterate backwards to safely handle removals from the array + for (int i = pairArray.size() - 1; i >= 0; --i) + { + pairCache->cleanOverlappingPair(pairArray[i], dispatcher); + } +} + ///////////////////////////////////////////////// void SimulationFeatures::WorldForwardStep( const Identity &_worldID, @@ -221,6 +234,12 @@ void SimulationFeatures::WorldForwardStep( } } + // Regenerate the cache if the collision masks have been updated. + if (worldInfo->collisionMasksDirty) + { + clearCollisionCache(worldInfo->world.get()); + } + // \todo(iche033) Stepping sim with varying dt may not work properly. // One example is the motor constraint that's created in // JointFeatures::SetJointVelocityCommand which assumes a fixed step @@ -238,6 +257,14 @@ void SimulationFeatures::WorldForwardStep( } } + if (worldInfo->collisionMasksDirty) + { + // manual sync to get up-to-date contacts for the current frame + // by forcing collision detection again + worldInfo->world->getCollisionWorld()->performDiscreteCollisionDetection(); + worldInfo->collisionMasksDirty = false; + } + this->WriteRequiredData(_h); this->Write(_h.Get()); } diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 7739cdcf3..900efe2cb 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -44,6 +44,7 @@ class BitmaskContactFilter : public dart::collision::BodyNodeCollisionFilter public: using DartShapeConstPtr = const dart::dynamics::ShapeNode*; private: std::unordered_map bitmaskMap; + private: std::unordered_map categoryBitmaskMap; public: bool ignoresCollision( DartCollisionConstPtr _object1, @@ -58,9 +59,20 @@ class BitmaskContactFilter : public dart::collision::BodyNodeCollisionFilter auto shape1Iter = bitmaskMap.find(shapeNode1); auto shape2Iter = bitmaskMap.find(shapeNode2); - if (shape1Iter != bitmaskMap.end() && shape2Iter != bitmaskMap.end() && - ((shape1Iter->second & shape2Iter->second) == 0)) - return true; + if (shape1Iter != bitmaskMap.end() && shape2Iter != bitmaskMap.end()) + { + // For backward compatibility, if category bitmask is not set, it + // defaults to the same value as collide bitmask. + auto category1Iter = categoryBitmaskMap.find(shapeNode1); + auto category2Iter = categoryBitmaskMap.find(shapeNode2); + uint16_t categoryMask1 = (category1Iter != categoryBitmaskMap.end()) ? + category1Iter->second : shape1Iter->second; + uint16_t categoryMask2 = (category2Iter != categoryBitmaskMap.end()) ? + category2Iter->second : shape2Iter->second; + + return !((categoryMask1 & shape2Iter->second) | + (categoryMask2 & shape1Iter->second)); + } return false; } @@ -85,12 +97,35 @@ class BitmaskContactFilter : public dart::collision::BodyNodeCollisionFilter bitmaskMap.erase(shapeIter); } + public: void SetIgnoredCategory(DartShapeConstPtr _shapePtr, uint16_t _mask) + { + categoryBitmaskMap[_shapePtr] = _mask; + } + + public: uint16_t GetIgnoredCategory(DartShapeConstPtr _shapePtr) const + { + auto shapeIter = categoryBitmaskMap.find(_shapePtr); + if (shapeIter != categoryBitmaskMap.end()) + return shapeIter->second; + // For backward compatibility, if category bitmask is not set, it + // defaults to the same value as collide bitmask. + return GetIgnoredCollision(_shapePtr); + } + + public: void RemoveIgnoredCategory(DartShapeConstPtr _shapePtr) + { + auto shapeIter = categoryBitmaskMap.find(_shapePtr); + if (shapeIter != categoryBitmaskMap.end()) + categoryBitmaskMap.erase(shapeIter); + } + public: void RemoveSkeletonCollisions(dart::dynamics::SkeletonPtr _skelPtr) { for (std::size_t i = 0; i < _skelPtr->getNumShapeNodes(); ++i) { auto shapePtr = _skelPtr->getShapeNode(i); this->RemoveIgnoredCollision(shapePtr); + this->RemoveIgnoredCategory(shapePtr); } } @@ -850,6 +885,33 @@ void EntityManagementFeatures::RemoveCollisionFilterMask( filterPtr->RemoveIgnoredCollision(shapeNode); } +void EntityManagementFeatures::SetCategoryFilterMask( + const Identity &_shapeID, uint16_t _mask) +{ + const auto shapeNode = this->ReferenceInterface(_shapeID)->node; + const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); + const auto filterPtr = GetFilterPtr(this, worldID); + filterPtr->SetIgnoredCategory(shapeNode, _mask); +} + +uint16_t EntityManagementFeatures::GetCategoryFilterMask( + const Identity &_shapeID) const +{ + const auto shapeNode = this->ReferenceInterface(_shapeID)->node; + const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); + const auto filterPtr = GetFilterPtr(this, worldID); + return filterPtr->GetIgnoredCategory(shapeNode); +} + +void EntityManagementFeatures::RemoveCategoryFilterMask( + const Identity &_shapeID) +{ + const auto shapeNode = this->ReferenceInterface(_shapeID)->node; + const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); + const auto filterPtr = GetFilterPtr(this, worldID); + filterPtr->RemoveIgnoredCategory(shapeNode); +} + Identity EntityManagementFeatures::GetWorldModel(const Identity &_worldID) const { auto modelID = this->modelProxiesToWorld.MaybeAt(_worldID); diff --git a/dartsim/src/EntityManagementFeatures.hh b/dartsim/src/EntityManagementFeatures.hh index fdb485e27..91a28004d 100644 --- a/dartsim/src/EntityManagementFeatures.hh +++ b/dartsim/src/EntityManagementFeatures.hh @@ -37,6 +37,7 @@ namespace dartsim { struct EntityManagementFeatureList : FeatureList< GetEntities, RemoveEntities, + CategoryFilterMaskFeature, ConstructEmptyWorldFeature, ConstructEmptyModelFeature, ConstructEmptyNestedModelFeature, @@ -178,6 +179,15 @@ class GZ_PHYSICS_DARTSIM_PLUGIN_VISIBLE EntityManagementFeatures : public: void RemoveCollisionFilterMask(const Identity &_shapeID) override; + // ----- Manage category filter masks ----- + public: void SetCategoryFilterMask( + const Identity &_shapeID, uint16_t _mask) override; + + public: uint16_t GetCategoryFilterMask( + const Identity &_shapeID) const override; + + public: void RemoveCategoryFilterMask(const Identity &_shapeID) override; + // ----- World model feature ----- public: Identity GetWorldModel(const Identity &_worldID) const override; }; diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 60c8aa77b..6f89bfe92 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -916,6 +917,7 @@ Identity SDFFeatures::ConstructSdfCollision( // TODO(addisu) We are using the coefficient specified in the tag. // Either add parameters specific to DART or generic to all physics engines. uint16_t collideBitmask = 0xFF; + std::optional categoryBitmask; if (_collision.Element()) { const auto &odeFriction = _collision.Element() @@ -989,12 +991,13 @@ Identity SDFFeatures::ConstructSdfCollision( surfaceBounce->Get("restitution_coefficient")); } #endif - // TODO(anyone) add category_bitmask as well - const auto bitmaskElement = _collision.Element() + const auto contactElement = _collision.Element() ->GetElement("surface") ->GetElement("contact"); - if (bitmaskElement->HasElement("collide_bitmask")) - collideBitmask = bitmaskElement->Get("collide_bitmask"); + if (contactElement->HasElement("collide_bitmask")) + collideBitmask = contactElement->Get("collide_bitmask"); + if (contactElement->HasElement("category_bitmask")) + categoryBitmask = contactElement->Get("category_bitmask"); } node->setRelativeTransform(ResolveSdfPose(_collision.SemanticPose()) * @@ -1005,6 +1008,8 @@ Identity SDFFeatures::ConstructSdfCollision( auto identity = this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); this->SetCollisionFilterMask(identity, collideBitmask); + if (categoryBitmask.has_value()) + this->SetCategoryFilterMask(identity, categoryBitmask.value()); return identity; } diff --git a/include/gz/physics/Shape.hh b/include/gz/physics/Shape.hh index 552033f13..28be5229f 100644 --- a/include/gz/physics/Shape.hh +++ b/include/gz/physics/Shape.hh @@ -230,7 +230,8 @@ namespace gz { /// \brief Set the collision filter bitmask of this shape /// \param[in] _mask - /// A sixteen bit bitmask, if the bitwise AND of two objects + /// A sixteen bit bitmask. + /// If ((category1 & collision2) | (category2 & collision1)) /// evaluates to 0, their collision will be ignored public: void SetCollisionFilterMask(const uint16_t _mask); @@ -256,6 +257,42 @@ namespace gz }; }; + ///////////////////////////////////////////////// + class GZ_PHYSICS_VISIBLE CategoryFilterMaskFeature + : public virtual Feature + { + public: template + class Shape : public virtual Feature::Shape + { + /// \brief Set the category filter bitmask of this shape + /// \param[in] _mask + /// A sixteen bit bitmask. + /// If ((category1 & collision2) | (category2 & collision1)) + /// evaluates to 0, their collision will be ignored + public: void SetCategoryFilterMask(const uint16_t _mask); + + /// \brief Get the category filter bitmask of this shape + /// \return The category filter bitmask of this shape + public: uint16_t GetCategoryFilterMask() const; + + /// \brief Removes the category filter bitmask from this shape + public: void RemoveCategoryFilterMask(); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual void SetCategoryFilterMask( + const Identity &_shapeID, const uint16_t _mask) = 0; + + public: virtual uint16_t GetCategoryFilterMask( + const Identity &_shapeID) const = 0; + + public: virtual void RemoveCategoryFilterMask( + const Identity &_shapeID) = 0; + }; + }; + ///////////////////////////////////////////////// /// \brief This feature retrieves the shape's slip compliance of the first /// and second friction direction in the friction pyramid model. diff --git a/include/gz/physics/detail/Shape.hh b/include/gz/physics/detail/Shape.hh index 507e79342..203dce4cc 100644 --- a/include/gz/physics/detail/Shape.hh +++ b/include/gz/physics/detail/Shape.hh @@ -127,6 +127,33 @@ namespace gz ->RemoveCollisionFilterMask(this->identity); } + ///////////////////////////////////////////////// + template + void CategoryFilterMaskFeature::Shape + ::SetCategoryFilterMask(const uint16_t _mask) + { + this->template Interface() + ->SetCategoryFilterMask(this->identity, _mask); + } + + ///////////////////////////////////////////////// + template + uint16_t CategoryFilterMaskFeature::Shape + ::GetCategoryFilterMask() const + { + return this->template Interface() + ->GetCategoryFilterMask(this->identity); + } + + ///////////////////////////////////////////////// + template + void CategoryFilterMaskFeature::Shape + ::RemoveCategoryFilterMask() + { + this->template Interface() + ->RemoveCategoryFilterMask(this->identity); + } + ///////////////////////////////////////////////// template auto GetShapeFrictionPyramidSlipCompliance::Shape diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh index 212f9c35a..38f6ba45f 100644 --- a/test/common_test/Worlds.hh +++ b/test/common_test/Worlds.hh @@ -52,8 +52,9 @@ const auto kMultipleCollisionsSdf = CommonTestWorld("multiple_collisions.sdf"); const auto kPendulumJointWrenchSdf = CommonTestWorld("pendulum_joint_wrench.sdf"); const auto kPoseOffsetSdf = CommonTestWorld("pose_offset.sdf"); -const auto kShapesWorld = CommonTestWorld("shapes.world"); +const auto kShapesCategoryBitmaskWorld = CommonTestWorld("shapes_category_bitmask.sdf"); const auto kShapesBitmaskWorld = CommonTestWorld("shapes_bitmask.sdf"); +const auto kShapesWorld = CommonTestWorld("shapes.world"); const auto kSlipComplianceSdf = CommonTestWorld("slip_compliance.sdf"); const auto kSphereSdf = CommonTestWorld("sphere.sdf"); const auto kStringPendulumSdf = CommonTestWorld("string_pendulum.sdf"); diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index e8a0c18ec..eadad8b3a 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -16,9 +16,12 @@ */ #include +#include #include #include #include +#include +#include #include #include @@ -1340,15 +1343,35 @@ TEST_F(SimulationFeaturesCollisionFilter, CollideBitmasks) auto filteredBox = world->GetModel("box_filtered"); auto collidingBox = world->GetModel("box_colliding"); + auto collidingShape = collidingBox->GetLink(0)->GetShape(0); + auto filteredShape = filteredBox->GetLink(0)->GetShape(0); + auto baseShape = baseBox->GetLink(0)->GetShape(0); + EXPECT_EQ(0x01, baseShape->GetCollisionFilterMask()); + EXPECT_EQ(0x02, filteredShape->GetCollisionFilterMask()); + EXPECT_EQ(0x03, collidingShape->GetCollisionFilterMask()); auto checkedOutput = StepWorld(world, true).first; EXPECT_TRUE(checkedOutput); auto contacts = world->GetContactsFromLastStep(); // Only box_colliding should collide with box_base EXPECT_NE(0u, contacts.size()); + for (auto &contact : contacts) + { + const auto &contactPoint = contact.template Get< + gz::physics::World3d::ContactPoint>(); + ASSERT_TRUE(contactPoint.collision1); + ASSERT_TRUE(contactPoint.collision2); + EXPECT_NE(contactPoint.collision1, contactPoint.collision2); + auto c1 = contactPoint.collision1; + auto c2 = contactPoint.collision2; + auto m1 = c1->GetLink()->GetModel(); + auto m2 = c2->GetLink()->GetModel(); + EXPECT_TRUE(m1->GetName() == "box_base" || + m1->GetName() == "box_colliding"); + EXPECT_TRUE(m2->GetName() == "box_base" || + m2->GetName() == "box_colliding"); + } // Now disable collisions for the colliding box as well - auto collidingShape = collidingBox->GetLink(0)->GetShape(0); - auto filteredShape = filteredBox->GetLink(0)->GetShape(0); collidingShape->SetCollisionFilterMask(0xF0); // Also test the getter EXPECT_EQ(0xF0, collidingShape->GetCollisionFilterMask()); @@ -1370,6 +1393,153 @@ TEST_F(SimulationFeaturesCollisionFilter, CollideBitmasks) } } +using FeaturesCategoryFilter = gz::physics::FeatureList< + FeaturesCollisionFilter, + gz::physics::CategoryFilterMaskFeature +>; + +using SimulationFeaturesCategoryFilter = + SimulationFeaturesTest; + +TEST_F(SimulationFeaturesCategoryFilter, CategoryBitmasks) +{ + for (const std::string &name : this->pluginNames) + { + // World consists of 2 category A shapes, 2 category B shapes, + // and 1 category C shape. The shapes in the same category + // should not collide with each other. + auto world = LoadPluginAndWorld( + this->loader, + name, + common_test::worlds::kShapesCategoryBitmaskWorld); + + auto categoryABox0 = world->GetModel("category_a_box_0"); + auto categoryABox1 = world->GetModel("category_a_box_1"); + auto categoryBBox0 = world->GetModel("category_b_box_0"); + auto categoryBBox1 = world->GetModel("category_b_box_1"); + auto categoryCBox0 = world->GetModel("category_c_box_0"); + + auto categoryABox0Shape = categoryABox0->GetLink(0)->GetShape(0); + auto categoryABox1Shape = categoryABox1->GetLink(0)->GetShape(0); + auto categoryBBox0Shape = categoryBBox0->GetLink(0)->GetShape(0); + auto categoryBBox1Shape = categoryBBox1->GetLink(0)->GetShape(0); + auto categoryCBox0Shape = categoryCBox0->GetLink(0)->GetShape(0); + + EXPECT_EQ(1, categoryABox0Shape->GetCategoryFilterMask()); + EXPECT_EQ(6, categoryABox0Shape->GetCollisionFilterMask()); + EXPECT_EQ(1, categoryABox1Shape->GetCategoryFilterMask()); + EXPECT_EQ(6, categoryABox1Shape->GetCollisionFilterMask()); + EXPECT_EQ(2, categoryBBox0Shape->GetCategoryFilterMask()); + EXPECT_EQ(5, categoryBBox0Shape->GetCollisionFilterMask()); + EXPECT_EQ(2, categoryBBox1Shape->GetCategoryFilterMask()); + EXPECT_EQ(5, categoryBBox1Shape->GetCollisionFilterMask()); + EXPECT_EQ(4, categoryCBox0Shape->GetCategoryFilterMask()); + EXPECT_EQ(3, categoryCBox0Shape->GetCollisionFilterMask()); + + auto checkedOutput = StepWorld(world, true).first; + EXPECT_TRUE(checkedOutput); + auto contacts = world->GetContactsFromLastStep(); + EXPECT_NE(0u, contacts.size()); + + // Here is a map of contact collision pairs that we expect to see. + // The first element is the collision name, and the second element + // is a list of collisions that we expect it to collide with. + std::unordered_map> + expectedCollisions; + expectedCollisions["category_a_box_0"] = {"category_b_box_1", + "category_c_box_0"}; + expectedCollisions["category_a_box_1"] = {"category_b_box_1", + "category_c_box_0"}; + expectedCollisions["category_b_box_0"] = {"category_c_box_0"}; + expectedCollisions["category_b_box_1"] = {"category_a_box_0", + "category_a_box_1", + "category_c_box_0"}; + expectedCollisions["category_c_box_0"] = {"category_a_box_0", + "category_a_box_1", + "category_b_box_0", + "ground_plane"}; + expectedCollisions["ground_plane"] = {"category_c_box_0"}; + + + // Verify expected collisions against actual contacts reported by the + // physics engine + auto checkCollisions = []( + decltype(contacts) _contacts, + const std::unordered_map> + &_expectedCollisions) + { + for (auto &contact : _contacts) + { + const auto &contactPoint = contact.template Get< + gz::physics::World3d::ContactPoint>(); + ASSERT_TRUE(contactPoint.collision1); + ASSERT_TRUE(contactPoint.collision2); + EXPECT_NE(contactPoint.collision1, contactPoint.collision2); + auto c1 = contactPoint.collision1; + auto c2 = contactPoint.collision2; + auto m1 = c1->GetLink()->GetModel(); + auto m2 = c2->GetLink()->GetModel(); + auto m1It = _expectedCollisions.find(m1->GetName()); + EXPECT_NE(_expectedCollisions.end(), m1It); + const std::vector &model1CollidingShapes = m1It->second; + auto m2It = std::find(model1CollidingShapes.begin(), + model1CollidingShapes.end(), + m2->GetName()); + EXPECT_NE(model1CollidingShapes.end(), m2It); + } + }; + + checkCollisions(contacts, expectedCollisions); + + // Now set category and collide bitmasks for cateory_b_box_1 to be the same + // as the shapes in Category A + categoryBBox1Shape->SetCategoryFilterMask(1); + categoryBBox1Shape->SetCollisionFilterMask(6); + EXPECT_EQ(1, categoryBBox1Shape->GetCategoryFilterMask()); + // Step and check collisions + checkedOutput = StepWorld(world, false).first; + EXPECT_FALSE(checkedOutput); + auto contacts2 = world->GetContactsFromLastStep(); + EXPECT_NE(0u, contacts2.size()); + + // Update the list of expected collision pairs and verify. + // category_b_box_1 should start colliding with the other category B shape. + expectedCollisions["category_b_box_0"].push_back("category_b_box_1"); + expectedCollisions["category_b_box_1"].push_back("category_b_box_0"); + // category_b_box_1 should no longer collide with category A shapes. + auto &catABox0Collisions = expectedCollisions["category_a_box_0"]; + catABox0Collisions.erase(std::remove(catABox0Collisions.begin(), + catABox0Collisions.end(), "category_b_box_1"), catABox0Collisions.end()); + auto &catABox1Collisions = expectedCollisions["category_a_box_1"]; + catABox1Collisions.erase(std::remove(catABox1Collisions.begin(), + catABox1Collisions.end(), "category_b_box_1"), catABox1Collisions.end()); + + checkCollisions(contacts2, expectedCollisions); + + // Now remove category bitmask for category_a_box_0 and verify that it + // returns its is now the same as its collide bitmask + categoryABox0Shape->RemoveCategoryFilterMask(); + EXPECT_EQ(categoryABox0Shape->GetCollisionFilterMask(), + categoryABox0Shape->GetCategoryFilterMask()); + // Step and check collisions + checkedOutput = StepWorld(world, false).first; + EXPECT_FALSE(checkedOutput); + auto contacts3 = world->GetContactsFromLastStep(); + EXPECT_NE(0u, contacts3.size()); + // There should be more contacts + EXPECT_LT(contacts2.size(), contacts3.size()); + + // Update the list of expected collision pairs and verify. + // Category A shapes should start colliding with each other + expectedCollisions["category_a_box_0"].push_back("category_a_box_1"); + expectedCollisions["category_a_box_1"].push_back("category_a_box_0"); + // It should also start colliding with category_b_box_1 which wa + // previously updated to category A masks. + expectedCollisions["category_a_box_0"].push_back("category_b_box_1"); + + checkCollisions(contacts3, expectedCollisions); + } +} TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) { diff --git a/test/common_test/worlds/shapes_category_bitmask.sdf b/test/common_test/worlds/shapes_category_bitmask.sdf new file mode 100644 index 000000000..4005ec478 --- /dev/null +++ b/test/common_test/worlds/shapes_category_bitmask.sdf @@ -0,0 +1,246 @@ + + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 10 0 1.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + 1 + 6 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 10 0 1.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + 1 + 6 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 10 1.1 1.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + 2 + 5 + + + + + + + 1 1 1 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 10 0.3 2.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + 2 + 5 + + + + + + + 1 1 1 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 10 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 5 5 1 + + + + + + 4 + 3 + + + + + + + 5 5 1 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + + + + +