Skip to content

Commit

Permalink
add parsing and test for gravity
Browse files Browse the repository at this point in the history
  • Loading branch information
AzulRadio committed May 8, 2024
1 parent 9101e86 commit 52f1d2e
Show file tree
Hide file tree
Showing 3 changed files with 35 additions and 0 deletions.
12 changes: 12 additions & 0 deletions include/sdf/Link.hh
Original file line number Diff line number Diff line change
Expand Up @@ -321,12 +321,24 @@ namespace sdf
/// \sa bool Model::EnableWind
public: bool EnableWind() const;

/// \brief Check if this link should be subject to gravity.
/// If true, this link should be affected by gravity.
/// \return true if the model should be subject to gravity, false otherwise.
/// \sa bool Model::EnableGravity
public: bool EnableGravity() const;

/// \brief Set whether this link should be subject to wind.
/// \param[in] _enableWind True or false depending on whether the link
/// should be subject to wind.
/// \sa Model::SetEnableWind(bool)
public: void SetEnableWind(bool _enableWind);

/// \brief Set whether this link should be subject to gravity.
/// \param[in] _enableGravity True or false depending on whether the link
/// should be subject to gravity.
/// \sa Model::SetEnableGravity(bool)
public: void SetEnableGravity(bool _enableGravity);

/// \brief Add a collision to the link.
/// \param[in] _collision Collision to add.
/// \return True if successful, false if a collision with the name already
Expand Down
19 changes: 19 additions & 0 deletions src/Link.cc
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,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 should be subject to gravity, false otherwise.
public: bool enableGravity = true;

/// \brief Scoped Pose Relative-To graph at the parent model scope.
public: sdf::ScopedGraph<sdf::PoseRelativeToGraph> poseRelativeToGraph;
};
Expand Down Expand Up @@ -189,6 +192,9 @@ Errors Link::Load(ElementPtr _sdf)
this->dataPtr->enableWind = _sdf->Get<bool>("enable_wind",
this->dataPtr->enableWind).first;

this->dataPtr->enableGravity = _sdf->Get<bool>("gravity",
this->dataPtr->enableGravity).first;

return errors;
}

Expand Down Expand Up @@ -578,12 +584,25 @@ bool Link::EnableWind() const
return this->dataPtr->enableWind;
}


/////////////////////////////////////////////////
bool Link::EnableGravity() const
{
return this->dataPtr->enableGravity;
}

/////////////////////////////////////////////////
void Link::SetEnableWind(const bool _enableWind)
{
this->dataPtr->enableWind = _enableWind;
}

/////////////////////////////////////////////////
void Link::SetEnableGravity(const bool _enableGravity)
{
this->dataPtr->enableGravity = _enableGravity;
}

//////////////////////////////////////////////////
bool Link::AddCollision(const Collision &_collision)
{
Expand Down
4 changes: 4 additions & 0 deletions src/Link_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,10 @@ TEST(DOMLink, Construction)
link.SetEnableWind(true);
EXPECT_TRUE(link.EnableWind());

EXPECT_TRUE(link.EnableGravity());
link.SetEnableGravity(false);
EXPECT_FALSE(link.EnableGravity());

EXPECT_EQ(0u, link.SensorCount());
EXPECT_EQ(nullptr, link.SensorByIndex(0));
EXPECT_EQ(nullptr, link.SensorByIndex(1));
Expand Down

0 comments on commit 52f1d2e

Please sign in to comment.