From fe3a501feb64f71e213fa9aad9c5e3f636f32ee8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 12 Jan 2022 21:13:03 +0100 Subject: [PATCH 1/3] Adding angular acceleration. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/Link.hh | 8 ++++++++ src/Link.cc | 13 +++++++++++++ 2 files changed, 21 insertions(+) diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh index 8d7f228417..80cb33e205 100644 --- 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 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 index 41f814cd88..6e1d39d4c6 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -17,6 +17,7 @@ #include +#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 Link::WorldAngularAcceleration( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + ////////////////////////////////////////////////// std::optional Link::WorldLinearAcceleration( const EntityComponentManager &_ecm) const @@ -257,6 +266,10 @@ std::optional Link::WorldLinearAcceleration( void Link::EnableAccelerationChecks(EntityComponentManager &_ecm, bool _enable) const { + enableComponent(_ecm, this->dataPtr->id, + _enable); + enableComponent(_ecm, this->dataPtr->id, + _enable); enableComponent(_ecm, this->dataPtr->id, _enable); enableComponent(_ecm, this->dataPtr->id, From 73afcaf3cba75e62c9b5d61ce86da749bd7bc88c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 12 Jan 2022 15:00:02 -0800 Subject: [PATCH 2/3] add test Signed-off-by: Ian Chen --- test/integration/link.cc | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/test/integration/link.cc b/test/integration/link.cc index 72b361635b..2584d1de5c 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -314,23 +315,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(eLink)); + EXPECT_NE(nullptr, + ecm.Component(eLink)); // After setting acceleration, we get the value math::Vector3d linAccel{1.0, 0.0, 0.0}; ecm.SetComponentData(eLink, linAccel); - EXPECT_EQ(linAccel, link.WorldLinearAcceleration(ecm)); + math::Vector3d angAccel{0.0, 1.0, 0.0}; + ecm.SetComponentData(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(eLink)); + EXPECT_EQ(nullptr, + ecm.Component(eLink)); } ////////////////////////////////////////////////// From da78b81de9f4028d86919a93ac01f08515218927 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 12 Jan 2022 20:40:35 -0800 Subject: [PATCH 3/3] update link api tutorial Signed-off-by: Ian Chen --- tutorials/migration_link_api.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorials/migration_link_api.md b/tutorials/migration_link_api.md index fc5f6c8349..5d290a0f03 100644 --- 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