Skip to content

Commit 20042db

Browse files
mergify[bot]ahcorde
mergify[bot]
and
ahcorde
authored
Enable/Disable collisions at runtime (#991) (#993)
(cherry picked from commit 42351fa) Signed-off-by: Alejandro Hernandez Cordero <ahcorde@gmail.com> Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
1 parent 65e3b83 commit 20042db

5 files changed

Lines changed: 226 additions & 0 deletions

File tree

‎dartsim/src/ModelFeatures.cc‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,54 @@ bool ModelFeatures::GetModelStatic(const Identity &_id) const
5656
return !modelInfo->model->isMobile();
5757
}
5858

59+
/////////////////////////////////////////////////
60+
void ModelFeatures::SetModelCollisionEnabled(
61+
const Identity &_id, bool _enabled)
62+
{
63+
auto modelInfo = this->GetModelInfo(_id);
64+
if (!modelInfo || !modelInfo->model)
65+
return;
66+
67+
for (const auto &linkInfo : modelInfo->links)
68+
{
69+
if (linkInfo->link)
70+
linkInfo->link->setCollidable(_enabled);
71+
}
72+
73+
// Also apply to nested models recursively.
74+
for (const std::size_t nestedID : modelInfo->nestedModels)
75+
{
76+
this->SetModelCollisionEnabled(this->GenerateIdentity(
77+
nestedID, this->GetModelInfo(nestedID)), _enabled);
78+
}
79+
}
80+
81+
/////////////////////////////////////////////////
82+
bool ModelFeatures::GetModelCollisionEnabled(const Identity &_id) const
83+
{
84+
const auto modelInfo = this->GetModelInfo(_id);
85+
if (!modelInfo || !modelInfo->model)
86+
return true;
87+
88+
for (const auto &linkInfo : modelInfo->links)
89+
{
90+
if (linkInfo->link && !linkInfo->link->isCollidable())
91+
return false;
92+
}
93+
94+
// Also check nested models.
95+
for (const std::size_t nestedID : modelInfo->nestedModels)
96+
{
97+
if (!this->GetModelCollisionEnabled(this->GenerateIdentity(
98+
nestedID, this->GetModelInfo(nestedID))))
99+
{
100+
return false;
101+
}
102+
}
103+
104+
return true;
105+
}
106+
59107
/////////////////////////////////////////////////
60108
void ModelFeatures::SetModelGravityEnabled(
61109
const Identity &_id, bool _enabled)

‎dartsim/src/ModelFeatures.hh‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ namespace dartsim {
3030

3131
struct ModelFeatureList : FeatureList<
3232
ModelStaticState,
33+
ModelCollisionEnabled,
3334
GravityEnabled
3435
> { };
3536

@@ -44,6 +45,12 @@ class ModelFeatures :
4445
// Documentation inherited
4546
public: bool GetModelStatic(const Identity &_id) const override;
4647

48+
// ----- Model Collision Enabled -----
49+
public: void SetModelCollisionEnabled(
50+
const Identity &_id, bool _enabled) override;
51+
52+
public: bool GetModelCollisionEnabled(const Identity &_id) const override;
53+
4754
// ----- Model Gravity Enabled -----
4855
public: void SetModelGravityEnabled(
4956
const Identity &_id, bool _enabled) override;

‎include/gz/physics/Model.hh‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,43 @@ namespace gz
5959
};
6060
};
6161

