Skip to content

Commit

Permalink
Support setting max contacts in dart's bullet collision detector (#593)
Browse files Browse the repository at this point in the history 
Signed-off-by: Ian Chen <[email protected]>
  • Loading branch information
@iche033
iche033 authored Dec 20, 2024
1 parent a369e18 commit a4eee2f
Show file tree
Hide file tree
Showing 5 changed files with 190 additions and 77 deletions.
2 changes: 1 addition & 1 deletion dartsim/src/EntityManagementFeatures.cc
View file
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
136 changes: 93 additions & 43 deletions dartsim/src/GzOdeCollisionDetector.cc → dartsim/src/GzCollisionDetector.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,69 +21,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()
GzCollisionDetector::GzCollisionDetector()
{
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;
}

/////////////////////////////////////////////////
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 @@ -135,3 +97,91 @@ 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()
{
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;
}
86 changes: 64 additions & 22 deletions dartsim/src/GzOdeCollisionDetector.hh → dartsim/src/GzCollisionDetector.hh
View file
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
View file
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
View file
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

0 comments on commit a4eee2f

Please sign in to comment.