diff --git a/src/Collision.cc b/src/Collision.cc index 05ca351bf..ca944712d 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -351,6 +351,7 @@ sdf::ElementPtr Collision::Element() const return this->dataPtr->sdf; } +///////////////////////////////////////////////// sdf::ElementPtr Collision::ToElement() const { sdf::Errors errors; @@ -377,8 +378,11 @@ sdf::ElementPtr Collision::ToElement(sdf::Errors &_errors) const poseElem->Set(_errors, this->RawPose()); // Set the density - sdf::ElementPtr densityElem = elem->GetElement("density", _errors); - densityElem->Set(this->Density()); + if (this->dataPtr->density.has_value()) + { + sdf::ElementPtr densityElem = elem->GetElement("density", _errors); + densityElem->Set(this->Density()); + } // Set the geometry elem->InsertElement(this->dataPtr->geom.ToElement(_errors), true); diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index c6076c859..1ab3d1b18 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -421,6 +421,8 @@ TEST(DOMCollision, ToElement) sdf::ElementPtr elem = collision.ToElement(); ASSERT_NE(nullptr, elem); + // Expect no density element + EXPECT_FALSE(elem->HasElement("density")); sdf::Collision collision2; collision2.Load(elem); @@ -435,6 +437,19 @@ TEST(DOMCollision, ToElement) ASSERT_NE(nullptr, surface2->Friction()); ASSERT_NE(nullptr, surface2->Friction()->ODE()); EXPECT_DOUBLE_EQ(1.23, surface2->Friction()->ODE()->Mu()); + + // Now set density in collision + const double kDensity = 1234.5; + collision.SetDensity(kDensity); + sdf::ElementPtr elemWithDensity = collision.ToElement(); + ASSERT_NE(nullptr, elemWithDensity); + // Expect density element + ASSERT_TRUE(elemWithDensity->HasElement("density")); + EXPECT_DOUBLE_EQ(kDensity, elemWithDensity->Get("density")); + + sdf::Collision collision3; + collision3.Load(elem); + EXPECT_DOUBLE_EQ(kDensity, collision.Density()); } /////////////////////////////////////////////////