From e01a2520bd4bac81e5cb578970b6b51359f0fbe7 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 16 Dec 2023 00:25:31 +0000 Subject: [PATCH 01/12] add max contacts feature Signed-off-by: Ian Chen --- dartsim/src/EntityManagementFeatures.cc | 3 +- dartsim/src/WorldFeatures.cc | 17 +++++++++++ dartsim/src/WorldFeatures.hh | 9 ++++++ include/gz/physics/World.hh | 38 +++++++++++++++++++++++++ include/gz/physics/detail/World.hh | 18 ++++++++++++ 5 files changed, 84 insertions(+), 1 deletion(-) diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 48ae13e07..be8171870 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -729,7 +729,8 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( // TODO(anyone) We need a machanism to configure maxNumContacts at runtime. auto &collOpt = world->getConstraintSolver()->getCollisionOption(); - collOpt.maxNumContacts = 10000; + //collOpt.maxNumContacts = 10000; + collOpt.maxNumContacts = 20; world->getConstraintSolver()->getCollisionOption().collisionFilter = std::make_shared(); diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc index d05eeefb8..7e6a08542 100644 --- a/dartsim/src/WorldFeatures.cc +++ b/dartsim/src/WorldFeatures.cc @@ -96,6 +96,23 @@ WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( return world->getGravity(); } +///////////////////////////////////////////////// +void WorldFeatures::SetWorldMaxContacts( + const Identity &_id, std::size_t _maxContacts) +{ + auto world = this->ReferenceInterface(_id); + auto &collOpt = world->getConstraintSolver()->getCollisionOption(); + collOpt.maxNumContacts = _maxContacts; +} + +///////////////////////////////////////////////// +std::size_t WorldFeatures::GetWorldMaxContacts(const Identity &_id) + const +{ + auto world = this->ReferenceInterface(_id); + return world->getConstraintSolver()->getCollisionOption().maxNumContacts; +} + ///////////////////////////////////////////////// void WorldFeatures::SetWorldSolver(const Identity &_id, const std::string &_solver) diff --git a/dartsim/src/WorldFeatures.hh b/dartsim/src/WorldFeatures.hh index a7fd9855b..fd23e0477 100644 --- a/dartsim/src/WorldFeatures.hh +++ b/dartsim/src/WorldFeatures.hh @@ -31,6 +31,7 @@ namespace dartsim { struct WorldFeatureList : FeatureList< CollisionDetector, Gravity, + MaxContacts, Solver > { }; @@ -53,6 +54,14 @@ class WorldFeatures : // Documentation inherited public: LinearVectorType GetWorldGravity(const Identity &_id) const override; + // Documentation inherited + public: void SetWorldMaxContacts( + const Identity &_id, std::size_t _maxContacts) override; + + // Documentation inherited + public: std::size_t GetWorldMaxContacts(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..c79afa8cf 100644 --- a/include/gz/physics/World.hh +++ b/include/gz/physics/World.hh @@ -130,6 +130,44 @@ namespace gz }; }; + ///////////////////////////////////////////////// + class GZ_PHYSICS_VISIBLE MaxContacts: public virtual Feature + { + /// \brief The World API for setting the collision detector. + 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 SetMaxContacts(std::size_t _maxContacts); + + /// \brief Set the maximum number of contacts allowed between two + /// entities. + /// \return Maximum number of contacts. + public: std::size_t GetMaxContacts() const; + }; + + /// \private The implementation API for the collision detector. + 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 SetWorldMaxContacts( + 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. + /// \param[in] _maxContacts Maximum number of contacts. + public: virtual std::size_t GetWorldMaxContacts( + 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..1986a51f3 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 MaxContacts::World::SetMaxContacts( + std::size_t _maxContacts) +{ + this->template Interface() + ->SetWorldMaxContacts(this->identity, _maxContacts); +} + +///////////////////////////////////////////////// +template +std::size_t MaxContacts::World:: + GetMaxContacts() const +{ + return this->template Interface() + ->GetWorldMaxContacts(this->identity); +} + ///////////////////////////////////////////////// template void Solver::World::SetSolver( From 72ad1467b6db36928975606a6400b641008a1f3d Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 19 Dec 2023 01:53:15 +0000 Subject: [PATCH 02/12] add support for max contacts in dart's ode collision detector Signed-off-by: Ian Chen --- dartsim/src/Base.hh | 36 +++++--- dartsim/src/CustomFeatures.cc | 2 +- dartsim/src/EntityManagementFeatures.cc | 29 ++++--- dartsim/src/GzOdeCollisionDetector.cc | 104 ++++++++++++++++++++++++ dartsim/src/GzOdeCollisionDetector.hh | 61 ++++++++++++++ dartsim/src/JointFeatures.cc | 2 +- dartsim/src/SDFFeatures.cc | 10 +-- dartsim/src/SimulationFeatures.cc | 8 +- dartsim/src/WorldFeatures.cc | 27 +++--- 9 files changed, 232 insertions(+), 47 deletions(-) create mode 100644 dartsim/src/GzOdeCollisionDetector.cc create mode 100644 dartsim/src/GzOdeCollisionDetector.hh diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index 684d570c1..813783fee 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -52,8 +52,8 @@ namespace gz { namespace physics { namespace dartsim { -/// \brief The structs ModelInfo, LinkInfo, JointInfo, and ShapeInfo are used -/// for two reasons: +/// \brief The structs WorldInfo, ModelInfo, LinkInfo, JointInfo, and ShapeInfo +/// are used for two reasons: /// 1) Holding extra information such as the name or offset /// that will be different from the underlying engine /// 2) Wrap shared pointers to DART entities. Since these shared pointers (eg. @@ -62,6 +62,13 @@ namespace dartsim { /// create a std::shared_ptr of the struct that wraps the corresponding DART /// shared pointer. +struct WorldInfo +{ + /// \brief Pointer to dart simulation world + dart::simulation::WorldPtr world; + /// \brief Maximum number of contacts between a pair of collision objects + int maxContacts = -1; +}; struct LinkInfo { @@ -275,6 +282,7 @@ class Base : public Implements3d> public: using LinkInfoPtr = std::shared_ptr; public: using JointInfoPtr = std::shared_ptr; public: using ShapeInfoPtr = std::shared_ptr; + public: using WorldInfoPtr = std::shared_ptr; public: inline Identity InitiateEngine(std::size_t /*_engineID*/) override { @@ -300,7 +308,9 @@ class Base : public Implements3d> { const std::size_t id = this->GetNextEntity(); _world->setName(_name); - this->worlds.AddEntity(id, _world, _name, 0); + auto worldInfo = std::make_shared(); + worldInfo->world = _world; + this->worlds.AddEntity(id, worldInfo, _name, 0); this->frames[id] = dart::dynamics::Frame::World(); auto model = dart::dynamics::Skeleton::create(""); @@ -308,7 +318,7 @@ class Base : public Implements3d> auto modelInfo = std::make_shared(); modelInfo->model = model; modelInfo->localName = _name; - this->modelProxiesToWorld.AddEntity(id, modelInfo, _world, 0); + this->modelProxiesToWorld.AddEntity(id, modelInfo, worldInfo, 0); return id; } @@ -319,7 +329,7 @@ class Base : public Implements3d> const std::size_t id = this->GetNextEntity(); auto entry = std::make_shared(_info); - const dart::simulation::WorldPtr &world = worlds[_worldID]; + const dart::simulation::WorldPtr &world = worlds[_worldID]->world; world->addSkeleton(entry->model); this->models.AddEntity(id, entry, _info.model, _worldID); if (_info.frame) @@ -339,7 +349,7 @@ class Base : public Implements3d> const std::size_t id = this->GetNextEntity(); auto entry = std::make_shared(_info); - const dart::simulation::WorldPtr &world = worlds[_worldID]; + const dart::simulation::WorldPtr &world = worlds[_worldID]->world; world->addSkeleton(entry->model); this->models.AddEntity(id, entry, _info.model, _parentID); @@ -424,7 +434,7 @@ class Base : public Implements3d> _link->link, pairJointBodyNode.second); _link->weldedNodes.emplace_back(pairJointBodyNode.second, weld); auto worldId = this->GetWorldOfModelImpl(models.objectToID[skeleton]); - auto dartWorld = this->worlds.at(worldId); + auto dartWorld = this->worlds.at(worldId)->world; dartWorld->getConstraintSolver()->addConstraint(weld); // Rebalance the link inertia between the original body node and its @@ -456,7 +466,7 @@ class Base : public Implements3d> { auto worldId = this->GetWorldOfModelImpl( this->models.objectToID[child->getSkeleton()]); - auto dartWorld = this->worlds.at(worldId); + auto dartWorld = this->worlds.at(worldId)->world; dartWorld->getConstraintSolver()->removeConstraint(it->second); // Okay to erase since we break afterward. _link->weldedNodes.erase(it); @@ -521,7 +531,7 @@ class Base : public Implements3d> public: bool RemoveModelImpl(const std::size_t _worldID, const std::size_t _modelID) { - const auto &world = this->worlds.at(_worldID); + const auto &world = this->worlds.at(_worldID)->world; auto modelInfo = this->models.at(_modelID); auto skel = modelInfo->model; // Remove the contents of the skeleton from local entity storage containers @@ -621,7 +631,7 @@ class Base : public Implements3d> { if (this->modelProxiesToWorld.HasEntity(_modelID)) { - auto world = this->worlds.at(_modelID); + auto world = this->worlds.at(_modelID)->world; return ::sdf::JoinName(world->getName(), _name); } else @@ -637,7 +647,7 @@ class Base : public Implements3d> << _name << "]\n"; return ""; } - auto world = this->worlds.at(worldID); + auto world = this->worlds.at(worldID)->world; return ::sdf::JoinName( world->getName(), ::sdf::JoinName(modelInfo->model->getName(), _name)); @@ -654,13 +664,13 @@ class Base : public Implements3d> return this->models.at(_modelID); } - public: EntityStorage worlds; + public: EntityStorage worlds; public: EntityStorage models; public: EntityStorage links; public: EntityStorage joints; public: EntityStorage shapes; public: std::unordered_map frames; - public: EntityStorage modelProxiesToWorld; + public: EntityStorage modelProxiesToWorld; /// \brief Map from the fully qualified link name (including the world name) /// to the BodyNode object. This is useful for keeping track of BodyNodes even diff --git a/dartsim/src/CustomFeatures.cc b/dartsim/src/CustomFeatures.cc index d1b458f2d..8b9949252 100644 --- a/dartsim/src/CustomFeatures.cc +++ b/dartsim/src/CustomFeatures.cc @@ -26,7 +26,7 @@ namespace dartsim { dart::simulation::WorldPtr CustomFeatures::GetDartsimWorld( const Identity &_worldID) { - return this->worlds.at(_worldID); + return this->worlds.at(_worldID)->world; } //! [implementation] diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index be8171870..5ae014d91 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -29,6 +29,8 @@ #include #include +#include "GzOdeCollisionDetector.hh" + namespace gz { namespace physics { namespace dartsim { @@ -105,7 +107,7 @@ class BitmaskContactFilter : public dart::collision::BodyNodeCollisionFilter static std::shared_ptr GetFilterPtr( const EntityManagementFeatures* _emf, std::size_t _worldID) { - const auto world = _emf->worlds.at(_worldID); + const auto world = _emf->worlds.at(_worldID)->world; // We need to cast the base class pointer to the derived class const auto filterPtr = std::static_pointer_cast( world->getConstraintSolver()->getCollisionOption() @@ -172,7 +174,7 @@ Identity EntityManagementFeatures::GetWorld( const std::string &EntityManagementFeatures::GetWorldName( const Identity &_worldID) const { - return this->ReferenceInterface(_worldID)->getName(); + return this->ReferenceInterface(_worldID)->world->getName(); } ///////////////////////////////////////////////// @@ -231,7 +233,7 @@ Identity EntityManagementFeatures::GetModel( const Identity &_worldID, const std::string &_modelName) const { const DartSkeletonPtr &model = - this->ReferenceInterface(_worldID)->getSkeleton(_modelName); + this->ReferenceInterface(_worldID)->world->getSkeleton(_modelName); // If the model doesn't exist in "models", it means the containing entity has // been removed. @@ -328,7 +330,7 @@ Identity EntityManagementFeatures::GetNestedModel( return this->GenerateInvalidId(); } - auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName); + auto nestedSkel = this->worlds.at(worldID)->world->getSkeleton(fullName); if (nullptr == nestedSkel) { return this->GenerateInvalidId(); @@ -709,7 +711,7 @@ bool EntityManagementFeatures::RemoveNestedModelByName(const Identity &_modelID, ::sdf::JoinName(modelInfo->model->getName(), _modelName); auto worldID = this->GetWorldOfModelImpl(_modelID); - auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName); + auto nestedSkel = this->worlds.at(worldID)->world->getSkeleton(fullName); if (nullptr == nestedSkel || !this->models.HasEntity(nestedSkel)) { return false; @@ -724,13 +726,18 @@ 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(); + // auto collisionDetector = dart::collision::OdeCollisionDetector::create(); + world->getConstraintSolver()->setCollisionDetector(collisionDetector); - // TODO(anyone) We need a machanism to configure maxNumContacts at runtime. auto &collOpt = world->getConstraintSolver()->getCollisionOption(); - //collOpt.maxNumContacts = 10000; - collOpt.maxNumContacts = 20; + // Set the max number of contacts for all collision objects + // in the world + collOpt.maxNumContacts = 10000; + + // Set the max number of contacts for a pair of collision objects + std::dynamic_pointer_cast( + collisionDetector)->SetMaxContacts(20); world->getConstraintSolver()->getCollisionOption().collisionFilter = std::make_shared(); @@ -809,7 +816,7 @@ Identity EntityManagementFeatures::ConstructEmptyLink( return this->GenerateInvalidId(); } - auto world = this->worlds.at(worldID); + auto world = this->worlds.at(worldID)->world; const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(model->getName(), bn->getName())); diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc new file mode 100644 index 000000000..ec51312d9 --- /dev/null +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2023 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 "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->LimitMaxContacts(_result); + return ret; +} + +///////////////////////////////////////////////// +bool GzOdeCollisionDetector::collide( + CollisionGroup *_group1, + CollisionGroup *_group2, + const CollisionOption &_option, + CollisionResult *_result) +{ + bool ret = OdeCollisionDetector::collide(_group1, _group2, _option, _result); + this->LimitMaxContacts(_result); + return ret; +} + +///////////////////////////////////////////////// +void GzOdeCollisionDetector::SetMaxContacts(int _maxContacts) +{ + this->maxCollisionPairContacts = _maxContacts; +} + +///////////////////////////////////////////////// +void GzOdeCollisionDetector::LimitMaxContacts( + CollisionResult* _result) +{ + if (this->maxCollisionPairContacts < 0) + return; + + auto allContacts = _result->getContacts(); + _result->clear(); + + std::unordered_map> + contactMap; + + for (auto &contact : allContacts) + { + //auto contactPair = + // std::make_pair(contact.collisionObject1, contact.CollisionObject2); + //auto &count = contactMap[contactPair]; + auto &count = contactMap[contact.collisionObject1][contact.collisionObject2]; + count++; + if (static_cast(count) <= this->maxCollisionPairContacts) + { + _result->addContact(contact); + } + // std::cerr << "1 " << contact.collisionObject1 << std::endl; + // std::cerr << "2 " << contact.collisionObject2 << std::endl; + // std::cerr << "count " << count << std::endl; + } + // std::cerr << "contacts " << _result->getNumContacts() << std::endl; +} diff --git a/dartsim/src/GzOdeCollisionDetector.hh b/dartsim/src/GzOdeCollisionDetector.hh new file mode 100644 index 000000000..ff079da5a --- /dev/null +++ b/dartsim/src/GzOdeCollisionDetector.hh @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2023 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 + +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 + public: void SetMaxContacts(int _maxContacts); + + /// \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 LimitMaxContacts(CollisionResult *_result); + + /// \brief Maximum number of contacts between a pair of collision objects. + private: int maxCollisionPairContacts = -1; + + private: static Registrar mRegistrar; +}; + +} +} diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 14c5fc7a4..97a530785 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -414,7 +414,7 @@ void JointFeatures::DetachJoint(const Identity &_jointId) // world. auto worldId = this->GetWorldOfModelImpl( this->models.IdentityOf(joint->getSkeleton())); - auto dartWorld = this->worlds.at(worldId); + auto dartWorld = this->worlds.at(worldId)->world; std::string modelName = oldName.substr(0, originalNameIndex - 1); skeleton = dartWorld->getSkeleton(modelName); if (skeleton) diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 819aee3f8..4f0c732c7 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -439,7 +439,7 @@ SDFFeatures::FindParentAndChildOfJoint(std::size_t _worldID, // itself. auto *const parent = this->FindBodyNode( - this->worlds.at(_worldID)->getName(), + this->worlds.at(_worldID)->world->getName(), _parentType == "model" ? _parentName : "", parentLinkName); if (nullptr == parent && parentLinkName != "world") @@ -451,7 +451,7 @@ SDFFeatures::FindParentAndChildOfJoint(std::size_t _worldID, } auto *const child = this->FindBodyNode( - this->worlds.at(_worldID)->getName(), + this->worlds.at(_worldID)->world->getName(), _parentType == "model" ? _parentName : "", childLinkName); if (nullptr == child) { @@ -471,7 +471,7 @@ Identity SDFFeatures::ConstructSdfWorld( { const Identity worldID = this->ConstructEmptyWorld(_engine, _sdfWorld.Name()); - const dart::simulation::WorldPtr &world = this->worlds.at(worldID); + const dart::simulation::WorldPtr &world = this->worlds.at(worldID)->world; world->setGravity(math::eigen3::convert(_sdfWorld.Gravity())); @@ -683,7 +683,7 @@ Identity SDFFeatures::ConstructSdfLink( return this->GenerateInvalidId(); } - auto world = this->worlds.at(worldID); + auto world = this->worlds.at(worldID)->world; const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(modelInfo.model->getName(), bn->getName())); @@ -777,7 +777,7 @@ Identity SDFFeatures::ConstructSdfJoint( // node is found, an error will not be printed. // const std::size_t worldID = this->GetWorldOfModelImpl(_modelID); - auto & world = this->worlds.at(worldID); + auto & world = this->worlds.at(worldID)->world; std::string parentLinkName; const auto resolveParentErrors = _sdfJoint.ResolveParentLink(parentLinkName); diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index a622c8f7c..36230f958 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -58,7 +58,7 @@ void SimulationFeatures::WorldForwardStep( const ForwardStep::Input & _u) { GZ_PROFILE("SimulationFeatures::WorldForwardStep"); - auto *world = this->ReferenceInterface(_worldID); + auto world = this->ReferenceInterface(_worldID)->world; auto *dtDur = _u.Query(); const double tol = 1e-6; @@ -155,7 +155,7 @@ std::vector SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { std::vector outContacts; - auto *const world = this->ReferenceInterface(_worldID); + auto const world = this->ReferenceInterface(_worldID)->world; const auto colResult = world->getLastCollisionResult(); for (const auto &dtContact : colResult.getContacts()) @@ -211,7 +211,7 @@ void SimulationFeatures::AddContactPropertiesCallback( const Identity& _worldID, const std::string& _callbackID, SurfaceParamsCallback _callback) { - auto *world = this->ReferenceInterface(_worldID); + auto *world = this->ReferenceInterface(_worldID)->world; auto handler = std::make_shared(); handler->surfaceParamsCallback = _callback; @@ -226,7 +226,7 @@ void SimulationFeatures::AddContactPropertiesCallback( bool SimulationFeatures::RemoveContactPropertiesCallback( const Identity& _worldID, const std::string& _callbackID) { - auto *world = this->ReferenceInterface(_worldID); + auto *world = this->ReferenceInterface(_worldID)->world; if (this->contactSurfaceHandlers.find(_callbackID) != this->contactSurfaceHandlers.end()) diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc index 7e6a08542..8acfb2205 100644 --- a/dartsim/src/WorldFeatures.cc +++ b/dartsim/src/WorldFeatures.cc @@ -30,8 +30,10 @@ #include +#include "GzOdeCollisionDetector.hh" #include "WorldFeatures.hh" + namespace gz { namespace physics { namespace dartsim { @@ -40,7 +42,7 @@ namespace dartsim { void WorldFeatures::SetWorldCollisionDetector( const Identity &_id, const std::string &_collisionDetector) { - auto world = this->ReferenceInterface(_id); + auto world = this->ReferenceInterface(_id)->world; auto collisionDetector = world->getConstraintSolver()->getCollisionDetector(); if (_collisionDetector == "bullet") @@ -53,7 +55,9 @@ void WorldFeatures::SetWorldCollisionDetector( } else if (_collisionDetector == "ode") { - collisionDetector = dart::collision::OdeCollisionDetector::create(); + collisionDetector = dart::collision::GzOdeCollisionDetector::create(); + std::dynamic_pointer_cast( + collisionDetector)->SetMaxContacts(20); } else if (_collisionDetector == "dart") { @@ -76,7 +80,7 @@ void WorldFeatures::SetWorldCollisionDetector( const std::string &WorldFeatures::GetWorldCollisionDetector(const Identity &_id) const { - auto world = this->ReferenceInterface(_id); + auto world = this->ReferenceInterface(_id)->world; return world->getConstraintSolver()->getCollisionDetector()->getType(); } @@ -84,7 +88,7 @@ const std::string &WorldFeatures::GetWorldCollisionDetector(const Identity &_id) void WorldFeatures::SetWorldGravity( const Identity &_id, const LinearVectorType &_gravity) { - auto world = this->ReferenceInterface(_id); + auto world = this->ReferenceInterface(_id)->world; world->setGravity(_gravity); } @@ -92,7 +96,7 @@ void WorldFeatures::SetWorldGravity( WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( const Identity &_id) const { - auto world = this->ReferenceInterface(_id); + auto world = this->ReferenceInterface(_id)->world; return world->getGravity(); } @@ -100,24 +104,23 @@ WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( void WorldFeatures::SetWorldMaxContacts( const Identity &_id, std::size_t _maxContacts) { - auto world = this->ReferenceInterface(_id); - auto &collOpt = world->getConstraintSolver()->getCollisionOption(); - collOpt.maxNumContacts = _maxContacts; + auto world = this->ReferenceInterface(_id); + world->maxContacts = static_cast(_maxContacts); } ///////////////////////////////////////////////// std::size_t WorldFeatures::GetWorldMaxContacts(const Identity &_id) const { - auto world = this->ReferenceInterface(_id); - return world->getConstraintSolver()->getCollisionOption().maxNumContacts; + auto world = this->ReferenceInterface(_id); + return static_cast(world->maxContacts); } ///////////////////////////////////////////////// void WorldFeatures::SetWorldSolver(const Identity &_id, const std::string &_solver) { - auto world = this->ReferenceInterface(_id); + auto world = this->ReferenceInterface(_id)->world; auto solver = dynamic_cast( @@ -157,7 +160,7 @@ void WorldFeatures::SetWorldSolver(const Identity &_id, ///////////////////////////////////////////////// const std::string &WorldFeatures::GetWorldSolver(const Identity &_id) const { - auto world = this->ReferenceInterface(_id); + auto world = this->ReferenceInterface(_id)->world; auto solver = dynamic_cast( From e0d0cb1c5db5ea80faff3299f95d99152ebeb8be Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 19 Dec 2023 23:01:05 +0000 Subject: [PATCH 03/12] add test Signed-off-by: Ian Chen --- dartsim/src/EntityManagementFeatures.cc | 2 +- dartsim/src/GzOdeCollisionDetector.cc | 1 - dartsim/src/WorldFeatures.cc | 21 +++++++--- test/common_test/simulation_features.cc | 53 ++++++++++++++++++++++++- 4 files changed, 69 insertions(+), 8 deletions(-) diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 5ae014d91..a43bd7468 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -737,7 +737,7 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( // Set the max number of contacts for a pair of collision objects std::dynamic_pointer_cast( - collisionDetector)->SetMaxContacts(20); + collisionDetector)->SetMaxContacts(20u); world->getConstraintSolver()->getCollisionOption().collisionFilter = std::make_shared(); diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc index ec51312d9..152d11980 100644 --- a/dartsim/src/GzOdeCollisionDetector.cc +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -100,5 +100,4 @@ void GzOdeCollisionDetector::LimitMaxContacts( // std::cerr << "2 " << contact.collisionObject2 << std::endl; // std::cerr << "count " << count << std::endl; } - // std::cerr << "contacts " << _result->getNumContacts() << std::endl; } diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc index 8acfb2205..ebcbbfff7 100644 --- a/dartsim/src/WorldFeatures.cc +++ b/dartsim/src/WorldFeatures.cc @@ -42,9 +42,9 @@ namespace dartsim { void WorldFeatures::SetWorldCollisionDetector( const Identity &_id, const std::string &_collisionDetector) { - auto world = this->ReferenceInterface(_id)->world; + auto world = this->ReferenceInterface(_id); auto collisionDetector = - world->getConstraintSolver()->getCollisionDetector(); + world->world->getConstraintSolver()->getCollisionDetector(); if (_collisionDetector == "bullet") { collisionDetector = dart::collision::BulletCollisionDetector::create(); @@ -56,8 +56,9 @@ void WorldFeatures::SetWorldCollisionDetector( else if (_collisionDetector == "ode") { collisionDetector = dart::collision::GzOdeCollisionDetector::create(); + // collisionDetector = dart::collision::OdeCollisionDetector::create(); std::dynamic_pointer_cast( - collisionDetector)->SetMaxContacts(20); + collisionDetector)->SetMaxContacts(world->maxContacts); } else if (_collisionDetector == "dart") { @@ -70,9 +71,9 @@ void WorldFeatures::SetWorldCollisionDetector( << collisionDetector->getType() << "]." << std::endl; } - world->getConstraintSolver()->setCollisionDetector(collisionDetector); + world->world->getConstraintSolver()->setCollisionDetector(collisionDetector); - gzmsg << "Using [" << world->getConstraintSolver()->getCollisionDetector() + gzmsg << "Using [" << world->world->getConstraintSolver()->getCollisionDetector() ->getType() << "] collision detector" << std::endl; } @@ -106,6 +107,16 @@ void WorldFeatures::SetWorldMaxContacts( { auto world = this->ReferenceInterface(_id); world->maxContacts = static_cast(_maxContacts); + auto collisionDetector = + world->world->getConstraintSolver()->getCollisionDetector(); + + auto odeCollisionDetector = + std::dynamic_pointer_cast( + collisionDetector); + if (odeCollisionDetector) + { + odeCollisionDetector->SetMaxContacts(world->maxContacts); + } } ///////////////////////////////////////////////// diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 44eb69456..f71609da4 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,61 @@ 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 FeaturesMaxContacts : gz::physics::FeatureList< + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetContactsFromLastStepFeature, + gz::physics::ForwardStep, + gz::physics::MaxContacts +> {}; + +template +class SimulationFeaturesMaxContactsTest : + public SimulationFeaturesTest{}; +using SimulationFeaturesMaxContactsTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesMaxContactsTest, + SimulationFeaturesMaxContactsTestTypes); + +///////////////////////////////////////////////// +TYPED_TEST(SimulationFeaturesMaxContactsTest, MaxContacts) +{ + 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(20u, world->GetMaxContacts()); + // Large box collides with other shapes + EXPECT_GT(contacts.size(), 30u); + + world->SetMaxContacts(1u); + EXPECT_EQ(1u, world->GetMaxContacts()); + checkedOutput = StepWorld(world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(4u, contacts.size()); + + world->SetMaxContacts(0u); + EXPECT_EQ(0u, world->GetMaxContacts()); + 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< From fa9830f751c62265a5a9238f6cf964fb57e944ff Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 19 Dec 2023 23:49:20 +0000 Subject: [PATCH 04/12] revert world changes, add tests Signed-off-by: Ian Chen --- dartsim/src/Base.hh | 36 +++++++------------ dartsim/src/CustomFeatures.cc | 2 +- dartsim/src/EntityManagementFeatures.cc | 18 ++++------ dartsim/src/GzOdeCollisionDetector.cc | 20 +++++++---- dartsim/src/GzOdeCollisionDetector.hh | 14 ++++++-- dartsim/src/JointFeatures.cc | 2 +- dartsim/src/SDFFeatures.cc | 10 +++--- dartsim/src/SimulationFeatures.cc | 8 ++--- dartsim/src/WorldFeatures.cc | 47 +++++++++++++++---------- test/common_test/simulation_features.cc | 2 +- 10 files changed, 85 insertions(+), 74 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index 813783fee..684d570c1 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -52,8 +52,8 @@ namespace gz { namespace physics { namespace dartsim { -/// \brief The structs WorldInfo, ModelInfo, LinkInfo, JointInfo, and ShapeInfo -/// are used for two reasons: +/// \brief The structs ModelInfo, LinkInfo, JointInfo, and ShapeInfo are used +/// for two reasons: /// 1) Holding extra information such as the name or offset /// that will be different from the underlying engine /// 2) Wrap shared pointers to DART entities. Since these shared pointers (eg. @@ -62,13 +62,6 @@ namespace dartsim { /// create a std::shared_ptr of the struct that wraps the corresponding DART /// shared pointer. -struct WorldInfo -{ - /// \brief Pointer to dart simulation world - dart::simulation::WorldPtr world; - /// \brief Maximum number of contacts between a pair of collision objects - int maxContacts = -1; -}; struct LinkInfo { @@ -282,7 +275,6 @@ class Base : public Implements3d> public: using LinkInfoPtr = std::shared_ptr; public: using JointInfoPtr = std::shared_ptr; public: using ShapeInfoPtr = std::shared_ptr; - public: using WorldInfoPtr = std::shared_ptr; public: inline Identity InitiateEngine(std::size_t /*_engineID*/) override { @@ -308,9 +300,7 @@ class Base : public Implements3d> { const std::size_t id = this->GetNextEntity(); _world->setName(_name); - auto worldInfo = std::make_shared(); - worldInfo->world = _world; - this->worlds.AddEntity(id, worldInfo, _name, 0); + this->worlds.AddEntity(id, _world, _name, 0); this->frames[id] = dart::dynamics::Frame::World(); auto model = dart::dynamics::Skeleton::create(""); @@ -318,7 +308,7 @@ class Base : public Implements3d> auto modelInfo = std::make_shared(); modelInfo->model = model; modelInfo->localName = _name; - this->modelProxiesToWorld.AddEntity(id, modelInfo, worldInfo, 0); + this->modelProxiesToWorld.AddEntity(id, modelInfo, _world, 0); return id; } @@ -329,7 +319,7 @@ class Base : public Implements3d> const std::size_t id = this->GetNextEntity(); auto entry = std::make_shared(_info); - const dart::simulation::WorldPtr &world = worlds[_worldID]->world; + const dart::simulation::WorldPtr &world = worlds[_worldID]; world->addSkeleton(entry->model); this->models.AddEntity(id, entry, _info.model, _worldID); if (_info.frame) @@ -349,7 +339,7 @@ class Base : public Implements3d> const std::size_t id = this->GetNextEntity(); auto entry = std::make_shared(_info); - const dart::simulation::WorldPtr &world = worlds[_worldID]->world; + const dart::simulation::WorldPtr &world = worlds[_worldID]; world->addSkeleton(entry->model); this->models.AddEntity(id, entry, _info.model, _parentID); @@ -434,7 +424,7 @@ class Base : public Implements3d> _link->link, pairJointBodyNode.second); _link->weldedNodes.emplace_back(pairJointBodyNode.second, weld); auto worldId = this->GetWorldOfModelImpl(models.objectToID[skeleton]); - auto dartWorld = this->worlds.at(worldId)->world; + auto dartWorld = this->worlds.at(worldId); dartWorld->getConstraintSolver()->addConstraint(weld); // Rebalance the link inertia between the original body node and its @@ -466,7 +456,7 @@ class Base : public Implements3d> { auto worldId = this->GetWorldOfModelImpl( this->models.objectToID[child->getSkeleton()]); - auto dartWorld = this->worlds.at(worldId)->world; + auto dartWorld = this->worlds.at(worldId); dartWorld->getConstraintSolver()->removeConstraint(it->second); // Okay to erase since we break afterward. _link->weldedNodes.erase(it); @@ -531,7 +521,7 @@ class Base : public Implements3d> public: bool RemoveModelImpl(const std::size_t _worldID, const std::size_t _modelID) { - const auto &world = this->worlds.at(_worldID)->world; + const auto &world = this->worlds.at(_worldID); auto modelInfo = this->models.at(_modelID); auto skel = modelInfo->model; // Remove the contents of the skeleton from local entity storage containers @@ -631,7 +621,7 @@ class Base : public Implements3d> { if (this->modelProxiesToWorld.HasEntity(_modelID)) { - auto world = this->worlds.at(_modelID)->world; + auto world = this->worlds.at(_modelID); return ::sdf::JoinName(world->getName(), _name); } else @@ -647,7 +637,7 @@ class Base : public Implements3d> << _name << "]\n"; return ""; } - auto world = this->worlds.at(worldID)->world; + auto world = this->worlds.at(worldID); return ::sdf::JoinName( world->getName(), ::sdf::JoinName(modelInfo->model->getName(), _name)); @@ -664,13 +654,13 @@ class Base : public Implements3d> return this->models.at(_modelID); } - public: EntityStorage worlds; + public: EntityStorage worlds; public: EntityStorage models; public: EntityStorage links; public: EntityStorage joints; public: EntityStorage shapes; public: std::unordered_map frames; - public: EntityStorage modelProxiesToWorld; + public: EntityStorage modelProxiesToWorld; /// \brief Map from the fully qualified link name (including the world name) /// to the BodyNode object. This is useful for keeping track of BodyNodes even diff --git a/dartsim/src/CustomFeatures.cc b/dartsim/src/CustomFeatures.cc index 8b9949252..d1b458f2d 100644 --- a/dartsim/src/CustomFeatures.cc +++ b/dartsim/src/CustomFeatures.cc @@ -26,7 +26,7 @@ namespace dartsim { dart::simulation::WorldPtr CustomFeatures::GetDartsimWorld( const Identity &_worldID) { - return this->worlds.at(_worldID)->world; + return this->worlds.at(_worldID); } //! [implementation] diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index a43bd7468..990ca5631 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -107,7 +106,7 @@ class BitmaskContactFilter : public dart::collision::BodyNodeCollisionFilter static std::shared_ptr GetFilterPtr( const EntityManagementFeatures* _emf, std::size_t _worldID) { - const auto world = _emf->worlds.at(_worldID)->world; + const auto world = _emf->worlds.at(_worldID); // We need to cast the base class pointer to the derived class const auto filterPtr = std::static_pointer_cast( world->getConstraintSolver()->getCollisionOption() @@ -174,7 +173,7 @@ Identity EntityManagementFeatures::GetWorld( const std::string &EntityManagementFeatures::GetWorldName( const Identity &_worldID) const { - return this->ReferenceInterface(_worldID)->world->getName(); + return this->ReferenceInterface(_worldID)->getName(); } ///////////////////////////////////////////////// @@ -233,7 +232,7 @@ Identity EntityManagementFeatures::GetModel( const Identity &_worldID, const std::string &_modelName) const { const DartSkeletonPtr &model = - this->ReferenceInterface(_worldID)->world->getSkeleton(_modelName); + this->ReferenceInterface(_worldID)->getSkeleton(_modelName); // If the model doesn't exist in "models", it means the containing entity has // been removed. @@ -330,7 +329,7 @@ Identity EntityManagementFeatures::GetNestedModel( return this->GenerateInvalidId(); } - auto nestedSkel = this->worlds.at(worldID)->world->getSkeleton(fullName); + auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName); if (nullptr == nestedSkel) { return this->GenerateInvalidId(); @@ -711,7 +710,7 @@ bool EntityManagementFeatures::RemoveNestedModelByName(const Identity &_modelID, ::sdf::JoinName(modelInfo->model->getName(), _modelName); auto worldID = this->GetWorldOfModelImpl(_modelID); - auto nestedSkel = this->worlds.at(worldID)->world->getSkeleton(fullName); + auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName); if (nullptr == nestedSkel || !this->models.HasEntity(nestedSkel)) { return false; @@ -727,7 +726,6 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( { const auto &world = std::make_shared(_name); auto collisionDetector = dart::collision::GzOdeCollisionDetector::create(); - // auto collisionDetector = dart::collision::OdeCollisionDetector::create(); world->getConstraintSolver()->setCollisionDetector(collisionDetector); auto &collOpt = world->getConstraintSolver()->getCollisionOption(); @@ -735,10 +733,6 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( // in the world collOpt.maxNumContacts = 10000; - // Set the max number of contacts for a pair of collision objects - std::dynamic_pointer_cast( - collisionDetector)->SetMaxContacts(20u); - world->getConstraintSolver()->getCollisionOption().collisionFilter = std::make_shared(); @@ -816,7 +810,7 @@ Identity EntityManagementFeatures::ConstructEmptyLink( return this->GenerateInvalidId(); } - auto world = this->worlds.at(worldID)->world; + auto world = this->worlds.at(worldID); const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(model->getName(), bn->getName())); diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc index 152d11980..4afb1f474 100644 --- a/dartsim/src/GzOdeCollisionDetector.cc +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -66,16 +66,23 @@ bool GzOdeCollisionDetector::collide( } ///////////////////////////////////////////////// -void GzOdeCollisionDetector::SetMaxContacts(int _maxContacts) +void GzOdeCollisionDetector::SetMaxContacts(std::size_t _maxContacts) { this->maxCollisionPairContacts = _maxContacts; } +///////////////////////////////////////////////// +std::size_t GzOdeCollisionDetector::GetMaxContacts() const +{ + return this->maxCollisionPairContacts; +} + ///////////////////////////////////////////////// void GzOdeCollisionDetector::LimitMaxContacts( - CollisionResult* _result) + CollisionResult *_result) { - if (this->maxCollisionPairContacts < 0) + if (this->maxCollisionPairContacts == + std::numeric_limits::max()) return; auto allContacts = _result->getContacts(); @@ -92,12 +99,11 @@ void GzOdeCollisionDetector::LimitMaxContacts( //auto &count = contactMap[contactPair]; auto &count = contactMap[contact.collisionObject1][contact.collisionObject2]; count++; - if (static_cast(count) <= this->maxCollisionPairContacts) + auto &otherCount = contactMap[contact.collisionObject2][contact.collisionObject1]; + std::size_t total = count + otherCount; + if (total <= this->maxCollisionPairContacts) { _result->addContact(contact); } - // std::cerr << "1 " << contact.collisionObject1 << std::endl; - // std::cerr << "2 " << contact.collisionObject2 << std::endl; - // std::cerr << "count " << count << std::endl; } } diff --git a/dartsim/src/GzOdeCollisionDetector.hh b/dartsim/src/GzOdeCollisionDetector.hh index ff079da5a..fb4db5e35 100644 --- a/dartsim/src/GzOdeCollisionDetector.hh +++ b/dartsim/src/GzOdeCollisionDetector.hh @@ -15,6 +15,7 @@ * */ +#include #include namespace dart { @@ -37,7 +38,15 @@ class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector /// \brief Set the maximum number of contacts between a pair of collision /// objects - public: void SetMaxContacts(int _maxContacts); + /// \param[in] _maxContacts Maximum number of contacts between a pair of + /// collision objects. + public: void SetMaxContacts(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 GetMaxContacts() const; + /// \brief Create the GzOdeCollisionDetector public: static std::shared_ptr create(); @@ -52,7 +61,8 @@ class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector private: void LimitMaxContacts(CollisionResult *_result); /// \brief Maximum number of contacts between a pair of collision objects. - private: int maxCollisionPairContacts = -1; + private: std::size_t maxCollisionPairContacts = + std::numeric_limits::max(); private: static Registrar mRegistrar; }; diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 97a530785..14c5fc7a4 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -414,7 +414,7 @@ void JointFeatures::DetachJoint(const Identity &_jointId) // world. auto worldId = this->GetWorldOfModelImpl( this->models.IdentityOf(joint->getSkeleton())); - auto dartWorld = this->worlds.at(worldId)->world; + auto dartWorld = this->worlds.at(worldId); std::string modelName = oldName.substr(0, originalNameIndex - 1); skeleton = dartWorld->getSkeleton(modelName); if (skeleton) diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 4f0c732c7..819aee3f8 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -439,7 +439,7 @@ SDFFeatures::FindParentAndChildOfJoint(std::size_t _worldID, // itself. auto *const parent = this->FindBodyNode( - this->worlds.at(_worldID)->world->getName(), + this->worlds.at(_worldID)->getName(), _parentType == "model" ? _parentName : "", parentLinkName); if (nullptr == parent && parentLinkName != "world") @@ -451,7 +451,7 @@ SDFFeatures::FindParentAndChildOfJoint(std::size_t _worldID, } auto *const child = this->FindBodyNode( - this->worlds.at(_worldID)->world->getName(), + this->worlds.at(_worldID)->getName(), _parentType == "model" ? _parentName : "", childLinkName); if (nullptr == child) { @@ -471,7 +471,7 @@ Identity SDFFeatures::ConstructSdfWorld( { const Identity worldID = this->ConstructEmptyWorld(_engine, _sdfWorld.Name()); - const dart::simulation::WorldPtr &world = this->worlds.at(worldID)->world; + const dart::simulation::WorldPtr &world = this->worlds.at(worldID); world->setGravity(math::eigen3::convert(_sdfWorld.Gravity())); @@ -683,7 +683,7 @@ Identity SDFFeatures::ConstructSdfLink( return this->GenerateInvalidId(); } - auto world = this->worlds.at(worldID)->world; + auto world = this->worlds.at(worldID); const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(modelInfo.model->getName(), bn->getName())); @@ -777,7 +777,7 @@ Identity SDFFeatures::ConstructSdfJoint( // node is found, an error will not be printed. // const std::size_t worldID = this->GetWorldOfModelImpl(_modelID); - auto & world = this->worlds.at(worldID)->world; + auto & world = this->worlds.at(worldID); std::string parentLinkName; const auto resolveParentErrors = _sdfJoint.ResolveParentLink(parentLinkName); diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 36230f958..a622c8f7c 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -58,7 +58,7 @@ void SimulationFeatures::WorldForwardStep( const ForwardStep::Input & _u) { GZ_PROFILE("SimulationFeatures::WorldForwardStep"); - auto world = this->ReferenceInterface(_worldID)->world; + auto *world = this->ReferenceInterface(_worldID); auto *dtDur = _u.Query(); const double tol = 1e-6; @@ -155,7 +155,7 @@ std::vector SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { std::vector outContacts; - auto const world = this->ReferenceInterface(_worldID)->world; + auto *const world = this->ReferenceInterface(_worldID); const auto colResult = world->getLastCollisionResult(); for (const auto &dtContact : colResult.getContacts()) @@ -211,7 +211,7 @@ void SimulationFeatures::AddContactPropertiesCallback( const Identity& _worldID, const std::string& _callbackID, SurfaceParamsCallback _callback) { - auto *world = this->ReferenceInterface(_worldID)->world; + auto *world = this->ReferenceInterface(_worldID); auto handler = std::make_shared(); handler->surfaceParamsCallback = _callback; @@ -226,7 +226,7 @@ void SimulationFeatures::AddContactPropertiesCallback( bool SimulationFeatures::RemoveContactPropertiesCallback( const Identity& _worldID, const std::string& _callbackID) { - auto *world = this->ReferenceInterface(_worldID)->world; + auto *world = this->ReferenceInterface(_worldID); if (this->contactSurfaceHandlers.find(_callbackID) != this->contactSurfaceHandlers.end()) diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc index ebcbbfff7..17573c9f2 100644 --- a/dartsim/src/WorldFeatures.cc +++ b/dartsim/src/WorldFeatures.cc @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -42,9 +41,9 @@ namespace dartsim { void WorldFeatures::SetWorldCollisionDetector( const Identity &_id, const std::string &_collisionDetector) { - auto world = this->ReferenceInterface(_id); + auto world = this->ReferenceInterface(_id); auto collisionDetector = - world->world->getConstraintSolver()->getCollisionDetector(); + world->getConstraintSolver()->getCollisionDetector(); if (_collisionDetector == "bullet") { collisionDetector = dart::collision::BulletCollisionDetector::create(); @@ -57,8 +56,6 @@ void WorldFeatures::SetWorldCollisionDetector( { collisionDetector = dart::collision::GzOdeCollisionDetector::create(); // collisionDetector = dart::collision::OdeCollisionDetector::create(); - std::dynamic_pointer_cast( - collisionDetector)->SetMaxContacts(world->maxContacts); } else if (_collisionDetector == "dart") { @@ -71,9 +68,9 @@ void WorldFeatures::SetWorldCollisionDetector( << collisionDetector->getType() << "]." << std::endl; } - world->world->getConstraintSolver()->setCollisionDetector(collisionDetector); + world->getConstraintSolver()->setCollisionDetector(collisionDetector); - gzmsg << "Using [" << world->world->getConstraintSolver()->getCollisionDetector() + gzmsg << "Using [" << world->getConstraintSolver()->getCollisionDetector() ->getType() << "] collision detector" << std::endl; } @@ -81,7 +78,7 @@ void WorldFeatures::SetWorldCollisionDetector( const std::string &WorldFeatures::GetWorldCollisionDetector(const Identity &_id) const { - auto world = this->ReferenceInterface(_id)->world; + auto world = this->ReferenceInterface(_id); return world->getConstraintSolver()->getCollisionDetector()->getType(); } @@ -89,7 +86,7 @@ const std::string &WorldFeatures::GetWorldCollisionDetector(const Identity &_id) void WorldFeatures::SetWorldGravity( const Identity &_id, const LinearVectorType &_gravity) { - auto world = this->ReferenceInterface(_id)->world; + auto world = this->ReferenceInterface(_id); world->setGravity(_gravity); } @@ -97,7 +94,7 @@ void WorldFeatures::SetWorldGravity( WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( const Identity &_id) const { - auto world = this->ReferenceInterface(_id)->world; + auto world = this->ReferenceInterface(_id); return world->getGravity(); } @@ -105,17 +102,21 @@ WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( void WorldFeatures::SetWorldMaxContacts( const Identity &_id, std::size_t _maxContacts) { - auto world = this->ReferenceInterface(_id); - world->maxContacts = static_cast(_maxContacts); + auto world = this->ReferenceInterface(_id); auto collisionDetector = - world->world->getConstraintSolver()->getCollisionDetector(); + world->getConstraintSolver()->getCollisionDetector(); auto odeCollisionDetector = std::dynamic_pointer_cast( collisionDetector); if (odeCollisionDetector) { - odeCollisionDetector->SetMaxContacts(world->maxContacts); + odeCollisionDetector->SetMaxContacts(_maxContacts); + } + else + { + gzwarn << "Currently max contacts feature is only supported by the " + << "ode collision detector in dartsim." << std::endl; } } @@ -123,15 +124,25 @@ void WorldFeatures::SetWorldMaxContacts( std::size_t WorldFeatures::GetWorldMaxContacts(const Identity &_id) const { - auto world = this->ReferenceInterface(_id); - return static_cast(world->maxContacts); + auto world = this->ReferenceInterface(_id); + auto collisionDetector = + world->getConstraintSolver()->getCollisionDetector(); + auto odeCollisionDetector = + std::dynamic_pointer_cast( + collisionDetector); + if (odeCollisionDetector) + { + return odeCollisionDetector->GetMaxContacts(); + } + + return 0u; } ///////////////////////////////////////////////// void WorldFeatures::SetWorldSolver(const Identity &_id, const std::string &_solver) { - auto world = this->ReferenceInterface(_id)->world; + auto world = this->ReferenceInterface(_id); auto solver = dynamic_cast( @@ -171,7 +182,7 @@ void WorldFeatures::SetWorldSolver(const Identity &_id, ///////////////////////////////////////////////// const std::string &WorldFeatures::GetWorldSolver(const Identity &_id) const { - auto world = this->ReferenceInterface(_id)->world; + auto world = this->ReferenceInterface(_id); auto solver = dynamic_cast( diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index f71609da4..a99b934fe 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -248,7 +248,7 @@ TYPED_TEST(SimulationFeaturesMaxContactsTest, MaxContacts) EXPECT_TRUE(checkedOutput); auto contacts = world->GetContactsFromLastStep(); - EXPECT_EQ(20u, world->GetMaxContacts()); + EXPECT_EQ(std::numeric_limits::max(), world->GetMaxContacts()); // Large box collides with other shapes EXPECT_GT(contacts.size(), 30u); From 9cc40ae53899f32eebc98adff51caa0a833ba831 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 19 Dec 2023 23:51:46 +0000 Subject: [PATCH 05/12] cleanup Signed-off-by: Ian Chen --- dartsim/src/GzOdeCollisionDetector.cc | 3 --- dartsim/src/WorldFeatures.cc | 1 - 2 files changed, 4 deletions(-) diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc index 4afb1f474..43caadf0c 100644 --- a/dartsim/src/GzOdeCollisionDetector.cc +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -94,9 +94,6 @@ void GzOdeCollisionDetector::LimitMaxContacts( for (auto &contact : allContacts) { - //auto contactPair = - // std::make_pair(contact.collisionObject1, contact.CollisionObject2); - //auto &count = contactMap[contactPair]; auto &count = contactMap[contact.collisionObject1][contact.collisionObject2]; count++; auto &otherCount = contactMap[contact.collisionObject2][contact.collisionObject1]; diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc index 17573c9f2..d8e0c4880 100644 --- a/dartsim/src/WorldFeatures.cc +++ b/dartsim/src/WorldFeatures.cc @@ -55,7 +55,6 @@ void WorldFeatures::SetWorldCollisionDetector( else if (_collisionDetector == "ode") { collisionDetector = dart::collision::GzOdeCollisionDetector::create(); - // collisionDetector = dart::collision::OdeCollisionDetector::create(); } else if (_collisionDetector == "dart") { From 1016ec02edcbd03d6740c1db57996dc3a2dceb8c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 20 Dec 2023 00:07:35 +0000 Subject: [PATCH 06/12] style, add includes Signed-off-by: Ian Chen --- dartsim/src/GzOdeCollisionDetector.cc | 9 +++++++-- dartsim/src/GzOdeCollisionDetector.hh | 2 ++ 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc index 43caadf0c..5e6200409 100644 --- a/dartsim/src/GzOdeCollisionDetector.cc +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -15,6 +15,9 @@ * */ +#include +#include + #include #include "GzOdeCollisionDetector.hh" @@ -94,9 +97,11 @@ void GzOdeCollisionDetector::LimitMaxContacts( for (auto &contact : allContacts) { - auto &count = contactMap[contact.collisionObject1][contact.collisionObject2]; + auto &count = + contactMap[contact.collisionObject1][contact.collisionObject2]; count++; - auto &otherCount = contactMap[contact.collisionObject2][contact.collisionObject1]; + auto &otherCount = + contactMap[contact.collisionObject2][contact.collisionObject1]; std::size_t total = count + otherCount; if (total <= this->maxCollisionPairContacts) { diff --git a/dartsim/src/GzOdeCollisionDetector.hh b/dartsim/src/GzOdeCollisionDetector.hh index fb4db5e35..bdb34e0d2 100644 --- a/dartsim/src/GzOdeCollisionDetector.hh +++ b/dartsim/src/GzOdeCollisionDetector.hh @@ -16,6 +16,8 @@ */ #include +#include + #include namespace dart { From c428a3c62f5cf5964469bfa29183aa5e39b036f2 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 20 Dec 2023 18:49:42 +0000 Subject: [PATCH 07/12] fix comments Signed-off-by: Ian Chen --- include/gz/physics/World.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/gz/physics/World.hh b/include/gz/physics/World.hh index c79afa8cf..6f4cf2aa6 100644 --- a/include/gz/physics/World.hh +++ b/include/gz/physics/World.hh @@ -133,7 +133,7 @@ namespace gz ///////////////////////////////////////////////// class GZ_PHYSICS_VISIBLE MaxContacts: public virtual Feature { - /// \brief The World API for setting the collision detector. + /// \brief The World API for getting and setting max contacts. public: template class World : public virtual Feature::World { @@ -148,7 +148,7 @@ namespace gz public: std::size_t GetMaxContacts() const; }; - /// \private The implementation API for the collision detector. + /// \private The implementation API for getting and setting max contacts. public: template class Implementation : public virtual Feature::Implementation { From 98b5e6108accc7b160500c83274666b2e16f007a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 8 Jan 2024 21:32:54 +0000 Subject: [PATCH 08/12] MaxContacts -> CollisionPairMaxTotalContacts Signed-off-by: Ian Chen --- dartsim/src/GzOdeCollisionDetector.cc | 10 +++---- dartsim/src/GzOdeCollisionDetector.hh | 6 ++-- dartsim/src/WorldFeatures.cc | 8 ++--- dartsim/src/WorldFeatures.hh | 6 ++-- include/gz/physics/World.hh | 11 +++---- include/gz/physics/detail/World.hh | 16 +++++----- test/common_test/simulation_features.cc | 39 ++++++++++++++----------- 7 files changed, 51 insertions(+), 45 deletions(-) diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc index 5e6200409..70a1bbd97 100644 --- a/dartsim/src/GzOdeCollisionDetector.cc +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -52,7 +52,7 @@ bool GzOdeCollisionDetector::collide( CollisionResult *_result) { bool ret = OdeCollisionDetector::collide(_group, _option, _result); - this->LimitMaxContacts(_result); + this->LimitCollisionPairMaxTotalContacts(_result); return ret; } @@ -64,24 +64,24 @@ bool GzOdeCollisionDetector::collide( CollisionResult *_result) { bool ret = OdeCollisionDetector::collide(_group1, _group2, _option, _result); - this->LimitMaxContacts(_result); + this->LimitCollisionPairMaxTotalContacts(_result); return ret; } ///////////////////////////////////////////////// -void GzOdeCollisionDetector::SetMaxContacts(std::size_t _maxContacts) +void GzOdeCollisionDetector::SetCollisionPairMaxTotalContacts(std::size_t _maxContacts) { this->maxCollisionPairContacts = _maxContacts; } ///////////////////////////////////////////////// -std::size_t GzOdeCollisionDetector::GetMaxContacts() const +std::size_t GzOdeCollisionDetector::GetCollisionPairMaxTotalContacts() const { return this->maxCollisionPairContacts; } ///////////////////////////////////////////////// -void GzOdeCollisionDetector::LimitMaxContacts( +void GzOdeCollisionDetector::LimitCollisionPairMaxTotalContacts( CollisionResult *_result) { if (this->maxCollisionPairContacts == diff --git a/dartsim/src/GzOdeCollisionDetector.hh b/dartsim/src/GzOdeCollisionDetector.hh index bdb34e0d2..82d1e3c11 100644 --- a/dartsim/src/GzOdeCollisionDetector.hh +++ b/dartsim/src/GzOdeCollisionDetector.hh @@ -42,12 +42,12 @@ class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector /// objects /// \param[in] _maxContacts Maximum number of contacts between a pair of /// collision objects. - public: void SetMaxContacts(std::size_t _maxContacts); + public: void SetCollisionPairMaxTotalContacts(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 GetMaxContacts() const; + public: std::size_t GetCollisionPairMaxTotalContacts() const; /// \brief Create the GzOdeCollisionDetector @@ -60,7 +60,7 @@ class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector /// 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 LimitMaxContacts(CollisionResult *_result); + private: void LimitCollisionPairMaxTotalContacts(CollisionResult *_result); /// \brief Maximum number of contacts between a pair of collision objects. private: std::size_t maxCollisionPairContacts = diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc index d8e0c4880..715c1231b 100644 --- a/dartsim/src/WorldFeatures.cc +++ b/dartsim/src/WorldFeatures.cc @@ -98,7 +98,7 @@ WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( } ///////////////////////////////////////////////// -void WorldFeatures::SetWorldMaxContacts( +void WorldFeatures::SetWorldCollisionPairMaxTotalContacts( const Identity &_id, std::size_t _maxContacts) { auto world = this->ReferenceInterface(_id); @@ -110,7 +110,7 @@ void WorldFeatures::SetWorldMaxContacts( collisionDetector); if (odeCollisionDetector) { - odeCollisionDetector->SetMaxContacts(_maxContacts); + odeCollisionDetector->SetCollisionPairMaxTotalContacts(_maxContacts); } else { @@ -120,7 +120,7 @@ void WorldFeatures::SetWorldMaxContacts( } ///////////////////////////////////////////////// -std::size_t WorldFeatures::GetWorldMaxContacts(const Identity &_id) +std::size_t WorldFeatures::GetWorldCollisionPairMaxTotalContacts(const Identity &_id) const { auto world = this->ReferenceInterface(_id); @@ -131,7 +131,7 @@ std::size_t WorldFeatures::GetWorldMaxContacts(const Identity &_id) collisionDetector); if (odeCollisionDetector) { - return odeCollisionDetector->GetMaxContacts(); + return odeCollisionDetector->GetCollisionPairMaxTotalContacts(); } return 0u; diff --git a/dartsim/src/WorldFeatures.hh b/dartsim/src/WorldFeatures.hh index fd23e0477..850d9439c 100644 --- a/dartsim/src/WorldFeatures.hh +++ b/dartsim/src/WorldFeatures.hh @@ -30,8 +30,8 @@ namespace dartsim { struct WorldFeatureList : FeatureList< CollisionDetector, + CollisionPairMaxTotalContacts, Gravity, - MaxContacts, Solver > { }; @@ -55,11 +55,11 @@ class WorldFeatures : public: LinearVectorType GetWorldGravity(const Identity &_id) const override; // Documentation inherited - public: void SetWorldMaxContacts( + public: void SetWorldCollisionPairMaxTotalContacts( const Identity &_id, std::size_t _maxContacts) override; // Documentation inherited - public: std::size_t GetWorldMaxContacts(const Identity &_id) + public: std::size_t GetWorldCollisionPairMaxTotalContacts(const Identity &_id) const override; // Documentation inherited diff --git a/include/gz/physics/World.hh b/include/gz/physics/World.hh index 6f4cf2aa6..fc6deeb6d 100644 --- a/include/gz/physics/World.hh +++ b/include/gz/physics/World.hh @@ -131,7 +131,8 @@ namespace gz }; ///////////////////////////////////////////////// - class GZ_PHYSICS_VISIBLE MaxContacts: public virtual Feature + class GZ_PHYSICS_VISIBLE CollisionPairMaxTotalContacts: + public virtual Feature { /// \brief The World API for getting and setting max contacts. public: template @@ -140,12 +141,12 @@ namespace gz /// \brief Set the maximum number of contacts allowed between two /// entities. /// \param[in] _maxContacts Maximum number of contacts. - public: void SetMaxContacts(std::size_t _maxContacts); + public: void SetCollisionPairMaxTotalContacts(std::size_t _maxContacts); /// \brief Set the maximum number of contacts allowed between two /// entities. /// \return Maximum number of contacts. - public: std::size_t GetMaxContacts() const; + public: std::size_t GetCollisionPairMaxTotalContacts() const; }; /// \private The implementation API for getting and setting max contacts. @@ -156,14 +157,14 @@ namespace gz /// contacts between two entities. /// \param[in] _id Identity of the world. /// \param[in] _maxContacts Maximum number of contacts. - public: virtual void SetWorldMaxContacts( + public: virtual void SetWorldCollisionPairMaxTotalContacts( 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. /// \param[in] _maxContacts Maximum number of contacts. - public: virtual std::size_t GetWorldMaxContacts( + public: virtual std::size_t GetWorldCollisionPairMaxTotalContacts( const Identity &_id) const = 0; }; }; diff --git a/include/gz/physics/detail/World.hh b/include/gz/physics/detail/World.hh index 1986a51f3..72421e270 100644 --- a/include/gz/physics/detail/World.hh +++ b/include/gz/physics/detail/World.hh @@ -101,20 +101,20 @@ auto Gravity::World::GetGravity( ///////////////////////////////////////////////// template -void MaxContacts::World::SetMaxContacts( - std::size_t _maxContacts) +void CollisionPairMaxTotalContacts::World::SetCollisionPairMaxTotalContacts(std::size_t _maxContacts) { - this->template Interface() - ->SetWorldMaxContacts(this->identity, _maxContacts); + this->template Interface() + ->SetWorldCollisionPairMaxTotalContacts(this->identity, _maxContacts); } ///////////////////////////////////////////////// template -std::size_t MaxContacts::World:: - GetMaxContacts() const +std::size_t CollisionPairMaxTotalContacts::World:: + GetCollisionPairMaxTotalContacts() const { - return this->template Interface() - ->GetWorldMaxContacts(this->identity); + return this->template Interface() + ->GetWorldCollisionPairMaxTotalContacts(this->identity); } ///////////////////////////////////////////////// diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index a99b934fe..63437a345 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -220,49 +220,54 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts) } // The features that an engine must have to be loaded by this loader. -struct FeaturesMaxContacts : gz::physics::FeatureList< +struct FeaturesCollisionPairMaxTotalContacts : gz::physics::FeatureList< gz::physics::sdf::ConstructSdfWorld, gz::physics::GetContactsFromLastStepFeature, gz::physics::ForwardStep, - gz::physics::MaxContacts + gz::physics::CollisionPairMaxTotalContacts > {}; template -class SimulationFeaturesMaxContactsTest : +class SimulationFeaturesCollisionPairMaxTotalContactsTest : public SimulationFeaturesTest{}; -using SimulationFeaturesMaxContactsTestTypes = - ::testing::Types; -TYPED_TEST_SUITE(SimulationFeaturesMaxContactsTest, - SimulationFeaturesMaxContactsTestTypes); +using SimulationFeaturesCollisionPairMaxTotalContactsTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesCollisionPairMaxTotalContactsTest, + SimulationFeaturesCollisionPairMaxTotalContactsTestTypes); ///////////////////////////////////////////////// -TYPED_TEST(SimulationFeaturesMaxContactsTest, MaxContacts) +TYPED_TEST(SimulationFeaturesCollisionPairMaxTotalContactsTest, + CollisionPairMaxTotalContacts) { for (const std::string &name : this->pluginNames) { - auto world = LoadPluginAndWorld( + auto world = LoadPluginAndWorld( this->loader, name, gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); - auto checkedOutput = StepWorld(world, true, 1).first; + auto checkedOutput = StepWorld( + world, true, 1).first; EXPECT_TRUE(checkedOutput); auto contacts = world->GetContactsFromLastStep(); - EXPECT_EQ(std::numeric_limits::max(), world->GetMaxContacts()); + EXPECT_EQ(std::numeric_limits::max(), + world->GetCollisionPairMaxTotalContacts()); // Large box collides with other shapes EXPECT_GT(contacts.size(), 30u); - world->SetMaxContacts(1u); - EXPECT_EQ(1u, world->GetMaxContacts()); - checkedOutput = StepWorld(world, true, 1).first; + world->SetCollisionPairMaxTotalContacts(1u); + EXPECT_EQ(1u, world->GetCollisionPairMaxTotalContacts()); + checkedOutput = StepWorld( + world, true, 1).first; EXPECT_TRUE(checkedOutput); contacts = world->GetContactsFromLastStep(); EXPECT_EQ(4u, contacts.size()); - world->SetMaxContacts(0u); - EXPECT_EQ(0u, world->GetMaxContacts()); - checkedOutput = StepWorld(world, true, 1).first; + world->SetCollisionPairMaxTotalContacts(0u); + EXPECT_EQ(0u, world->GetCollisionPairMaxTotalContacts()); + checkedOutput = StepWorld( + world, true, 1).first; EXPECT_TRUE(checkedOutput); contacts = world->GetContactsFromLastStep(); From af1655cb7469fc18bc8471546ea73de81a8827dd Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 8 Jan 2024 21:35:47 +0000 Subject: [PATCH 09/12] line wrap Signed-off-by: Ian Chen --- dartsim/src/GzOdeCollisionDetector.cc | 3 ++- dartsim/src/WorldFeatures.cc | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc index 70a1bbd97..d75eeb7aa 100644 --- a/dartsim/src/GzOdeCollisionDetector.cc +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -69,7 +69,8 @@ bool GzOdeCollisionDetector::collide( } ///////////////////////////////////////////////// -void GzOdeCollisionDetector::SetCollisionPairMaxTotalContacts(std::size_t _maxContacts) +void GzOdeCollisionDetector::SetCollisionPairMaxTotalContacts( + std::size_t _maxContacts) { this->maxCollisionPairContacts = _maxContacts; } diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc index 715c1231b..6e6e2ea4b 100644 --- a/dartsim/src/WorldFeatures.cc +++ b/dartsim/src/WorldFeatures.cc @@ -120,8 +120,8 @@ void WorldFeatures::SetWorldCollisionPairMaxTotalContacts( } ///////////////////////////////////////////////// -std::size_t WorldFeatures::GetWorldCollisionPairMaxTotalContacts(const Identity &_id) - const +std::size_t WorldFeatures::GetWorldCollisionPairMaxTotalContacts( + const Identity &_id) const { auto world = this->ReferenceInterface(_id); auto collisionDetector = From e4e51e1ebcd041981e4047aeb75adaa0f771875c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 13 Jan 2024 00:45:59 +0000 Subject: [PATCH 10/12] copyright year Signed-off-by: Ian Chen --- dartsim/src/GzOdeCollisionDetector.cc | 2 +- dartsim/src/GzOdeCollisionDetector.hh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc index d75eeb7aa..868505a20 100644 --- a/dartsim/src/GzOdeCollisionDetector.cc +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2023 Open Source Robotics Foundation + * 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. diff --git a/dartsim/src/GzOdeCollisionDetector.hh b/dartsim/src/GzOdeCollisionDetector.hh index 82d1e3c11..f3859fd82 100644 --- a/dartsim/src/GzOdeCollisionDetector.hh +++ b/dartsim/src/GzOdeCollisionDetector.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2023 Open Source Robotics Foundation + * 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. From b140329abacec3cf1961b1dc7924d4985fed556f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 13 Jan 2024 01:18:41 +0000 Subject: [PATCH 11/12] CollisionPairMaxTotalContacts -> CollisionPairMaxContacts Signed-off-by: Ian Chen --- dartsim/src/GzOdeCollisionDetector.cc | 10 +++---- dartsim/src/GzOdeCollisionDetector.hh | 6 ++--- dartsim/src/WorldFeatures.cc | 8 +++--- dartsim/src/WorldFeatures.hh | 6 ++--- include/gz/physics/World.hh | 10 +++---- include/gz/physics/detail/World.hh | 16 +++++------ test/common_test/simulation_features.cc | 36 ++++++++++++------------- 7 files changed, 46 insertions(+), 46 deletions(-) diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc index 868505a20..228ee9562 100644 --- a/dartsim/src/GzOdeCollisionDetector.cc +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -52,7 +52,7 @@ bool GzOdeCollisionDetector::collide( CollisionResult *_result) { bool ret = OdeCollisionDetector::collide(_group, _option, _result); - this->LimitCollisionPairMaxTotalContacts(_result); + this->LimitCollisionPairMaxContacts(_result); return ret; } @@ -64,25 +64,25 @@ bool GzOdeCollisionDetector::collide( CollisionResult *_result) { bool ret = OdeCollisionDetector::collide(_group1, _group2, _option, _result); - this->LimitCollisionPairMaxTotalContacts(_result); + this->LimitCollisionPairMaxContacts(_result); return ret; } ///////////////////////////////////////////////// -void GzOdeCollisionDetector::SetCollisionPairMaxTotalContacts( +void GzOdeCollisionDetector::SetCollisionPairMaxContacts( std::size_t _maxContacts) { this->maxCollisionPairContacts = _maxContacts; } ///////////////////////////////////////////////// -std::size_t GzOdeCollisionDetector::GetCollisionPairMaxTotalContacts() const +std::size_t GzOdeCollisionDetector::GetCollisionPairMaxContacts() const { return this->maxCollisionPairContacts; } ///////////////////////////////////////////////// -void GzOdeCollisionDetector::LimitCollisionPairMaxTotalContacts( +void GzOdeCollisionDetector::LimitCollisionPairMaxContacts( CollisionResult *_result) { if (this->maxCollisionPairContacts == diff --git a/dartsim/src/GzOdeCollisionDetector.hh b/dartsim/src/GzOdeCollisionDetector.hh index f3859fd82..de8233e93 100644 --- a/dartsim/src/GzOdeCollisionDetector.hh +++ b/dartsim/src/GzOdeCollisionDetector.hh @@ -42,12 +42,12 @@ class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector /// objects /// \param[in] _maxContacts Maximum number of contacts between a pair of /// collision objects. - public: void SetCollisionPairMaxTotalContacts(std::size_t _maxContacts); + 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 GetCollisionPairMaxTotalContacts() const; + public: std::size_t GetCollisionPairMaxContacts() const; /// \brief Create the GzOdeCollisionDetector @@ -60,7 +60,7 @@ class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector /// 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 LimitCollisionPairMaxTotalContacts(CollisionResult *_result); + private: void LimitCollisionPairMaxContacts(CollisionResult *_result); /// \brief Maximum number of contacts between a pair of collision objects. private: std::size_t maxCollisionPairContacts = diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc index 6e6e2ea4b..da7088580 100644 --- a/dartsim/src/WorldFeatures.cc +++ b/dartsim/src/WorldFeatures.cc @@ -98,7 +98,7 @@ WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( } ///////////////////////////////////////////////// -void WorldFeatures::SetWorldCollisionPairMaxTotalContacts( +void WorldFeatures::SetWorldCollisionPairMaxContacts( const Identity &_id, std::size_t _maxContacts) { auto world = this->ReferenceInterface(_id); @@ -110,7 +110,7 @@ void WorldFeatures::SetWorldCollisionPairMaxTotalContacts( collisionDetector); if (odeCollisionDetector) { - odeCollisionDetector->SetCollisionPairMaxTotalContacts(_maxContacts); + odeCollisionDetector->SetCollisionPairMaxContacts(_maxContacts); } else { @@ -120,7 +120,7 @@ void WorldFeatures::SetWorldCollisionPairMaxTotalContacts( } ///////////////////////////////////////////////// -std::size_t WorldFeatures::GetWorldCollisionPairMaxTotalContacts( +std::size_t WorldFeatures::GetWorldCollisionPairMaxContacts( const Identity &_id) const { auto world = this->ReferenceInterface(_id); @@ -131,7 +131,7 @@ std::size_t WorldFeatures::GetWorldCollisionPairMaxTotalContacts( collisionDetector); if (odeCollisionDetector) { - return odeCollisionDetector->GetCollisionPairMaxTotalContacts(); + return odeCollisionDetector->GetCollisionPairMaxContacts(); } return 0u; diff --git a/dartsim/src/WorldFeatures.hh b/dartsim/src/WorldFeatures.hh index 850d9439c..da15810dd 100644 --- a/dartsim/src/WorldFeatures.hh +++ b/dartsim/src/WorldFeatures.hh @@ -30,7 +30,7 @@ namespace dartsim { struct WorldFeatureList : FeatureList< CollisionDetector, - CollisionPairMaxTotalContacts, + CollisionPairMaxContacts, Gravity, Solver > { }; @@ -55,11 +55,11 @@ class WorldFeatures : public: LinearVectorType GetWorldGravity(const Identity &_id) const override; // Documentation inherited - public: void SetWorldCollisionPairMaxTotalContacts( + public: void SetWorldCollisionPairMaxContacts( const Identity &_id, std::size_t _maxContacts) override; // Documentation inherited - public: std::size_t GetWorldCollisionPairMaxTotalContacts(const Identity &_id) + public: std::size_t GetWorldCollisionPairMaxContacts(const Identity &_id) const override; // Documentation inherited diff --git a/include/gz/physics/World.hh b/include/gz/physics/World.hh index fc6deeb6d..554ca266d 100644 --- a/include/gz/physics/World.hh +++ b/include/gz/physics/World.hh @@ -131,7 +131,7 @@ namespace gz }; ///////////////////////////////////////////////// - class GZ_PHYSICS_VISIBLE CollisionPairMaxTotalContacts: + class GZ_PHYSICS_VISIBLE CollisionPairMaxContacts: public virtual Feature { /// \brief The World API for getting and setting max contacts. @@ -141,12 +141,12 @@ namespace gz /// \brief Set the maximum number of contacts allowed between two /// entities. /// \param[in] _maxContacts Maximum number of contacts. - public: void SetCollisionPairMaxTotalContacts(std::size_t _maxContacts); + public: void SetCollisionPairMaxContacts(std::size_t _maxContacts); /// \brief Set the maximum number of contacts allowed between two /// entities. /// \return Maximum number of contacts. - public: std::size_t GetCollisionPairMaxTotalContacts() const; + public: std::size_t GetCollisionPairMaxContacts() const; }; /// \private The implementation API for getting and setting max contacts. @@ -157,14 +157,14 @@ namespace gz /// contacts between two entities. /// \param[in] _id Identity of the world. /// \param[in] _maxContacts Maximum number of contacts. - public: virtual void SetWorldCollisionPairMaxTotalContacts( + 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. /// \param[in] _maxContacts Maximum number of contacts. - public: virtual std::size_t GetWorldCollisionPairMaxTotalContacts( + public: virtual std::size_t GetWorldCollisionPairMaxContacts( const Identity &_id) const = 0; }; }; diff --git a/include/gz/physics/detail/World.hh b/include/gz/physics/detail/World.hh index 72421e270..d5270913c 100644 --- a/include/gz/physics/detail/World.hh +++ b/include/gz/physics/detail/World.hh @@ -101,20 +101,20 @@ auto Gravity::World::GetGravity( ///////////////////////////////////////////////// template -void CollisionPairMaxTotalContacts::World::SetCollisionPairMaxTotalContacts(std::size_t _maxContacts) +void CollisionPairMaxContacts::World::SetCollisionPairMaxContacts(std::size_t _maxContacts) { - this->template Interface() - ->SetWorldCollisionPairMaxTotalContacts(this->identity, _maxContacts); + this->template Interface() + ->SetWorldCollisionPairMaxContacts(this->identity, _maxContacts); } ///////////////////////////////////////////////// template -std::size_t CollisionPairMaxTotalContacts::World:: - GetCollisionPairMaxTotalContacts() const +std::size_t CollisionPairMaxContacts::World:: + GetCollisionPairMaxContacts() const { - return this->template Interface() - ->GetWorldCollisionPairMaxTotalContacts(this->identity); + return this->template Interface() + ->GetWorldCollisionPairMaxContacts(this->identity); } ///////////////////////////////////////////////// diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 63437a345..005693f05 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -220,53 +220,53 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts) } // The features that an engine must have to be loaded by this loader. -struct FeaturesCollisionPairMaxTotalContacts : gz::physics::FeatureList< +struct FeaturesCollisionPairMaxContacts : gz::physics::FeatureList< gz::physics::sdf::ConstructSdfWorld, gz::physics::GetContactsFromLastStepFeature, gz::physics::ForwardStep, - gz::physics::CollisionPairMaxTotalContacts + gz::physics::CollisionPairMaxContacts > {}; template -class SimulationFeaturesCollisionPairMaxTotalContactsTest : +class SimulationFeaturesCollisionPairMaxContactsTest : public SimulationFeaturesTest{}; -using SimulationFeaturesCollisionPairMaxTotalContactsTestTypes = - ::testing::Types; -TYPED_TEST_SUITE(SimulationFeaturesCollisionPairMaxTotalContactsTest, - SimulationFeaturesCollisionPairMaxTotalContactsTestTypes); +using SimulationFeaturesCollisionPairMaxContactsTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesCollisionPairMaxContactsTest, + SimulationFeaturesCollisionPairMaxContactsTestTypes); ///////////////////////////////////////////////// -TYPED_TEST(SimulationFeaturesCollisionPairMaxTotalContactsTest, - CollisionPairMaxTotalContacts) +TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest, + CollisionPairMaxContacts) { for (const std::string &name : this->pluginNames) { - auto world = LoadPluginAndWorld( + auto world = LoadPluginAndWorld( this->loader, name, gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); - auto checkedOutput = StepWorld( + auto checkedOutput = StepWorld( world, true, 1).first; EXPECT_TRUE(checkedOutput); auto contacts = world->GetContactsFromLastStep(); EXPECT_EQ(std::numeric_limits::max(), - world->GetCollisionPairMaxTotalContacts()); + world->GetCollisionPairMaxContacts()); // Large box collides with other shapes EXPECT_GT(contacts.size(), 30u); - world->SetCollisionPairMaxTotalContacts(1u); - EXPECT_EQ(1u, world->GetCollisionPairMaxTotalContacts()); - checkedOutput = StepWorld( + 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->SetCollisionPairMaxTotalContacts(0u); - EXPECT_EQ(0u, world->GetCollisionPairMaxTotalContacts()); - checkedOutput = StepWorld( + world->SetCollisionPairMaxContacts(0u); + EXPECT_EQ(0u, world->GetCollisionPairMaxContacts()); + checkedOutput = StepWorld( world, true, 1).first; EXPECT_TRUE(checkedOutput); From 89d63477103ce082d334ebd2ea92f31109c6500b Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 18 Jan 2024 18:06:18 +0000 Subject: [PATCH 12/12] fix doc Signed-off-by: Ian Chen --- include/gz/physics/World.hh | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/include/gz/physics/World.hh b/include/gz/physics/World.hh index 554ca266d..dabc453ef 100644 --- a/include/gz/physics/World.hh +++ b/include/gz/physics/World.hh @@ -134,7 +134,8 @@ namespace gz class GZ_PHYSICS_VISIBLE CollisionPairMaxContacts: public virtual Feature { - /// \brief The World API for getting and setting max contacts. + /// \brief The World API for getting and setting the maximum + /// number of contacts between two entities. public: template class World : public virtual Feature::World { @@ -143,7 +144,7 @@ namespace gz /// \param[in] _maxContacts Maximum number of contacts. public: void SetCollisionPairMaxContacts(std::size_t _maxContacts); - /// \brief Set the maximum number of contacts allowed between two + /// \brief Get the maximum number of contacts allowed between two /// entities. /// \return Maximum number of contacts. public: std::size_t GetCollisionPairMaxContacts() const; @@ -163,7 +164,7 @@ namespace gz /// \brief Implementation API for getting the maximum number of /// contacts between two entities. /// \param[in] _id Identity of the world. - /// \param[in] _maxContacts Maximum number of contacts. + /// \return Maximum number of contacts. public: virtual std::size_t GetWorldCollisionPairMaxContacts( const Identity &_id) const = 0; };