diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 3e5260e73..3f433598d 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -414,6 +414,14 @@ namespace sdf /// \sa Model::SetEnableWind(bool) public: void SetEnableWind(bool _enableWind); + /// \brief Check if this link is kinematic only + /// \return true if the link is kinematic only, false otherwise. + public: bool Kinematic() const; + + /// \brief Set whether this link is kinematic only. + /// \param[in] _kinematic True to make the link kinematic only, + public: void SetKinematic(bool _kinematic); + /// \brief Check if the automatic calculation for the link inertial /// is enabled or not. /// \return True if automatic calculation is enabled. This can be done diff --git a/src/Link.cc b/src/Link.cc index 085feab93..892cf3643 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -87,6 +87,9 @@ class sdf::Link::Implementation /// \brief True if this link should be subject to wind, false otherwise. public: bool enableWind = false; + /// \brief True if this link is kinematic only + public: bool kinematic = false; + /// \brief True if automatic caluclation for the link inertial is enabled public: bool autoInertia = false; @@ -313,6 +316,9 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) this->dataPtr->enableWind = _sdf->Get("enable_wind", this->dataPtr->enableWind).first; + this->dataPtr->kinematic = _sdf->Get("kinematic", + this->dataPtr->kinematic).first; + return errors; } @@ -841,6 +847,18 @@ void Link::SetEnableWind(const bool _enableWind) this->dataPtr->enableWind = _enableWind; } +///////////////////////////////////////////////// +bool Link::Kinematic() const +{ + return this->dataPtr->kinematic; +} + +///////////////////////////////////////////////// +void Link::SetKinematic(bool _kinematic) +{ + this->dataPtr->kinematic = _kinematic; +} + ///////////////////////////////////////////////// bool Link::AutoInertia() const { @@ -1022,6 +1040,9 @@ sdf::ElementPtr Link::ToElement() const // wind mode elem->GetElement("enable_wind")->Set(this->EnableWind()); + // kinematic + elem->GetElement("kinematic")->Set(this->Kinematic()); + // Collisions for (const sdf::Collision &collision : this->dataPtr->collisions) { diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 2182ff2e3..ed4853fe8 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -72,6 +72,10 @@ TEST(DOMLink, Construction) link.SetEnableWind(true); EXPECT_TRUE(link.EnableWind()); + EXPECT_FALSE(link.Kinematic()); + link.SetKinematic(true); + EXPECT_TRUE(link.Kinematic()); + EXPECT_FALSE(link.AutoInertiaSaved()); link.SetAutoInertiaSaved(true); EXPECT_TRUE(link.AutoInertiaSaved()); @@ -902,6 +906,7 @@ TEST(DOMLink, ToElement) EXPECT_TRUE(link.SetInertial(inertial)); link.SetRawPose(gz::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); link.SetEnableWind(true); + link.SetKinematic(true); for (int j = 0; j <= 1; ++j) { @@ -998,6 +1003,7 @@ TEST(DOMLink, ToElement) EXPECT_EQ(link.Density(), link2.Density()); EXPECT_EQ(link.RawPose(), link2.RawPose()); EXPECT_EQ(link.EnableWind(), link2.EnableWind()); + EXPECT_EQ(link.Kinematic(), link2.Kinematic()); EXPECT_EQ(link.CollisionCount(), link2.CollisionCount()); for (uint64_t i = 0; i < link2.CollisionCount(); ++i) EXPECT_NE(nullptr, link2.CollisionByIndex(i)); diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 9917067d5..31d493fe9 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -147,6 +147,7 @@ TEST(DOMLink, InertialDoublePendulum) upperLink->RawPose()); EXPECT_EQ("", upperLink->PoseRelativeTo()); EXPECT_TRUE(upperLink->EnableWind()); + EXPECT_TRUE(upperLink->Kinematic()); const gz::math::Inertiald inertialUpper = upperLink->Inertial(); EXPECT_DOUBLE_EQ(1.0, inertialUpper.MassMatrix().Mass()); diff --git a/test/sdf/double_pendulum.sdf b/test/sdf/double_pendulum.sdf index b6339f6e3..3b2979022 100644 --- a/test/sdf/double_pendulum.sdf +++ b/test/sdf/double_pendulum.sdf @@ -57,6 +57,7 @@ true + true 0 0 2.1 -1.5708 0 0 0