From 28e212cdd77ff4427aa87963c3720d3c12127d05 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 7 Feb 2022 13:11:02 +0800 Subject: [PATCH 1/2] Adds a `Link::SetLinearVelocity()` method This PR adds a `Link::SetLinearVelocity()` method. I forsee this method being useful for testing behaviour of systems like the hydrodynamics or liftDrag plugin which are dependent on velocity for their output forces. Signed-off-by: Arjo Chakravarty --- include/ignition/gazebo/Link.hh | 7 +++++++ src/Link.cc | 20 +++++++++++++++++++ test/integration/link.cc | 34 +++++++++++++++++++++++++++++++++ 3 files changed, 61 insertions(+) diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh index 094567951b..a9fb440252 100644 --- a/include/ignition/gazebo/Link.hh +++ b/include/ignition/gazebo/Link.hh @@ -220,6 +220,13 @@ namespace ignition public: void EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable = true) const; + /// \brief Set the linear velocity on this link. If this is set wrenches + /// on this link will be ignored for the current time step. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _vel Linear velocity to set. + public: void SetLinearVelocity(EntityComponentManager &_ecm, + const math::Vector3d &_vel) const; + /// \brief Get the angular acceleration of the body in the world frame. /// \param[in] _ecm Entity-component manager. /// \return Angular acceleration of the body in the world frame or diff --git a/src/Link.cc b/src/Link.cc index 855adf4352..0d07f17140 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -26,6 +26,7 @@ #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/LinearAcceleration.hh" #include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/LinearVelocityCmd.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" @@ -246,6 +247,25 @@ void Link::EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable) _enable); } +////////////////////////////////////////////////// +void Link::SetLinearVelocity(EntityComponentManager &_ecm, + const math::Vector3d &_vel) const +{ + auto vel = + _ecm.Component(this->dataPtr->id); + + if (vel == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LinearVelocityCmd(_vel)); + } + else + { + vel->Data() = _vel; + } +} + ////////////////////////////////////////////////// std::optional Link::WorldAngularAcceleration( const EntityComponentManager &_ecm) const diff --git a/test/integration/link.cc b/test/integration/link.cc index da64000838..53a1dc9f7c 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -468,6 +469,39 @@ TEST_F(LinkIntegrationTest, LinkKineticEnergy) EXPECT_DOUBLE_EQ(100.0, *link.WorldKineticEnergy(ecm)); } +////////////////////////////////////////////////// +TEST_F(LinkIntegrationTest, LinkSetVelocity) +{ + EntityComponentManager ecm; + EventManager eventMgr; + SdfEntityCreator creator(ecm, eventMgr); + + auto eLink = ecm.CreateEntity(); + ecm.CreateComponent(eLink, components::Link()); + + Link link(eLink); + EXPECT_EQ(eLink, link.Entity()); + + ASSERT_TRUE(link.Valid(ecm)); + + // No LinearVelocityCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLink)); + + math::Vector3d linVel(1, 0, 0); + link.SetLinearVelocity(ecm, linVel); + + // LinearVelocity cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLink)); + EXPECT_EQ(linVel, + ecm.Component(eLink)->Data()); + + // Make sure the linear velocity is updated + math::Vector3d linVel2(0, 0, 0); + link.SetLinearVelocity(ecm, linVel2); + EXPECT_EQ(linVel2, + ecm.Component(eLink)->Data()); +} + ////////////////////////////////////////////////// TEST_F(LinkIntegrationTest, LinkAddWorldForce) { From 6ab486f4ac18801daf5e8853aec1a5b018e860fa Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 8 Feb 2022 09:48:10 +0800 Subject: [PATCH 2/2] Updated docstring Signed-off-by: Arjo Chakravarty --- include/ignition/gazebo/Link.hh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh index 108888fc22..9dd3d5ccc7 100644 --- a/include/ignition/gazebo/Link.hh +++ b/include/ignition/gazebo/Link.hh @@ -220,10 +220,10 @@ namespace ignition public: void EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable = true) const; - /// \brief Set the linear velocity on this link. If this is set wrenches + /// \brief Set the linear velocity on this link. If this is set, wrenches /// on this link will be ignored for the current time step. - /// \param[in] _ecm Entity-component manager. - /// \param[in] _vel Linear velocity to set. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _vel Linear velocity to set in Link's Frame. public: void SetLinearVelocity(EntityComponentManager &_ecm, const math::Vector3d &_vel) const;