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

Support setting max contacts in dart's bullet collision detector (backport #593) #709

Merged
merged 2 commits into from
Dec 21, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
2 changes: 1 addition & 1 deletion dartsim/src/EntityManagementFeatures.cc
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@
#include <dart/collision/CollisionFilter.hpp>
#include <dart/collision/CollisionObject.hpp>

#include "GzOdeCollisionDetector.hh"
#include "GzCollisionDetector.hh"

namespace gz {
namespace physics {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,76 +22,31 @@

#include <dart/collision/CollisionObject.hpp>

#include "GzOdeCollisionDetector.hh"
#include "GzCollisionDetector.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()
{
// GzOdeCollisionDetector constructor calls the OdeCollisionDetector
// constructor, that calls the non-thread safe dInitODE2(0).
// To mitigate this problem, we use a static mutex to ensure that
// each GzOdeCollisionDetector constructor is called not at the same time.
// See https://github.com/gazebosim/gz-sim/issues/18 for more info.
static std::mutex odeInitMutex;
std::unique_lock<std::mutex> lock(odeInitMutex);
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->LimitCollisionPairMaxContacts(_result);
return ret;
}

/////////////////////////////////////////////////
bool GzOdeCollisionDetector::collide(
CollisionGroup *_group1,
CollisionGroup *_group2,
const CollisionOption &_option,
CollisionResult *_result)
GzCollisionDetector::GzCollisionDetector()
{
bool ret = OdeCollisionDetector::collide(_group1, _group2, _option, _result);
this->LimitCollisionPairMaxContacts(_result);
return ret;
}

/////////////////////////////////////////////////
void GzOdeCollisionDetector::SetCollisionPairMaxContacts(
void GzCollisionDetector::SetCollisionPairMaxContacts(
std::size_t _maxContacts)
{
this->maxCollisionPairContacts = _maxContacts;
}

/////////////////////////////////////////////////
std::size_t GzOdeCollisionDetector::GetCollisionPairMaxContacts() const
std::size_t GzCollisionDetector::GetCollisionPairMaxContacts() const
{
return this->maxCollisionPairContacts;
}

/////////////////////////////////////////////////
void GzOdeCollisionDetector::LimitCollisionPairMaxContacts(
void GzCollisionDetector::LimitCollisionPairMaxContacts(
CollisionResult *_result)
{
if (this->maxCollisionPairContacts ==
Expand Down Expand Up @@ -143,3 +98,98 @@ void GzOdeCollisionDetector::LimitCollisionPairMaxContacts(
}
}
}

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

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

/////////////////////////////////////////////////
std::shared_ptr<GzOdeCollisionDetector> GzOdeCollisionDetector::create()
{
// GzOdeCollisionDetector constructor calls the OdeCollisionDetector
// constructor, that calls the non-thread safe dInitODE2(0).
// To mitigate this problem, we use a static mutex to ensure that
// each GzOdeCollisionDetector constructor is called not at the same time.
// See https://github.com/gazebosim/gz-sim/issues/18 for more info.
static std::mutex odeInitMutex;
std::unique_lock<std::mutex> lock(odeInitMutex);
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->LimitCollisionPairMaxContacts(_result);
return ret;
}

/////////////////////////////////////////////////
bool GzOdeCollisionDetector::collide(
CollisionGroup *_group1,
CollisionGroup *_group2,
const CollisionOption &_option,
CollisionResult *_result)
{
bool ret = OdeCollisionDetector::collide(_group1, _group2, _option, _result);
this->LimitCollisionPairMaxContacts(_result);
return ret;
}

/////////////////////////////////////////////////
GzBulletCollisionDetector::GzBulletCollisionDetector()
: BulletCollisionDetector(), GzCollisionDetector()
{
}

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

/////////////////////////////////////////////////
std::shared_ptr<GzBulletCollisionDetector> GzBulletCollisionDetector::create()
{
return std::shared_ptr<GzBulletCollisionDetector>(
new GzBulletCollisionDetector());
}

/////////////////////////////////////////////////
bool GzBulletCollisionDetector::collide(
CollisionGroup *_group,
const CollisionOption &_option,
CollisionResult *_result)
{
bool ret = BulletCollisionDetector::collide(_group, _option, _result);
this->LimitCollisionPairMaxContacts(_result);
return ret;
}

/////////////////////////////////////////////////
bool GzBulletCollisionDetector::collide(
CollisionGroup *_group1,
CollisionGroup *_group2,
const CollisionOption &_option,
CollisionResult *_result)
{
bool ret = BulletCollisionDetector::collide(
_group1, _group2, _option, _result);
this->LimitCollisionPairMaxContacts(_result);
return ret;
}
Original file line number Diff line number Diff line change
Expand Up @@ -15,15 +15,51 @@
*
*/

#ifndef GZ_PHYSICS_DARTSIM_SRC_GZCOLLISIONDETECTOR_HH_
#define GZ_PHYSICS_DARTSIM_SRC_GZCOLLISIONDETECTOR_HH_

#include <cstdio>
#include <limits>
#include <memory>

#include <dart/collision/CollisionResult.hpp>
#include <dart/collision/bullet/BulletCollisionDetector.hpp>
#include <dart/collision/ode/OdeCollisionDetector.hpp>

namespace dart {
namespace collision {

class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector
class GzCollisionDetector
{
/// \brief Set the maximum number of contacts between a pair of collision
/// objects
/// \param[in] _maxContacts Maximum number of contacts between a pair of
/// collision objects.
public: virtual 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: virtual std::size_t GetCollisionPairMaxContacts() const;

/// Constructor
protected: GzCollisionDetector();

/// \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
protected: virtual void LimitCollisionPairMaxContacts(
CollisionResult *_result);

/// \brief Maximum number of contacts between a pair of collision objects.
protected: std::size_t maxCollisionPairContacts =
std::numeric_limits<std::size_t>::max();
};

class GzOdeCollisionDetector :
public dart::collision::OdeCollisionDetector,
public dart::collision::GzCollisionDetector
{
// Documentation inherited
public: bool collide(
Expand All @@ -38,36 +74,42 @@ class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector
const CollisionOption& option = CollisionOption(false, 1u, nullptr),
CollisionResult* result = nullptr) override;

/// \brief Set the maximum number of contacts between a pair of collision
/// objects
/// \param[in] _maxContacts Maximum number of contacts between a pair of
/// collision objects.
public: void SetCollisionPairMaxContacts(std::size_t _maxContacts);

/// \brief Get the maximum number of contacts between a pair of collision
/// objects
/// \return Maximum number of contacts between a pair of collision objects.
public: std::size_t GetCollisionPairMaxContacts() const;


/// \brief Create the GzOdeCollisionDetector
public: static std::shared_ptr<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 LimitCollisionPairMaxContacts(CollisionResult *_result);
private: static Registrar<GzOdeCollisionDetector> mRegistrar;
};

/// \brief Maximum number of contacts between a pair of collision objects.
private: std::size_t maxCollisionPairContacts =
std::numeric_limits<std::size_t>::max();
class GzBulletCollisionDetector :
public dart::collision::BulletCollisionDetector,
public dart::collision::GzCollisionDetector
{
// Documentation inherited
public: bool collide(
CollisionGroup* group,
const CollisionOption& option = CollisionOption(false, 1u, nullptr),
CollisionResult* result = nullptr) override;

private: static Registrar<GzOdeCollisionDetector> mRegistrar;
// Documentation inherited
public: bool collide(
CollisionGroup* group1,
CollisionGroup* group2,
const CollisionOption& option = CollisionOption(false, 1u, nullptr),
CollisionResult* result = nullptr) override;

/// \brief Create the GzBulletCollisionDetector
public: static std::shared_ptr<GzBulletCollisionDetector> create();

/// Constructor
protected: GzBulletCollisionDetector();

private: static Registrar<GzBulletCollisionDetector> mRegistrar;
};

}
}

#endif
28 changes: 17 additions & 11 deletions dartsim/src/WorldFeatures.cc
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@

#include <gz/common/Console.hh>

#include "GzOdeCollisionDetector.hh"
#include "GzCollisionDetector.hh"
#include "WorldFeatures.hh"


Expand All @@ -46,7 +46,7 @@ void WorldFeatures::SetWorldCollisionDetector(
world->getConstraintSolver()->getCollisionDetector();
if (_collisionDetector == "bullet")
{
collisionDetector = dart::collision::BulletCollisionDetector::create();
collisionDetector = dart::collision::GzBulletCollisionDetector::create();
}
else if (_collisionDetector == "fcl")
{
Expand Down Expand Up @@ -105,17 +105,17 @@ void WorldFeatures::SetWorldCollisionPairMaxContacts(
auto collisionDetector =
world->getConstraintSolver()->getCollisionDetector();

auto odeCollisionDetector =
std::dynamic_pointer_cast<dart::collision::GzOdeCollisionDetector>(
auto gzCollisionDetector =
std::dynamic_pointer_cast<dart::collision::GzCollisionDetector>(
collisionDetector);
if (odeCollisionDetector)
if (gzCollisionDetector)
{
odeCollisionDetector->SetCollisionPairMaxContacts(_maxContacts);
gzCollisionDetector->SetCollisionPairMaxContacts(_maxContacts);
}
else
{
gzwarn << "Currently max contacts feature is only supported by the "
<< "ode collision detector in dartsim." << std::endl;
<< "bullet and ode collision detector in dartsim." << std::endl;
}
}

Expand All @@ -126,12 +126,18 @@ std::size_t WorldFeatures::GetWorldCollisionPairMaxContacts(
auto world = this->ReferenceInterface<dart::simulation::World>(_id);
auto collisionDetector =
world->getConstraintSolver()->getCollisionDetector();
auto odeCollisionDetector =
std::dynamic_pointer_cast<dart::collision::GzOdeCollisionDetector>(

auto gzCollisionDetector =
std::dynamic_pointer_cast<dart::collision::GzCollisionDetector>(
collisionDetector);
if (odeCollisionDetector)
if (gzCollisionDetector)
{
return odeCollisionDetector->GetCollisionPairMaxContacts();
return gzCollisionDetector->GetCollisionPairMaxContacts();
}
else
{
gzwarn << "Currently max contacts feature is only supported by the "
<< "bullet and ode collision detector in dartsim." << std::endl;
}

return 0u;
Expand Down
15 changes: 15 additions & 0 deletions test/common_test/simulation_features.cc
Original file line number Diff line number Diff line change
Expand Up @@ -228,6 +228,7 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts)
// The features that an engine must have to be loaded by this loader.
struct FeaturesCollisionPairMaxContacts : gz::physics::FeatureList<
gz::physics::sdf::ConstructSdfWorld,
gz::physics::CollisionDetector,
gz::physics::CollisionPairMaxContacts,
gz::physics::FindFreeGroupFeature,
gz::physics::ForwardStep,
Expand Down Expand Up @@ -282,6 +283,20 @@ TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest,

contacts = world->GetContactsFromLastStep();
EXPECT_EQ(0u, contacts.size());

if (name == "gz::physics::dartsim::Plugin")
{
EXPECT_EQ("ode", world->GetCollisionDetector());
world->SetCollisionDetector("bullet");
EXPECT_EQ("bullet", world->GetCollisionDetector());
world->SetCollisionPairMaxContacts(1u);
EXPECT_EQ(1u, world->GetCollisionPairMaxContacts());
checkedOutput = StepWorld<FeaturesCollisionPairMaxContacts>(
world, true, 1).first;
EXPECT_TRUE(checkedOutput);
contacts = world->GetContactsFromLastStep();
EXPECT_EQ(5u, contacts.size());
}
}
}

Expand Down
Loading