diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 48ae13e07..990ca5631 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -22,13 +22,14 @@ #include #include -#include #include #include #include #include +#include "GzOdeCollisionDetector.hh" + namespace gz { namespace physics { namespace dartsim { @@ -724,11 +725,12 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( const Identity &/*_engineID*/, const std::string &_name) { const auto &world = std::make_shared(_name); - world->getConstraintSolver()->setCollisionDetector( - dart::collision::OdeCollisionDetector::create()); + auto collisionDetector = dart::collision::GzOdeCollisionDetector::create(); + world->getConstraintSolver()->setCollisionDetector(collisionDetector); - // TODO(anyone) We need a machanism to configure maxNumContacts at runtime. auto &collOpt = world->getConstraintSolver()->getCollisionOption(); + // Set the max number of contacts for all collision objects + // in the world collOpt.maxNumContacts = 10000; world->getConstraintSolver()->getCollisionOption().collisionFilter = diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc new file mode 100644 index 000000000..228ee9562 --- /dev/null +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include + +#include "GzOdeCollisionDetector.hh" + +using namespace dart; +using namespace collision; + +///////////////////////////////////////////////// +GzOdeCollisionDetector::GzOdeCollisionDetector() + : OdeCollisionDetector() +{ +} + +///////////////////////////////////////////////// +GzOdeCollisionDetector::Registrar + GzOdeCollisionDetector::mRegistrar{ + GzOdeCollisionDetector::getStaticType(), + []() -> std::shared_ptr { + return GzOdeCollisionDetector::create(); + }}; + +///////////////////////////////////////////////// +std::shared_ptr GzOdeCollisionDetector::create() +{ + return std::shared_ptr(new GzOdeCollisionDetector()); +} + +///////////////////////////////////////////////// +bool GzOdeCollisionDetector::collide( + CollisionGroup *_group, + const CollisionOption &_option, + CollisionResult *_result) +{ + bool ret = OdeCollisionDetector::collide(_group, _option, _result); + this->LimitCollisionPairMaxContacts(_result); + return ret; +} + +///////////////////////////////////////////////// +bool GzOdeCollisionDetector::collide( + CollisionGroup *_group1, + CollisionGroup *_group2, + const CollisionOption &_option, + CollisionResult *_result) +{ + bool ret = OdeCollisionDetector::collide(_group1, _group2, _option, _result); + this->LimitCollisionPairMaxContacts(_result); + return ret; +} + +///////////////////////////////////////////////// +void GzOdeCollisionDetector::SetCollisionPairMaxContacts( + std::size_t _maxContacts) +{ + this->maxCollisionPairContacts = _maxContacts; +} + +///////////////////////////////////////////////// +std::size_t GzOdeCollisionDetector::GetCollisionPairMaxContacts() const +{ + return this->maxCollisionPairContacts; +} + +///////////////////////////////////////////////// +void GzOdeCollisionDetector::LimitCollisionPairMaxContacts( + CollisionResult *_result) +{ + if (this->maxCollisionPairContacts == + std::numeric_limits::max()) + return; + + auto allContacts = _result->getContacts(); + _result->clear(); + + std::unordered_map> + contactMap; + + for (auto &contact : allContacts) + { + auto &count = + contactMap[contact.collisionObject1][contact.collisionObject2]; + count++; + auto &otherCount = + contactMap[contact.collisionObject2][contact.collisionObject1]; + std::size_t total = count + otherCount; + if (total <= this->maxCollisionPairContacts) + { + _result->addContact(contact); + } + } +} diff --git a/dartsim/src/GzOdeCollisionDetector.hh b/dartsim/src/GzOdeCollisionDetector.hh new file mode 100644 index 000000000..de8233e93 --- /dev/null +++ b/dartsim/src/GzOdeCollisionDetector.hh @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include + +namespace dart { +namespace collision { + +class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector +{ + // Documentation inherited + public: bool collide( + CollisionGroup* group, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) override; + + // Documentation inherited + public: bool collide( + CollisionGroup* group1, + CollisionGroup* group2, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) override; + + /// \brief Set the maximum number of contacts between a pair of collision + /// objects + /// \param[in] _maxContacts Maximum number of contacts between a pair of + /// collision objects. + public: void SetCollisionPairMaxContacts(std::size_t _maxContacts); + + /// \brief Get the maximum number of contacts between a pair of collision + /// objects + /// \return Maximum number of contacts between a pair of collision objects. + public: std::size_t GetCollisionPairMaxContacts() const; + + + /// \brief Create the GzOdeCollisionDetector + public: static std::shared_ptr create(); + + /// Constructor + protected: GzOdeCollisionDetector(); + + /// \brief Limit max number of contacts between a pair of collision objects. + /// The function modifies the contacts vector inside the CollisionResult + /// object to cap the number of contacts for each collision pair based on the + /// maxCollisionPairContacts value + private: void LimitCollisionPairMaxContacts(CollisionResult *_result); + + /// \brief Maximum number of contacts between a pair of collision objects. + private: std::size_t maxCollisionPairContacts = + std::numeric_limits::max(); + + private: static Registrar mRegistrar; +}; + +} +} diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc index d05eeefb8..da7088580 100644 --- a/dartsim/src/WorldFeatures.cc +++ b/dartsim/src/WorldFeatures.cc @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -30,8 +29,10 @@ #include +#include "GzOdeCollisionDetector.hh" #include "WorldFeatures.hh" + namespace gz { namespace physics { namespace dartsim { @@ -53,7 +54,7 @@ void WorldFeatures::SetWorldCollisionDetector( } else if (_collisionDetector == "ode") { - collisionDetector = dart::collision::OdeCollisionDetector::create(); + collisionDetector = dart::collision::GzOdeCollisionDetector::create(); } else if (_collisionDetector == "dart") { @@ -96,6 +97,46 @@ WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( return world->getGravity(); } +///////////////////////////////////////////////// +void WorldFeatures::SetWorldCollisionPairMaxContacts( + const Identity &_id, std::size_t _maxContacts) +{ + auto world = this->ReferenceInterface(_id); + auto collisionDetector = + world->getConstraintSolver()->getCollisionDetector(); + + auto odeCollisionDetector = + std::dynamic_pointer_cast( + collisionDetector); + if (odeCollisionDetector) + { + odeCollisionDetector->SetCollisionPairMaxContacts(_maxContacts); + } + else + { + gzwarn << "Currently max contacts feature is only supported by the " + << "ode collision detector in dartsim." << std::endl; + } +} + +///////////////////////////////////////////////// +std::size_t WorldFeatures::GetWorldCollisionPairMaxContacts( + const Identity &_id) const +{ + auto world = this->ReferenceInterface(_id); + auto collisionDetector = + world->getConstraintSolver()->getCollisionDetector(); + auto odeCollisionDetector = + std::dynamic_pointer_cast( + collisionDetector); + if (odeCollisionDetector) + { + return odeCollisionDetector->GetCollisionPairMaxContacts(); + } + + return 0u; +} + ///////////////////////////////////////////////// void WorldFeatures::SetWorldSolver(const Identity &_id, const std::string &_solver) diff --git a/dartsim/src/WorldFeatures.hh b/dartsim/src/WorldFeatures.hh index a7fd9855b..da15810dd 100644 --- a/dartsim/src/WorldFeatures.hh +++ b/dartsim/src/WorldFeatures.hh @@ -30,6 +30,7 @@ namespace dartsim { struct WorldFeatureList : FeatureList< CollisionDetector, + CollisionPairMaxContacts, Gravity, Solver > { }; @@ -53,6 +54,14 @@ class WorldFeatures : // Documentation inherited public: LinearVectorType GetWorldGravity(const Identity &_id) const override; + // Documentation inherited + public: void SetWorldCollisionPairMaxContacts( + const Identity &_id, std::size_t _maxContacts) override; + + // Documentation inherited + public: std::size_t GetWorldCollisionPairMaxContacts(const Identity &_id) + const override; + // Documentation inherited public: void SetWorldSolver(const Identity &_id, const std::string &_solver) override; diff --git a/include/gz/physics/World.hh b/include/gz/physics/World.hh index f99e65951..dabc453ef 100644 --- a/include/gz/physics/World.hh +++ b/include/gz/physics/World.hh @@ -130,6 +130,46 @@ namespace gz }; }; + ///////////////////////////////////////////////// + class GZ_PHYSICS_VISIBLE CollisionPairMaxContacts: + public virtual Feature + { + /// \brief The World API for getting and setting the maximum + /// number of contacts between two entities. + public: template + class World : public virtual Feature::World + { + /// \brief Set the maximum number of contacts allowed between two + /// entities. + /// \param[in] _maxContacts Maximum number of contacts. + public: void SetCollisionPairMaxContacts(std::size_t _maxContacts); + + /// \brief Get the maximum number of contacts allowed between two + /// entities. + /// \return Maximum number of contacts. + public: std::size_t GetCollisionPairMaxContacts() const; + }; + + /// \private The implementation API for getting and setting max contacts. + public: template + class Implementation : public virtual Feature::Implementation + { + /// \brief Implementation API for setting the maximum number of + /// contacts between two entities. + /// \param[in] _id Identity of the world. + /// \param[in] _maxContacts Maximum number of contacts. + public: virtual void SetWorldCollisionPairMaxContacts( + const Identity &_id, std::size_t _maxContacts) = 0; + + /// \brief Implementation API for getting the maximum number of + /// contacts between two entities. + /// \param[in] _id Identity of the world. + /// \return Maximum number of contacts. + public: virtual std::size_t GetWorldCollisionPairMaxContacts( + const Identity &_id) const = 0; + }; + }; + ///////////////////////////////////////////////// class GZ_PHYSICS_VISIBLE Solver : public virtual Feature { diff --git a/include/gz/physics/detail/World.hh b/include/gz/physics/detail/World.hh index 00ee40042..d5270913c 100644 --- a/include/gz/physics/detail/World.hh +++ b/include/gz/physics/detail/World.hh @@ -99,6 +99,24 @@ auto Gravity::World::GetGravity( _inCoordinatesOf); } +///////////////////////////////////////////////// +template +void CollisionPairMaxContacts::World::SetCollisionPairMaxContacts(std::size_t _maxContacts) +{ + this->template Interface() + ->SetWorldCollisionPairMaxContacts(this->identity, _maxContacts); +} + +///////////////////////////////////////////////// +template +std::size_t CollisionPairMaxContacts::World:: + GetCollisionPairMaxContacts() const +{ + return this->template Interface() + ->GetWorldCollisionPairMaxContacts(this->identity); +} + ///////////////////////////////////////////////// template void Solver::World::SetSolver( diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 44eb69456..005693f05 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -48,6 +48,7 @@ #include #include #include +#include #include @@ -213,11 +214,66 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts) EXPECT_TRUE(checkedOutput); auto contacts = world->GetContactsFromLastStep(); - // Only box_colliding should collide with box_base + // Large box collides with other shapes EXPECT_NE(0u, contacts.size()); } } +// The features that an engine must have to be loaded by this loader. +struct FeaturesCollisionPairMaxContacts : gz::physics::FeatureList< + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetContactsFromLastStepFeature, + gz::physics::ForwardStep, + gz::physics::CollisionPairMaxContacts +> {}; + +template +class SimulationFeaturesCollisionPairMaxContactsTest : + public SimulationFeaturesTest{}; +using SimulationFeaturesCollisionPairMaxContactsTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesCollisionPairMaxContactsTest, + SimulationFeaturesCollisionPairMaxContactsTestTypes); + +///////////////////////////////////////////////// +TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest, + CollisionPairMaxContacts) +{ + for (const std::string &name : this->pluginNames) + { + auto world = LoadPluginAndWorld( + this->loader, + name, + gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + auto checkedOutput = StepWorld( + world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + auto contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(std::numeric_limits::max(), + world->GetCollisionPairMaxContacts()); + // Large box collides with other shapes + EXPECT_GT(contacts.size(), 30u); + + world->SetCollisionPairMaxContacts(1u); + EXPECT_EQ(1u, world->GetCollisionPairMaxContacts()); + checkedOutput = StepWorld( + world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(4u, contacts.size()); + + world->SetCollisionPairMaxContacts(0u); + EXPECT_EQ(0u, world->GetCollisionPairMaxContacts()); + checkedOutput = StepWorld( + world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(0u, contacts.size()); + } +} // The features that an engine must have to be loaded by this loader. struct FeaturesStep : gz::physics::FeatureList<