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 <[email protected]>
gazebosim · Feb 7, 2022 · 28e212c · 28e212c
1 parent 717a7e9
commit 28e212c
Show file tree

Hide file tree

Showing 3 changed files with 61 additions and 0 deletions.
diff --git 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
@@ -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<components::LinearVelocityCmd>(this->dataPtr->id);
+
+    if (vel == nullptr)
+    {
+      _ecm.CreateComponent(
+          this->dataPtr->id,
+          components::LinearVelocityCmd(_vel));
+    }
+    else
+    {
+      vel->Data() = _vel;
+    }
+}
+
 //////////////////////////////////////////////////
 std::optional<math::Vector3d> Link::WorldAngularAcceleration(
     const EntityComponentManager &_ecm) const

diff --git a/test/integration/link.cc b/test/integration/link.cc
@@ -29,6 +29,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>
@@ -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<components::LinearVelocityCmd>(eLink));
+
+  math::Vector3d linVel(1, 0, 0);
+  link.SetLinearVelocity(ecm, linVel);
+
+  // LinearVelocity cmd should exist
+  EXPECT_NE(nullptr, ecm.Component<components::LinearVelocityCmd>(eLink));
+  EXPECT_EQ(linVel,
+    ecm.Component<components::LinearVelocityCmd>(eLink)->Data());
+
+  // Make sure the linear velocity is updated
+  math::Vector3d linVel2(0, 0, 0);
+  link.SetLinearVelocity(ecm, linVel2);
+  EXPECT_EQ(linVel2,
+    ecm.Component<components::LinearVelocityCmd>(eLink)->Data());
+}
+
 //////////////////////////////////////////////////
 TEST_F(LinkIntegrationTest, LinkAddWorldForce)
 {