diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index f54f432630..d67a17d4de 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 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] _torque Torque to be applied expressed in world coordinates + /// \param[in] _offset The point of application of the force expressed + /// in the link frame + public: void AddWorldWrench(EntityComponentManager &_ecm, + 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 bf58837f29..8378661dbe 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -412,23 +412,46 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm, const math::Vector3d &_force, const math::Vector3d &_torque) const { - auto linkWrenchComp = - _ecm.Component(this->dataPtr->id); + this->AddWorldWrench(_ecm, _force, _torque, math::Vector3d::Zero); +} + +////////////////////////////////////////////////// +void Link::AddWorldWrench(EntityComponentManager &_ecm, + const math::Vector3d &_force, + const math::Vector3d &_torque, + const math::Vector3d &_offset) const +{ + math::Pose3d linkWorldPose; + auto worldPoseComp = _ecm.Component(this->dataPtr->id); + if (worldPoseComp) + { + linkWorldPose = worldPoseComp->Data(); + } + else + { + linkWorldPose = worldPose(this->dataPtr->id, _ecm); + } - components::ExternalWorldWrenchCmd wrench; + // 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; msgs::Set(wrench.Data().mutable_force(), _force); - msgs::Set(wrench.Data().mutable_torque(), _torque); + 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::Convert(linkWrenchComp->Data().force()) + _force); msgs::Set(linkWrenchComp->Data().mutable_torque(), - msgs::Convert(linkWrenchComp->Data().torque()) + _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..6ecf5d7d3b 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -81,15 +81,16 @@ class gz::sim::systems::ApplyLinkWrenchPrivate /// 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. /// \return Target link entity. Link decomposeMessage(const EntityComponentManager &_ecm, const msgs::EntityWrench &_msg, math::Vector3d &_force, - math::Vector3d &_torque) + math::Vector3d &_torque, math::Vector3d &_offset) { 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, torque, offset); 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, torque, offset); 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, 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, torque); + link.AddWorldWrench(_ecm, force, torque, offset); } } diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.hh b/src/systems/apply_link_wrench/ApplyLinkWrench.hh index 625db47578..f2c449e51f 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.hh +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.hh @@ -38,6 +38,9 @@ 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 frame. + /// /// ## Topics /// /// * /world//wrench diff --git a/test/integration/link.cc b/test/integration/link.cc index fd04e8bddb..10063b247a 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -608,6 +608,32 @@ 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, torque, offset); + + wrenchComp = ecm.Component(eLink); + EXPECT_NE(nullptr, wrenchComp); + wrenchMsg = wrenchComp->Data(); + + 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.AddWorldWrench(ecm, -force, -torque, offset); + 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(