Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Minor optimization for picking contact points in dartsim's ODE collision detector #584

Merged
merged 24 commits into from
Jun 10, 2024
Merged
Changes from 1 commit
Commits
Show all changes
24 commits
Select commit Hold shift + click to select a range
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
add support for max contacts in dart's ode collision detector
Signed-off-by: Ian Chen <[email protected]>
iche033 committed Dec 19, 2023
commit 72ad1467b6db36928975606a6400b641008a1f3d
36 changes: 23 additions & 13 deletions dartsim/src/Base.hh
Original file line number Diff line number Diff line change
@@ -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<FeatureList<Feature>>
public: using LinkInfoPtr = std::shared_ptr<LinkInfo>;
public: using JointInfoPtr = std::shared_ptr<JointInfo>;
public: using ShapeInfoPtr = std::shared_ptr<ShapeInfo>;
public: using WorldInfoPtr = std::shared_ptr<WorldInfo>;

public: inline Identity InitiateEngine(std::size_t /*_engineID*/) override
{
@@ -300,15 +308,17 @@ class Base : public Implements3d<FeatureList<Feature>>
{
const std::size_t id = this->GetNextEntity();
_world->setName(_name);
this->worlds.AddEntity(id, _world, _name, 0);
auto worldInfo = std::make_shared<WorldInfo>();
worldInfo->world = _world;
this->worlds.AddEntity(id, worldInfo, _name, 0);

this->frames[id] = dart::dynamics::Frame::World();
auto model = dart::dynamics::Skeleton::create("");

auto modelInfo = std::make_shared<ModelInfo>();
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<FeatureList<Feature>>
const std::size_t id = this->GetNextEntity();
auto entry = std::make_shared<ModelInfo>(_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<FeatureList<Feature>>
const std::size_t id = this->GetNextEntity();
auto entry = std::make_shared<ModelInfo>(_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<FeatureList<Feature>>
_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<FeatureList<Feature>>
{
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<FeatureList<Feature>>
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<FeatureList<Feature>>
{
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<FeatureList<Feature>>
<< _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<FeatureList<Feature>>
return this->models.at(_modelID);
}

public: EntityStorage<DartWorldPtr, std::string> worlds;
public: EntityStorage<WorldInfoPtr, std::string> worlds;
public: EntityStorage<ModelInfoPtr, DartConstSkeletonPtr> models;
public: EntityStorage<LinkInfoPtr, const DartBodyNode*> links;
public: EntityStorage<JointInfoPtr, const DartJoint*> joints;
public: EntityStorage<ShapeInfoPtr, const DartShapeNode*> shapes;
public: std::unordered_map<std::size_t, dart::dynamics::Frame*> frames;
public: EntityStorage<ModelInfoPtr, DartWorldPtr> modelProxiesToWorld;
public: EntityStorage<ModelInfoPtr, WorldInfoPtr> 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
2 changes: 1 addition & 1 deletion dartsim/src/CustomFeatures.cc
Original file line number Diff line number Diff line change
@@ -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]

29 changes: 18 additions & 11 deletions dartsim/src/EntityManagementFeatures.cc
Original file line number Diff line number Diff line change
@@ -29,6 +29,8 @@
#include <dart/collision/CollisionFilter.hpp>
#include <dart/collision/CollisionObject.hpp>

#include "GzOdeCollisionDetector.hh"

namespace gz {
namespace physics {
namespace dartsim {
@@ -105,7 +107,7 @@ class BitmaskContactFilter : public dart::collision::BodyNodeCollisionFilter
static std::shared_ptr<BitmaskContactFilter> 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<BitmaskContactFilter>(
world->getConstraintSolver()->getCollisionOption()
@@ -172,7 +174,7 @@ Identity EntityManagementFeatures::GetWorld(
const std::string &EntityManagementFeatures::GetWorldName(
const Identity &_worldID) const
{
return this->ReferenceInterface<DartWorld>(_worldID)->getName();
return this->ReferenceInterface<WorldInfo>(_worldID)->world->getName();
}

/////////////////////////////////////////////////
@@ -231,7 +233,7 @@ Identity EntityManagementFeatures::GetModel(
const Identity &_worldID, const std::string &_modelName) const
{
const DartSkeletonPtr &model =
this->ReferenceInterface<DartWorld>(_worldID)->getSkeleton(_modelName);
this->ReferenceInterface<WorldInfo>(_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<dart::simulation::World>(_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<dart::collision::GzOdeCollisionDetector>(
collisionDetector)->SetMaxContacts(20);

world->getConstraintSolver()->getCollisionOption().collisionFilter =
std::make_shared<BitmaskContactFilter>();
@@ -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()));
104 changes: 104 additions & 0 deletions dartsim/src/GzOdeCollisionDetector.cc
Original file line number Diff line number Diff line change
@@ -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 <dart/collision/CollisionObject.hpp>

#include "GzOdeCollisionDetector.hh"

using namespace dart;
using namespace collision;

/////////////////////////////////////////////////
GzOdeCollisionDetector::GzOdeCollisionDetector()
: OdeCollisionDetector()
{
}

/////////////////////////////////////////////////
GzOdeCollisionDetector::Registrar<GzOdeCollisionDetector>
GzOdeCollisionDetector::mRegistrar{
GzOdeCollisionDetector::getStaticType(),
[]() -> std::shared_ptr<GzOdeCollisionDetector> {
return GzOdeCollisionDetector::create();
}};

/////////////////////////////////////////////////
std::shared_ptr<GzOdeCollisionDetector> GzOdeCollisionDetector::create()
{
return std::shared_ptr<GzOdeCollisionDetector>(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<dart::collision::CollisionObject *,
std::unordered_map<dart::collision::CollisionObject *, std::size_t>>
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<int>(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;
}
61 changes: 61 additions & 0 deletions dartsim/src/GzOdeCollisionDetector.hh
Original file line number Diff line number Diff line change
@@ -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 <dart/collision/ode/OdeCollisionDetector.hpp>

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<GzOdeCollisionDetector> 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<GzOdeCollisionDetector> mRegistrar;
};

}
}
Loading