diff --git a/src/Link.cc b/src/Link.cc index 659b25cbe..085feab93 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -987,6 +987,11 @@ sdf::ElementPtr Link::ToElement() const inertiaElem->GetElement("iyz")->Set(massMatrix.Iyz()); inertiaElem->GetElement("izz")->Set(massMatrix.Izz()); + if (this->dataPtr->density.has_value()) + { + inertialElem->GetElement("density")->Set(*this->dataPtr->density); + } + if (this->dataPtr->inertial.FluidAddedMass().has_value()) { auto addedMass = this->dataPtr->inertial.FluidAddedMass().value(); diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 931fe0977..2182ff2e3 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -984,12 +984,18 @@ TEST(DOMLink, ToElement) sdf::ElementPtr elem = link.ToElement(); ASSERT_NE(nullptr, elem); + // Expect no density element + { + auto inertialElem = elem->FindElement("inertial"); + EXPECT_FALSE(inertialElem->HasElement("density")); + } sdf::Link link2; link2.Load(elem); EXPECT_EQ(link.Name(), link2.Name()); EXPECT_EQ(link.Inertial(), link2.Inertial()); + EXPECT_EQ(link.Density(), link2.Density()); EXPECT_EQ(link.RawPose(), link2.RawPose()); EXPECT_EQ(link.EnableWind(), link2.EnableWind()); EXPECT_EQ(link.CollisionCount(), link2.CollisionCount()); @@ -1015,6 +1021,23 @@ TEST(DOMLink, ToElement) EXPECT_EQ(link.ProjectorCount(), link2.ProjectorCount()); for (uint64_t i = 0; i < link2.ProjectorCount(); ++i) EXPECT_NE(nullptr, link2.ProjectorByIndex(i)); + + // Now set density in link + const double kDensity = 1234.5; + link.SetDensity(kDensity); + sdf::ElementPtr elemWithDensity = link.ToElement(); + ASSERT_NE(nullptr, elemWithDensity); + // Expect density element + { + auto inertialElem = elemWithDensity->FindElement("inertial"); + ASSERT_TRUE(inertialElem->HasElement("density")); + EXPECT_DOUBLE_EQ(kDensity, inertialElem->Get("density")); + } + + sdf::Link link3; + link3.Load(elemWithDensity); + ASSERT_TRUE(link3.Density().has_value()); + EXPECT_DOUBLE_EQ(kDensity, *link3.Density()); } /////////////////////////////////////////////////