Adding angular acceleration to the Link class (#1288)

* Adding angular acceleration. Signed-off-by: Carlos Agüero <[email protected]> * add test Signed-off-by: Ian Chen <[email protected]> * update link api tutorial Signed-off-by: Ian Chen <[email protected]> Co-authored-by: Ian Chen <[email protected]>
gazebosim · Jan 13, 2022 · 5b6ef9b · 5b6ef9b
1 parent 19aba6d
commit 5b6ef9b
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 2 deletions.
diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh
@@ -220,6 +220,14 @@ namespace ignition
       public: void EnableVelocityChecks(EntityComponentManager &_ecm,
           bool _enable = true) 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
+       /// nullopt if acceleration checks aren't enabled.
+       /// \sa EnableAccelerationChecks
+       public: std::optional<math::Vector3d> WorldAngularAcceleration(
+           const EntityComponentManager &_ecm) const;
+
       /// \brief Get the linear acceleration of the body in the world frame.
       /// \param[in] _ecm Entity-component manager.
       /// \return Linear acceleration of the body in the world frame or nullopt

diff --git a/src/Link.cc b/src/Link.cc
@@ -17,6 +17,7 @@
 
 #include <ignition/msgs/Utility.hh>
 
+#include "ignition/gazebo/components/AngularAcceleration.hh"
 #include "ignition/gazebo/components/AngularVelocity.hh"
 #include "ignition/gazebo/components/CanonicalLink.hh"
 #include "ignition/gazebo/components/Collision.hh"
@@ -245,6 +246,14 @@ void Link::EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable)
       _enable);
 }
 
+//////////////////////////////////////////////////
+std::optional<math::Vector3d> Link::WorldAngularAcceleration(
+    const EntityComponentManager &_ecm) const
+{
+  return _ecm.ComponentData<components::WorldAngularAcceleration>(
+      this->dataPtr->id);
+}
+
 //////////////////////////////////////////////////
 std::optional<math::Vector3d> Link::WorldLinearAcceleration(
     const EntityComponentManager &_ecm) const
@@ -257,6 +266,10 @@ std::optional<math::Vector3d> Link::WorldLinearAcceleration(
 void Link::EnableAccelerationChecks(EntityComponentManager &_ecm, bool _enable)
     const
 {
+  enableComponent<components::WorldAngularAcceleration>(_ecm, this->dataPtr->id,
+      _enable);
+  enableComponent<components::AngularAcceleration>(_ecm, this->dataPtr->id,
+      _enable);
   enableComponent<components::WorldLinearAcceleration>(_ecm, this->dataPtr->id,
       _enable);
   enableComponent<components::LinearAcceleration>(_ecm, this->dataPtr->id,

diff --git a/test/integration/link.cc b/test/integration/link.cc
@@ -20,6 +20,7 @@
 #include <ignition/common/Console.hh>
 #include <ignition/common/Util.hh>
 
+#include <ignition/gazebo/components/AngularAcceleration.hh>
 #include <ignition/gazebo/components/AngularVelocity.hh>
 #include <ignition/gazebo/components/CanonicalLink.hh>
 #include <ignition/gazebo/components/Collision.hh>
@@ -315,23 +316,33 @@ TEST_F(LinkIntegrationTest, LinkAccelerations)
 
   // Before we enable acceleration, acceleration should return nullopt
   EXPECT_EQ(std::nullopt, link.WorldLinearAcceleration(ecm));
+  EXPECT_EQ(std::nullopt, link.WorldAngularAcceleration(ecm));
 
   // After enabling, they should return default values
   link.EnableAccelerationChecks(ecm);
   EXPECT_EQ(math::Vector3d::Zero, link.WorldLinearAcceleration(ecm));
+  EXPECT_EQ(math::Vector3d::Zero, link.WorldAngularAcceleration(ecm));
   EXPECT_NE(nullptr, ecm.Component<components::WorldLinearAcceleration>(eLink));
+  EXPECT_NE(nullptr,
+      ecm.Component<components::WorldAngularAcceleration>(eLink));
 
   // After setting acceleration, we get the value
   math::Vector3d linAccel{1.0, 0.0, 0.0};
   ecm.SetComponentData<components::WorldLinearAcceleration>(eLink, linAccel);
-
   EXPECT_EQ(linAccel, link.WorldLinearAcceleration(ecm));
 
+  math::Vector3d angAccel{0.0, 1.0, 0.0};
+  ecm.SetComponentData<components::WorldAngularAcceleration>(eLink, angAccel);
+  EXPECT_EQ(angAccel, link.WorldAngularAcceleration(ecm));
+
   // Disabling accelerations goes back to nullopt
   link.EnableAccelerationChecks(ecm, false);
 
   EXPECT_EQ(std::nullopt, link.WorldLinearAcceleration(ecm));
+  EXPECT_EQ(std::nullopt, link.WorldAngularAcceleration(ecm));
   EXPECT_EQ(nullptr, ecm.Component<components::WorldLinearAcceleration>(eLink));
+  EXPECT_EQ(nullptr,
+      ecm.Component<components::WorldAngularAcceleration>(eLink));
 }
 
 //////////////////////////////////////////////////

diff --git a/tutorials/migration_link_api.md b/tutorials/migration_link_api.md
@@ -141,7 +141,7 @@ URI | TODO
 UpdateParameters | TODO
 VisualPose | See visual API
 WindMode | `ignition::gazebo::Link::WindMode`
-WorldAngularAccel | TODO
+WorldAngularAccel | `ignition::gazebo::Link::WorldAngularAcceleration`
 WorldAngularMomentum | TODO
 WorldAngularVel | `ignition::gazebo::Link::WorldAngularVelocity`
 WorldCoGLinearVel | TODO