From 6b3f196ae8655ca54e59b2b580f574d847096458 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Fri, 30 Jun 2023 17:54:58 +0200 Subject: [PATCH 1/6] Add force offset support to ApplyLinkWrench system and to Link API Signed-off-by: Henrique-BO --- include/gz/sim/Link.hh | 13 ++++++ src/Link.cc | 44 +++++++++++++++++++ .../apply_link_wrench/ApplyLinkWrench.cc | 20 +++++---- 3 files changed, 69 insertions(+), 8 deletions(-) diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index f54f432630..7ca27cdc1f 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -306,6 +306,19 @@ namespace gz const math::Vector3d &_force, const math::Vector3d &_torque) const; + /// \brief Add a wrench expressed in world coordinates and applied to + /// the link at the given offset from its center of mass. This wrench + /// is applied for one simulation step. + /// \param[in] _ecm Mutable Entity-component manager. + /// \param[in] _force Force to be applied expressed in world coordinates + /// \param[in] _offset The point of application of the force expressed + /// in the link-fixed frame and relative to the center of mass. + /// \param[in] _torque Torque to be applied expressed in world coordinates + public: void AddWorldWrench(EntityComponentManager &_ecm, + const math::Vector3d &_force, + const math::Vector3d &_offset, + const math::Vector3d &_torque) const; + /// \brief Pointer to private data. private: std::unique_ptr dataPtr; }; diff --git a/src/Link.cc b/src/Link.cc index bf58837f29..522fec40b3 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -432,3 +432,47 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm, msgs::Convert(linkWrenchComp->Data().torque()) + _torque); } } + +////////////////////////////////////////////////// +void Link::AddWorldWrench(EntityComponentManager &_ecm, + const math::Vector3d &_force, + const math::Vector3d &_offset, + const math::Vector3d &_torque) const +{ + auto inertial = _ecm.Component(this->dataPtr->id); + math::Pose3d linkWorldPose = worldPose(this->dataPtr->id, _ecm); + + // Can't apply force if the inertial's pose is not found + if (!inertial) + { + gzdbg << "Inertial Component not found" << std::endl; + return; + } + + // We want the force to be applied at an offset from the center of mass, but + // ExternalWorldWrenchCmd applies the force at the link origin so we need to + // compute the resulting force and torque on the link origin. + auto posComWorldCoord = linkWorldPose.Rot().RotateVector( + _offset + inertial->Data().Pose().Pos()); + + math::Vector3d torqueWithOffset = _torque + posComWorldCoord.Cross(_force); + + auto linkWrenchComp = + _ecm.Component(this->dataPtr->id); + + if (!linkWrenchComp) + { + components::ExternalWorldWrenchCmd wrench; + msgs::Set(wrench.Data().mutable_force(), _force); + msgs::Set(wrench.Data().mutable_torque(), torqueWithOffset); + _ecm.CreateComponent(this->dataPtr->id, wrench); + } + else + { + msgs::Set(linkWrenchComp->Data().mutable_force(), + msgs::Convert(linkWrenchComp->Data().force()) + _force); + + msgs::Set(linkWrenchComp->Data().mutable_torque(), + msgs::Convert(linkWrenchComp->Data().torque()) + torqueWithOffset); + } +} diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index 084d3b71cd..4d0b30a344 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -80,16 +80,17 @@ class gz::sim::systems::ApplyLinkWrenchPrivate /// \param[in] _msg Entity message. If it's a link, that link is returned. If /// it's a model, its canonical link is returned. /// \param[out] Force to apply. +/// \param[out] Offset of the force application point relative to the +/// link's center of mass. /// \param[out] Torque to apply. /// \return Target link entity. Link decomposeMessage(const EntityComponentManager &_ecm, const msgs::EntityWrench &_msg, math::Vector3d &_force, - math::Vector3d &_torque) + math::Vector3d &_offset, math::Vector3d &_torque) { if (_msg.wrench().has_force_offset()) { - gzwarn << "Force offset currently not supported, it will be ignored." - << std::endl; + _offset = msgs::Convert(_msg.wrench().force_offset()); } if (_msg.wrench().has_force()) @@ -270,8 +271,9 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, auto msg = this->dataPtr->newWrenches.front(); math::Vector3d force; + math::Vector3d offset; math::Vector3d torque; - auto link = decomposeMessage(_ecm, msg, force, torque); + auto link = decomposeMessage(_ecm, msg, force, offset, torque); if (!link.Valid(_ecm)) { gzerr << "Entity not found." << std::endl @@ -280,11 +282,12 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, continue; } - link.AddWorldWrench(_ecm, force, torque); + link.AddWorldWrench(_ecm, force, offset, torque); if (this->dataPtr->verbose) { - gzdbg << "Applying wrench [" << force << " " << torque << "] to entity [" + gzdbg << "Applying wrench [" << force << " " << torque + << "] with force offset [" << offset << "] to entity [" << link.Entity() << "] for 1 time step." << std::endl; } @@ -295,15 +298,16 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, for (auto msg : this->dataPtr->persistentWrenches) { math::Vector3d force; + math::Vector3d offset; math::Vector3d torque; - auto link = decomposeMessage(_ecm, msg, force, torque); + auto link = decomposeMessage(_ecm, msg, force, offset, torque); if (!link.Valid(_ecm)) { // Not an error, persistent wrenches can be applied preemptively before // an entity is inserted continue; } - link.AddWorldWrench(_ecm, force, torque); + link.AddWorldWrench(_ecm, force, offset, torque); } } From 34101f7d6184f415aa2164a602642d2fb3d0aa34 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Thu, 6 Jul 2023 10:58:12 -0300 Subject: [PATCH 2/6] Added additional tests to LinkAddWorldForce integration test Signed-off-by: Henrique-BO --- src/Link.cc | 18 ++++++++++++++---- test/integration/link.cc | 27 +++++++++++++++++++++++++++ 2 files changed, 41 insertions(+), 4 deletions(-) diff --git a/src/Link.cc b/src/Link.cc index 522fec40b3..34c10be331 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -435,12 +435,12 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm, ////////////////////////////////////////////////// void Link::AddWorldWrench(EntityComponentManager &_ecm, - const math::Vector3d &_force, - const math::Vector3d &_offset, - const math::Vector3d &_torque) const + const math::Vector3d &_force, + const math::Vector3d &_offset, + const math::Vector3d &_torque) const { auto inertial = _ecm.Component(this->dataPtr->id); - math::Pose3d linkWorldPose = worldPose(this->dataPtr->id, _ecm); + auto worldPoseComp = _ecm.Component(this->dataPtr->id); // Can't apply force if the inertial's pose is not found if (!inertial) @@ -449,6 +449,16 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm, return; } + math::Pose3d linkWorldPose; + if (worldPoseComp) + { + linkWorldPose = worldPoseComp->Data(); + } + else + { + linkWorldPose = worldPose(this->dataPtr->id, _ecm); + } + // We want the force to be applied at an offset from the center of mass, but // ExternalWorldWrenchCmd applies the force at the link origin so we need to // compute the resulting force and torque on the link origin. diff --git a/test/integration/link.cc b/test/integration/link.cc index fd04e8bddb..cd45cb89c6 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -608,6 +608,33 @@ TEST_F(LinkIntegrationTest, LinkAddWorldForce) EXPECT_NE(nullptr, wrenchComp); wrenchMsg = wrenchComp->Data(); + EXPECT_EQ(math::Vector3d::Zero, math::Vector3d( + wrenchMsg.force().x(), wrenchMsg.force().y(), wrenchMsg.force().z())); + EXPECT_EQ(math::Vector3d::Zero, math::Vector3d( + wrenchMsg.torque().x(), wrenchMsg.torque().y(), wrenchMsg.torque().z())); + + // apply world force at an offset and world torque + math::Vector3d torque{1.0, 0.0, 0.0}; + link.AddWorldWrench(ecm, force, offset, torque); + + wrenchComp = ecm.Component(eLink); + EXPECT_NE(nullptr, wrenchComp); + wrenchMsg = wrenchComp->Data(); + + expectedTorque = + torque + + linkWorldPose.Rot().RotateVector(offset + inertiaPose.Pos()).Cross(force); + EXPECT_EQ(force, math::Vector3d( + wrenchMsg.force().x(), wrenchMsg.force().y(), wrenchMsg.force().z())); + EXPECT_EQ(expectedTorque, math::Vector3d( + wrenchMsg.torque().x(), wrenchMsg.torque().y(), wrenchMsg.torque().z())); + + // apply opposite wrench again and verify the resulting wrench values are zero + link.AddWorldWrench(ecm, -force, offset, -torque); + wrenchComp = ecm.Component(eLink); + EXPECT_NE(nullptr, wrenchComp); + wrenchMsg = wrenchComp->Data(); + EXPECT_EQ(math::Vector3d::Zero, math::Vector3d( wrenchMsg.force().x(), wrenchMsg.force().y(), wrenchMsg.force().z())); EXPECT_EQ(math::Vector3d::Zero, math::Vector3d( From 361cd4fd30ae1ae7be110c57c63215d05482ba78 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Mon, 17 Jul 2023 12:30:43 -0300 Subject: [PATCH 3/6] Rename function to AddWorldWrenchRelativeToCOM Signed-off-by: Henrique-BO --- include/gz/sim/Link.hh | 9 +++++---- src/Link.cc | 8 ++++---- src/systems/apply_link_wrench/ApplyLinkWrench.cc | 4 ++-- test/integration/link.cc | 4 ++-- 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index 7ca27cdc1f..2eed13b158 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -314,10 +314,11 @@ namespace gz /// \param[in] _offset The point of application of the force expressed /// in the link-fixed frame and relative to the center of mass. /// \param[in] _torque Torque to be applied expressed in world coordinates - public: void AddWorldWrench(EntityComponentManager &_ecm, - const math::Vector3d &_force, - const math::Vector3d &_offset, - const math::Vector3d &_torque) const; + public: void AddWorldWrenchRelativeToCOM( + EntityComponentManager &_ecm, + const math::Vector3d &_force, + const math::Vector3d &_offset, + const math::Vector3d &_torque) const; /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/Link.cc b/src/Link.cc index 34c10be331..2a6cd58352 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -434,10 +434,10 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm, } ////////////////////////////////////////////////// -void Link::AddWorldWrench(EntityComponentManager &_ecm, - const math::Vector3d &_force, - const math::Vector3d &_offset, - const math::Vector3d &_torque) const +void Link::AddWorldWrenchRelativeToCOM(EntityComponentManager &_ecm, + const math::Vector3d &_force, + const math::Vector3d &_offset, + const math::Vector3d &_torque) const { auto inertial = _ecm.Component(this->dataPtr->id); auto worldPoseComp = _ecm.Component(this->dataPtr->id); diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index 4d0b30a344..ef57a20428 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -282,7 +282,7 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, continue; } - link.AddWorldWrench(_ecm, force, offset, torque); + link.AddWorldWrenchRelativeToCOM(_ecm, force, offset, torque); if (this->dataPtr->verbose) { @@ -307,7 +307,7 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, // an entity is inserted continue; } - link.AddWorldWrench(_ecm, force, offset, torque); + link.AddWorldWrenchRelativeToCOM(_ecm, force, offset, torque); } } diff --git a/test/integration/link.cc b/test/integration/link.cc index cd45cb89c6..4e6890f8ea 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -615,7 +615,7 @@ TEST_F(LinkIntegrationTest, LinkAddWorldForce) // apply world force at an offset and world torque math::Vector3d torque{1.0, 0.0, 0.0}; - link.AddWorldWrench(ecm, force, offset, torque); + link.AddWorldWrenchRelativeToCOM(ecm, force, offset, torque); wrenchComp = ecm.Component(eLink); EXPECT_NE(nullptr, wrenchComp); @@ -630,7 +630,7 @@ TEST_F(LinkIntegrationTest, LinkAddWorldForce) wrenchMsg.torque().x(), wrenchMsg.torque().y(), wrenchMsg.torque().z())); // apply opposite wrench again and verify the resulting wrench values are zero - link.AddWorldWrench(ecm, -force, offset, -torque); + link.AddWorldWrenchRelativeToCOM(ecm, -force, offset, -torque); wrenchComp = ecm.Component(eLink); EXPECT_NE(nullptr, wrenchComp); wrenchMsg = wrenchComp->Data(); From 336b1196b26df67206d0116b11b9584039bff57a Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Mon, 24 Jul 2023 14:02:13 -0300 Subject: [PATCH 4/6] Using link inertial frame Signed-off-by: Henrique-BO --- include/gz/sim/Link.hh | 2 +- src/Link.cc | 4 ++-- src/systems/apply_link_wrench/ApplyLinkWrench.hh | 4 ++++ test/integration/link.cc | 4 +++- 4 files changed, 10 insertions(+), 4 deletions(-) diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index 2eed13b158..914f982559 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -312,7 +312,7 @@ namespace gz /// \param[in] _ecm Mutable Entity-component manager. /// \param[in] _force Force to be applied expressed in world coordinates /// \param[in] _offset The point of application of the force expressed - /// in the link-fixed frame and relative to the center of mass. + /// in the link's inertial frame and relative to the center of mass /// \param[in] _torque Torque to be applied expressed in world coordinates public: void AddWorldWrenchRelativeToCOM( EntityComponentManager &_ecm, diff --git a/src/Link.cc b/src/Link.cc index 2a6cd58352..45bef352db 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -448,7 +448,6 @@ void Link::AddWorldWrenchRelativeToCOM(EntityComponentManager &_ecm, gzdbg << "Inertial Component not found" << std::endl; return; } - math::Pose3d linkWorldPose; if (worldPoseComp) { @@ -463,7 +462,8 @@ void Link::AddWorldWrenchRelativeToCOM(EntityComponentManager &_ecm, // ExternalWorldWrenchCmd applies the force at the link origin so we need to // compute the resulting force and torque on the link origin. auto posComWorldCoord = linkWorldPose.Rot().RotateVector( - _offset + inertial->Data().Pose().Pos()); + inertial->Data().Pose().Rot().RotateVector(_offset) + + inertial->Data().Pose().Pos()); math::Vector3d torqueWithOffset = _torque + posComWorldCoord.Cross(_force); diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.hh b/src/systems/apply_link_wrench/ApplyLinkWrench.hh index 625db47578..f7b7d668bd 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.hh +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.hh @@ -38,6 +38,10 @@ namespace systems /// that will receive a wrench. If a model is provided, its canonical link /// will receive it. No other entity types are supported. /// + /// Forces and torques are expressed in world coordinates, and the force + /// offset is expressed in the link's inertial frame (i.e., relative to the + /// center of mass) + /// /// ## Topics /// /// * /world//wrench diff --git a/test/integration/link.cc b/test/integration/link.cc index 4e6890f8ea..5f6446e012 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -621,9 +621,11 @@ TEST_F(LinkIntegrationTest, LinkAddWorldForce) EXPECT_NE(nullptr, wrenchComp); wrenchMsg = wrenchComp->Data(); + math::Vector3d posComWorldCoord = linkWorldPose.Rot().RotateVector( + inertiaPose.Rot().RotateVector(offset) + inertiaPose.Pos()); expectedTorque = torque + - linkWorldPose.Rot().RotateVector(offset + inertiaPose.Pos()).Cross(force); + posComWorldCoord.Cross(force); EXPECT_EQ(force, math::Vector3d( wrenchMsg.force().x(), wrenchMsg.force().y(), wrenchMsg.force().z())); EXPECT_EQ(expectedTorque, math::Vector3d( From ee693bc4d68687ec189f1aeeb9c4e2c79fc65425 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Wed, 9 Aug 2023 11:37:03 -0300 Subject: [PATCH 5/6] Use all offsets on link frame Signed-off-by: Henrique-BO --- include/gz/sim/Link.hh | 7 ++--- src/Link.cc | 29 +++++-------------- .../apply_link_wrench/ApplyLinkWrench.cc | 8 ++--- .../apply_link_wrench/ApplyLinkWrench.hh | 3 +- test/integration/link.cc | 11 +++---- 5 files changed, 20 insertions(+), 38 deletions(-) diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index 914f982559..004dc1fb1b 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -307,15 +307,14 @@ namespace gz const math::Vector3d &_torque) const; /// \brief Add a wrench expressed in world coordinates and applied to - /// the link at the given offset from its center of mass. This wrench + /// the link at an offset from the link's origin. This wrench /// is applied for one simulation step. /// \param[in] _ecm Mutable Entity-component manager. /// \param[in] _force Force to be applied expressed in world coordinates /// \param[in] _offset The point of application of the force expressed - /// in the link's inertial frame and relative to the center of mass + /// in the link frame /// \param[in] _torque Torque to be applied expressed in world coordinates - public: void AddWorldWrenchRelativeToCOM( - EntityComponentManager &_ecm, + public: void AddWorldWrench(EntityComponentManager &_ecm, const math::Vector3d &_force, const math::Vector3d &_offset, const math::Vector3d &_torque) const; diff --git a/src/Link.cc b/src/Link.cc index 45bef352db..5610502f38 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -434,21 +434,13 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm, } ////////////////////////////////////////////////// -void Link::AddWorldWrenchRelativeToCOM(EntityComponentManager &_ecm, - const math::Vector3d &_force, - const math::Vector3d &_offset, - const math::Vector3d &_torque) const +void Link::AddWorldWrench(EntityComponentManager &_ecm, + const math::Vector3d &_force, + const math::Vector3d &_offset, + const math::Vector3d &_torque) const { - auto inertial = _ecm.Component(this->dataPtr->id); - auto worldPoseComp = _ecm.Component(this->dataPtr->id); - - // Can't apply force if the inertial's pose is not found - if (!inertial) - { - gzdbg << "Inertial Component not found" << std::endl; - return; - } math::Pose3d linkWorldPose; + auto worldPoseComp = _ecm.Component(this->dataPtr->id); if (worldPoseComp) { linkWorldPose = worldPoseComp->Data(); @@ -458,18 +450,13 @@ void Link::AddWorldWrenchRelativeToCOM(EntityComponentManager &_ecm, linkWorldPose = worldPose(this->dataPtr->id, _ecm); } - // We want the force to be applied at an offset from the center of mass, but - // ExternalWorldWrenchCmd applies the force at the link origin so we need to - // compute the resulting force and torque on the link origin. - auto posComWorldCoord = linkWorldPose.Rot().RotateVector( - inertial->Data().Pose().Rot().RotateVector(_offset) + - inertial->Data().Pose().Pos()); - + // We want the force to be applied at an offset from the link origin, so we + // must compute the resulting force and torque on the link origin. + auto posComWorldCoord = linkWorldPose.Rot().RotateVector(_offset); math::Vector3d torqueWithOffset = _torque + posComWorldCoord.Cross(_force); auto linkWrenchComp = _ecm.Component(this->dataPtr->id); - if (!linkWrenchComp) { components::ExternalWorldWrenchCmd wrench; diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index ef57a20428..2d943cdbd2 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -80,8 +80,8 @@ class gz::sim::systems::ApplyLinkWrenchPrivate /// \param[in] _msg Entity message. If it's a link, that link is returned. If /// it's a model, its canonical link is returned. /// \param[out] Force to apply. -/// \param[out] Offset of the force application point relative to the -/// link's center of mass. +/// \param[out] Offset of the force application point expressed in the link +/// frame. /// \param[out] Torque to apply. /// \return Target link entity. Link decomposeMessage(const EntityComponentManager &_ecm, @@ -282,7 +282,7 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, continue; } - link.AddWorldWrenchRelativeToCOM(_ecm, force, offset, torque); + link.AddWorldWrench(_ecm, force, offset, torque); if (this->dataPtr->verbose) { @@ -307,7 +307,7 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, // an entity is inserted continue; } - link.AddWorldWrenchRelativeToCOM(_ecm, force, offset, torque); + link.AddWorldWrench(_ecm, force, offset, torque); } } diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.hh b/src/systems/apply_link_wrench/ApplyLinkWrench.hh index f7b7d668bd..f2c449e51f 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.hh +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.hh @@ -39,8 +39,7 @@ namespace systems /// will receive it. No other entity types are supported. /// /// Forces and torques are expressed in world coordinates, and the force - /// offset is expressed in the link's inertial frame (i.e., relative to the - /// center of mass) + /// offset is expressed in the link frame. /// /// ## Topics /// diff --git a/test/integration/link.cc b/test/integration/link.cc index 5f6446e012..42a14dc807 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -615,24 +615,21 @@ TEST_F(LinkIntegrationTest, LinkAddWorldForce) // apply world force at an offset and world torque math::Vector3d torque{1.0, 0.0, 0.0}; - link.AddWorldWrenchRelativeToCOM(ecm, force, offset, torque); + link.AddWorldWrench(ecm, force, offset, torque); wrenchComp = ecm.Component(eLink); EXPECT_NE(nullptr, wrenchComp); wrenchMsg = wrenchComp->Data(); - math::Vector3d posComWorldCoord = linkWorldPose.Rot().RotateVector( - inertiaPose.Rot().RotateVector(offset) + inertiaPose.Pos()); - expectedTorque = - torque + - posComWorldCoord.Cross(force); + math::Vector3d posComWorldCoord = linkWorldPose.Rot().RotateVector(offset); + expectedTorque = torque + posComWorldCoord.Cross(force); EXPECT_EQ(force, math::Vector3d( wrenchMsg.force().x(), wrenchMsg.force().y(), wrenchMsg.force().z())); EXPECT_EQ(expectedTorque, math::Vector3d( wrenchMsg.torque().x(), wrenchMsg.torque().y(), wrenchMsg.torque().z())); // apply opposite wrench again and verify the resulting wrench values are zero - link.AddWorldWrenchRelativeToCOM(ecm, -force, offset, -torque); + link.AddWorldWrench(ecm, -force, offset, -torque); wrenchComp = ecm.Component(eLink); EXPECT_NE(nullptr, wrenchComp); wrenchMsg = wrenchComp->Data(); From f36ac6908d157e7136c76884bde0f85873c4d41f Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Wed, 9 Aug 2023 14:54:33 -0300 Subject: [PATCH 6/6] Reorder function parameters Signed-off-by: Henrique-BO --- include/gz/sim/Link.hh | 8 +++---- src/Link.cc | 24 +++---------------- .../apply_link_wrench/ApplyLinkWrench.cc | 12 +++++----- test/integration/link.cc | 4 ++-- 4 files changed, 15 insertions(+), 33 deletions(-) diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index 004dc1fb1b..d67a17d4de 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -311,13 +311,13 @@ namespace gz /// is applied for one simulation step. /// \param[in] _ecm Mutable Entity-component manager. /// \param[in] _force Force to be applied expressed in world coordinates + /// \param[in] _torque Torque to be applied expressed in world coordinates /// \param[in] _offset The point of application of the force expressed /// in the link frame - /// \param[in] _torque Torque to be applied expressed in world coordinates public: void AddWorldWrench(EntityComponentManager &_ecm, - const math::Vector3d &_force, - const math::Vector3d &_offset, - const math::Vector3d &_torque) const; + const math::Vector3d &_force, + const math::Vector3d &_torque, + const math::Vector3d &_offset) const; /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/Link.cc b/src/Link.cc index 5610502f38..8378661dbe 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -412,32 +412,14 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm, const math::Vector3d &_force, const math::Vector3d &_torque) const { - auto linkWrenchComp = - _ecm.Component(this->dataPtr->id); - - components::ExternalWorldWrenchCmd wrench; - - if (!linkWrenchComp) - { - msgs::Set(wrench.Data().mutable_force(), _force); - msgs::Set(wrench.Data().mutable_torque(), _torque); - _ecm.CreateComponent(this->dataPtr->id, wrench); - } - else - { - msgs::Set(linkWrenchComp->Data().mutable_force(), - msgs::Convert(linkWrenchComp->Data().force()) + _force); - - msgs::Set(linkWrenchComp->Data().mutable_torque(), - msgs::Convert(linkWrenchComp->Data().torque()) + _torque); - } + this->AddWorldWrench(_ecm, _force, _torque, math::Vector3d::Zero); } ////////////////////////////////////////////////// void Link::AddWorldWrench(EntityComponentManager &_ecm, const math::Vector3d &_force, - const math::Vector3d &_offset, - const math::Vector3d &_torque) const + const math::Vector3d &_torque, + const math::Vector3d &_offset) const { math::Pose3d linkWorldPose; auto worldPoseComp = _ecm.Component(this->dataPtr->id); diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index 2d943cdbd2..6ecf5d7d3b 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -80,13 +80,13 @@ class gz::sim::systems::ApplyLinkWrenchPrivate /// \param[in] _msg Entity message. If it's a link, that link is returned. If /// it's a model, its canonical link is returned. /// \param[out] Force to apply. +/// \param[out] Torque to apply. /// \param[out] Offset of the force application point expressed in the link /// frame. -/// \param[out] Torque to apply. /// \return Target link entity. Link decomposeMessage(const EntityComponentManager &_ecm, const msgs::EntityWrench &_msg, math::Vector3d &_force, - math::Vector3d &_offset, math::Vector3d &_torque) + math::Vector3d &_torque, math::Vector3d &_offset) { if (_msg.wrench().has_force_offset()) { @@ -273,7 +273,7 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, math::Vector3d force; math::Vector3d offset; math::Vector3d torque; - auto link = decomposeMessage(_ecm, msg, force, offset, torque); + auto link = decomposeMessage(_ecm, msg, force, torque, offset); if (!link.Valid(_ecm)) { gzerr << "Entity not found." << std::endl @@ -282,7 +282,7 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, continue; } - link.AddWorldWrench(_ecm, force, offset, torque); + link.AddWorldWrench(_ecm, force, torque, offset); if (this->dataPtr->verbose) { @@ -300,14 +300,14 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, math::Vector3d force; math::Vector3d offset; math::Vector3d torque; - auto link = decomposeMessage(_ecm, msg, force, offset, torque); + auto link = decomposeMessage(_ecm, msg, force, torque, offset); if (!link.Valid(_ecm)) { // Not an error, persistent wrenches can be applied preemptively before // an entity is inserted continue; } - link.AddWorldWrench(_ecm, force, offset, torque); + link.AddWorldWrench(_ecm, force, torque, offset); } } diff --git a/test/integration/link.cc b/test/integration/link.cc index 42a14dc807..10063b247a 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -615,7 +615,7 @@ TEST_F(LinkIntegrationTest, LinkAddWorldForce) // apply world force at an offset and world torque math::Vector3d torque{1.0, 0.0, 0.0}; - link.AddWorldWrench(ecm, force, offset, torque); + link.AddWorldWrench(ecm, force, torque, offset); wrenchComp = ecm.Component(eLink); EXPECT_NE(nullptr, wrenchComp); @@ -629,7 +629,7 @@ TEST_F(LinkIntegrationTest, LinkAddWorldForce) wrenchMsg.torque().x(), wrenchMsg.torque().y(), wrenchMsg.torque().z())); // apply opposite wrench again and verify the resulting wrench values are zero - link.AddWorldWrench(ecm, -force, offset, -torque); + link.AddWorldWrench(ecm, -force, -torque, offset); wrenchComp = ecm.Component(eLink); EXPECT_NE(nullptr, wrenchComp); wrenchMsg = wrenchComp->Data();