62+
/////////////////////////////////////////////////
63+
/// \brief Feature for enabling and disabling collisions on a model.
64+
/// When collisions are disabled, the model's links will not collide with
65+
/// other entities in the world.
66+
class GZ_PHYSICS_VISIBLE ModelCollisionEnabled : public virtual Feature
67+
{
68+
/// \brief The Model API for enabling and disabling collisions.
69+
public: template <typename PolicyT, typename FeaturesT>
70+
class Model : public virtual Feature::Model<PolicyT, FeaturesT>
71+
{
72+
/// \brief Enable or disable collisions for this model.
73+
/// \param[in] _enabled True to enable collisions, false to disable.
74+
public: void SetCollisionEnabled(bool _enabled);
75+
76+
/// \brief Get whether collisions are enabled for this model.
77+
/// \return True if collisions are enabled, false otherwise.
78+
public: bool GetCollisionEnabled() const;
79+
};
80+
81+
/// \private The implementation API for model collision enabled state.
82+
public: template <typename PolicyT>
83+
class Implementation : public virtual Feature::Implementation<PolicyT>
84+
{
85+
/// \brief Implementation API for enabling/disabling model collisions.
86+
/// \param[in] _id Identity of the model.
87+
/// \param[in] _enabled True to enable collisions, false to disable.
88+
public: virtual void SetModelCollisionEnabled(
89+
const Identity &_id, bool _enabled) = 0;
90+
91+
/// \brief Implementation API for getting the model collision state.
92+
/// \param[in] _id Identity of the model.
93+
/// \return True if collisions are enabled for the model.
94+
public: virtual bool GetModelCollisionEnabled(
95+
const Identity &_id) const = 0;
96+
};
97+
};
98+
6299
}
63100
}
64101

‎include/gz/physics/detail/Model.hh‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,24 @@ bool ModelStaticState::Model<PolicyT, FeaturesT>::GetStatic() const
4040
->GetModelStatic(this->identity);
4141
}
4242

43+
/////////////////////////////////////////////////
44+
template <typename PolicyT, typename FeaturesT>
45+
void ModelCollisionEnabled::Model<PolicyT, FeaturesT>::SetCollisionEnabled(
46+
bool _enabled)
47+
{
48+
this->template Interface<ModelCollisionEnabled>()
49+
->SetModelCollisionEnabled(this->identity, _enabled);
50+
}
51+
52+
/////////////////////////////////////////////////
53+
template <typename PolicyT, typename FeaturesT>
54+
bool ModelCollisionEnabled::Model<PolicyT, FeaturesT>::GetCollisionEnabled()
55+
const
56+
{
57+
return this->template Interface<ModelCollisionEnabled>()
58+
->GetModelCollisionEnabled(this->identity);
59+
}
60+
4361
}
4462
}
4563

‎test/common_test/model_features.cc‎

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,11 @@ using ModelGravityStaticFeaturesList = gz::physics::FeatureList<
6464
gz::physics::GravityEnabled
6565
>;
6666

67+
using ModelCollisionFeaturesList = gz::physics::FeatureList<
68+
ModelBaseFeaturesList,
69+
gz::physics::ModelCollisionEnabled
70+
>;
71+
6772
// ---------------------------------------------------------------------------
6873
// Typed test fixture
6974
// ---------------------------------------------------------------------------
@@ -96,6 +101,8 @@ template <typename T> class ModelStaticTest : public ModelFeaturesTest<T> {};
96101
template <typename T> class ModelGravityTest : public ModelFeaturesTest<T> {};
97102
template <typename T> class ModelGravityStaticTest
98103
: public ModelFeaturesTest<T> {};
104+
template <typename T> class ModelCollisionTest
105+
: public ModelFeaturesTest<T> {};
99106

100107
// ---------------------------------------------------------------------------
101108
// Helper: load a world from an SDF file using the given engine.
@@ -151,6 +158,9 @@ using ModelGravityStaticTestTypes =
151158
::testing::Types<ModelGravityStaticFeaturesList>;
152159
TYPED_TEST_SUITE(ModelGravityStaticTest, ModelGravityStaticTestTypes);
153160

161+
using ModelCollisionTestTypes = ::testing::Types<ModelCollisionFeaturesList>;
162+
TYPED_TEST_SUITE(ModelCollisionTest, ModelCollisionTestTypes);
163+
154164
// ---------------------------------------------------------------------------
155165
// Tests
156166
// ---------------------------------------------------------------------------
@@ -732,6 +742,112 @@ TYPED_TEST(ModelGravityTest, AddedMassLinkGravityInvariantPreserved)
732742
}
733743
}
734744

