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)
 {