diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc index dba85640da..d9e796931b 100644 --- a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc @@ -15,23 +15,23 @@ * */ +#include #include #include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include +#include #include +#include +#include +#include +#include +#include +#include #include -#include #include "ApplyForceTorque.hh" @@ -50,6 +50,9 @@ namespace sim /// \brief Publisher for EntityWrench messages public: transport::Node::Publisher pub; + /// \brief To synchronize member access + public: std::mutex mutex; + /// \brief Name of the selected model public: QString modelName; @@ -73,6 +76,9 @@ namespace sim /// \brief Torque to be applied public: math::Vector3d torque{0.0, 0.0, 0.0}; + + /// \brief Offset from the link origin to the center of mass in world coords + public: math::Vector3d inertialPos; }; } } @@ -113,13 +119,13 @@ void ApplyForceTorque::LoadConfig(const tinyxml2::XMLElement */*_pluginElem*/) gz::gui::App()->findChild ()->installEventFilter(this); - gz::gui::App()->findChild - ()->QuickWindow()->installEventFilter(this); } ///////////////////////////////////////////////// bool ApplyForceTorque::eventFilter(QObject *_obj, QEvent *_event) { + std::lock_guard lock(this->dataPtr->mutex); + if (_event->type() == gz::sim::gui::events::EntitiesSelected::kType) { @@ -142,6 +148,8 @@ bool ApplyForceTorque::eventFilter(QObject *_obj, QEvent *_event) void ApplyForceTorque::Update(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { + std::lock_guard lock(this->dataPtr->mutex); + if (this->dataPtr->changedEntity) { this->dataPtr->changedEntity = false; @@ -202,6 +210,19 @@ void ApplyForceTorque::Update(const UpdateInfo &/*_info*/, } } + // Get the position of the center of mass + if (this->dataPtr->selectedEntity.has_value()) + { + auto linkWorldPose = worldPose(*this->dataPtr->selectedEntity, _ecm); + auto inertial = _ecm.Component( + *this->dataPtr->selectedEntity); + if (inertial) + { + this->dataPtr->inertialPos = + linkWorldPose.Rot().RotateVector(inertial->Data().Pose().Pos()); + } + } + emit this->ModelNameChanged(); emit this->LinkNameListChanged(); emit this->LinkIndexChanged(); @@ -265,6 +286,8 @@ void ApplyForceTorque::ApplyAll() ///////////////////////////////////////////////// void ApplyForceTorquePrivate::PublishWrench(bool _applyForce, bool _applyTorque) { + std::lock_guard lock(this->mutex); + if (!this->selectedEntity.has_value()) { gzdbg << "No link selected" << std::endl; @@ -274,7 +297,8 @@ void ApplyForceTorquePrivate::PublishWrench(bool _applyForce, bool _applyTorque) math::Vector3d forceToApply = _applyForce ? this->force : math::Vector3d::Zero; math::Vector3d torqueToApply = - _applyTorque ? this->torque : math::Vector3d::Zero; + _applyTorque ? this->torque : math::Vector3d::Zero + + this->inertialPos.Cross(forceToApply); gzdbg << "Applying wrench [" << forceToApply[0] << " " << diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh b/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh index 44610da08c..e1ed4c1450 100644 --- a/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh @@ -20,6 +20,12 @@ #include +#include +#include +#include +#include + +#include #include namespace gz diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml index 67190be839..2c4749754c 100644 --- a/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml @@ -77,7 +77,7 @@ GridLayout { Layout.columnSpan: 8 id: forceText color: "dimgrey" - text: qsTr("Force (applied to link origin):") + text: qsTr("Force (applied to the center of mass):") } Label {