745+
//////////////////////////////////////////////////
746+
TYPED_TEST(ModelCollisionTest, ModelCollisionEnabledSetGet)
747+
{
748+
for (const std::string &name : this->pluginNames)
749+
{
750+
gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);
751+
752+
auto engine =
753+
gz::physics::RequestEngine3d<ModelCollisionFeaturesList>::From(plugin);
754+
ASSERT_NE(nullptr, engine);
755+
756+
auto world = LoadWorld<ModelCollisionFeaturesList>(
757+
engine, common_test::worlds::kSphereGravitySdf);
758+
ASSERT_NE(nullptr, world);
759+
760+
auto model = world->GetModel("sphere");
761+
ASSERT_NE(nullptr, model);
762+
763+
EXPECT_TRUE(model->GetCollisionEnabled());
764+
765+
model->SetCollisionEnabled(false);
766+
EXPECT_FALSE(model->GetCollisionEnabled());
767+
768+
model->SetCollisionEnabled(true);
769+
EXPECT_TRUE(model->GetCollisionEnabled());
770+
}
771+
}
772+
773+
//////////////////////////////////////////////////
774+
TYPED_TEST(ModelCollisionTest, NestedModelCollisionPropagates)
775+
{
776+
for (const std::string &name : this->pluginNames)
777+
{
778+
gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);
779+
780+
auto engine =
781+
gz::physics::RequestEngine3d<ModelCollisionFeaturesList>::From(plugin);
782+
ASSERT_NE(nullptr, engine);
783+
784+
auto world = LoadWorld<ModelCollisionFeaturesList>(
785+
engine, common_test::worlds::kWorldSingleNestedModelSdf);
786+
ASSERT_NE(nullptr, world);
787+
788+
auto parentModel = world->GetModel("parent_model");
789+
ASSERT_NE(nullptr, parentModel);
790+
791+
auto nestedModel = parentModel->GetNestedModel("nested_model");
792+
ASSERT_NE(nullptr, nestedModel);
793+
794+
// Disable collisions on the parent: nested model must follow.
795+
parentModel->SetCollisionEnabled(false);
796+
EXPECT_FALSE(parentModel->GetCollisionEnabled());
797+
EXPECT_FALSE(nestedModel->GetCollisionEnabled());
798+
799+
// Re-enable collisions on the parent: nested model must follow.
800+
parentModel->SetCollisionEnabled(true);
801+
EXPECT_TRUE(parentModel->GetCollisionEnabled());
802+
EXPECT_TRUE(nestedModel->GetCollisionEnabled());
803+
}
804+
}
805+
806+
//////////////////////////////////////////////////
807+
// With collisions disabled the falling sphere must pass through the static box
808+
// instead of resting on top of it.
809+
TYPED_TEST(ModelCollisionTest, ModelCollisionDisabledPassThrough)
810+
{
811+
for (const std::string &name : this->pluginNames)
812+
{
813+
gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);
814+
815+
auto engine =
816+
gz::physics::RequestEngine3d<ModelCollisionFeaturesList>::From(plugin);
817+
ASSERT_NE(nullptr, engine);
818+
819+
// falling.world: sphere (radius 1) at z=2 above a static box whose top
820+
// surface is at z=0. Gravity is the default (~ -9.8).
821+
auto world = LoadWorld<ModelCollisionFeaturesList>(
822+
engine, common_test::worlds::kFallingWorld);
823+
ASSERT_NE(nullptr, world);
824+
825+
auto sphere = world->GetModel("sphere");
826+
ASSERT_NE(nullptr, sphere);
827+
828+
auto sphereLink = sphere->GetLink("sphere_link");
829+
ASSERT_NE(nullptr, sphereLink);
830+
831+
EXPECT_TRUE(sphere->GetCollisionEnabled());
832+
833+
// Disable collisions and let the sphere fall for long enough that, in the
834+
// presence of collisions, it would have already contacted the box.
835+
sphere->SetCollisionEnabled(false);
836+
EXPECT_FALSE(sphere->GetCollisionEnabled());
837+
838+
const double initialZ =
839+
sphereLink->FrameDataRelativeToWorld().pose.translation().z();
840+
const double afterZ =
841+
StepAndGetPosition<ModelCollisionFeaturesList>(
842+
world, sphereLink, 1000).z();
843+
844+
// Without collisions the sphere must keep falling well below the box top
845+
// (z=0), which would be impossible if collisions were active.
846+
EXPECT_LT(afterZ, 0.0);
847+
EXPECT_LT(afterZ, initialZ);
848+
}
849+
}
850+
735851
// ---------------------------------------------------------------------------
736852
// main
737853
// ---------------------------------------------------------------------------

0 commit comments

Comments
 (0)