diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc index 6d42ddff22..24a607d20f 100644 --- a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc @@ -15,6 +15,7 @@ * */ +#include #include #include @@ -26,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -129,12 +131,12 @@ namespace sim /// \brief Torque to be applied in link-fixed frame 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; - /// \brief Pose of the link-fixed frame public: math::Pose3d linkWorldPose; + /// \brief Pose of the inertial frame relative to the link frame + public: math::Pose3d inertialPose; + /// \brief Pointer to the rendering scene public: rendering::ScenePtr scene{nullptr}; @@ -209,7 +211,14 @@ ApplyForceTorque::ApplyForceTorque() } ///////////////////////////////////////////////// -ApplyForceTorque::~ApplyForceTorque() = default; +ApplyForceTorque::~ApplyForceTorque() +{ + if (!this->dataPtr->scene) + return; + this->dataPtr->scene->DestroyNode(this->dataPtr->forceVisual, true); + this->dataPtr->scene->DestroyNode(this->dataPtr->torqueVisual, true); + this->dataPtr->scene->DestroyNode(this->dataPtr->gizmoVisual, true); +} ///////////////////////////////////////////////// void ApplyForceTorque::LoadConfig(const tinyxml2::XMLElement */*_pluginElem*/) @@ -443,9 +452,8 @@ void ApplyForceTorque::Update(const UpdateInfo &/*_info*/, *this->dataPtr->selectedEntity); if (inertial) { - this->dataPtr->inertialPos = - linkWorldPose.Rot().RotateVector(inertial->Data().Pose().Pos()); this->dataPtr->linkWorldPose = linkWorldPose; + this->dataPtr->inertialPose = inertial->Data().Pose(); } } @@ -499,6 +507,27 @@ void ApplyForceTorque::SetForce(QVector3D _force) this->dataPtr->vectorRot = math::Matrix4d::LookAt( -this->dataPtr->force, math::Vector3d::Zero).Rotation(); } + emit this->ForceMagChanged(); +} + +///////////////////////////////////////////////// +double ApplyForceTorque::ForceMag() const +{ + return this->dataPtr->force.Length(); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::SetForceMag(double _forceMag) +{ + if (this->dataPtr->force == math::Vector3d::Zero) + { + this->dataPtr->force.X() = _forceMag; + } + else + { + this->dataPtr->force = _forceMag * this->dataPtr->force.Normalized(); + } + emit this->ForceChanged(); } ///////////////////////////////////////////////// @@ -521,6 +550,33 @@ void ApplyForceTorque::SetTorque(QVector3D _torque) this->dataPtr->vectorRot = math::Matrix4d::LookAt( -this->dataPtr->torque, math::Vector3d::Zero).Rotation(); } + emit this->TorqueMagChanged(); +} + +///////////////////////////////////////////////// +double ApplyForceTorque::TorqueMag() const +{ + return this->dataPtr->torque.Length(); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::SetTorqueMag(double _torqueMag) +{ + if (this->dataPtr->torque == math::Vector3d::Zero) + { + this->dataPtr->torque.X() = _torqueMag; + } + else + { + this->dataPtr->torque = _torqueMag * this->dataPtr->torque.Normalized(); + } + emit this->TorqueChanged(); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::UpdateOffset(double _x, double _y, double _z) +{ + this->dataPtr->offset.Set(_x, _y, _z); } ///////////////////////////////////////////////// @@ -553,15 +609,21 @@ void ApplyForceTorquePrivate::PublishWrench(bool _applyForce, bool _applyTorque) } // Force and torque in world coordinates - math::Vector3d forceToApply = this->linkWorldPose.Rot().RotateVector( - _applyForce ? this->force : math::Vector3d::Zero); - math::Vector3d torqueToApply = this->linkWorldPose.Rot().RotateVector( - _applyTorque ? this->torque : math::Vector3d::Zero) + - this->inertialPos.Cross(forceToApply); + math::Vector3d forceToApply = _applyForce ? + this->linkWorldPose.Rot().RotateVector(this->force) : + math::Vector3d::Zero; + math::Vector3d torqueToApply = _applyTorque ? + this->linkWorldPose.Rot().RotateVector(this->torque) : + math::Vector3d::Zero; + // The ApplyLinkWrench system takes the offset in the link frame + math::Vector3d offsetToApply = _applyForce ? + this->offset + this->inertialPose.Pos() : + math::Vector3d::Zero; msgs::EntityWrench msg; msg.mutable_entity()->set_id(*this->selectedEntity); msgs::Set(msg.mutable_wrench()->mutable_force(), forceToApply); + msgs::Set(msg.mutable_wrench()->mutable_force_offset(), offsetToApply); msgs::Set(msg.mutable_wrench()->mutable_torque(), torqueToApply); this->pub.Publish(msg); @@ -632,14 +694,16 @@ void ApplyForceTorquePrivate::OnRender() ///////////////////////////////////////////////// void ApplyForceTorquePrivate::UpdateVisuals() { + math::Pose3d inertialWorldPose = this->linkWorldPose * this->inertialPose; // Update force visualization if (this->force != math::Vector3d::Zero && this->selectedEntity.has_value()) { math::Vector3d worldForce = this->linkWorldPose.Rot().RotateVector(this->force); - math::Vector3d applicationPoint = this->linkWorldPose.Pos() + - this->inertialPos + this->linkWorldPose.Rot().RotateVector(this->offset); + math::Vector3d applicationPoint = + inertialWorldPose.Pos() + + this->linkWorldPose.Rot().RotateVector(this->offset); double scale = applicationPoint.Distance(this->camera->WorldPose().Pos()) / 2.0; this->wrenchVis.UpdateVectorVisual( @@ -658,7 +722,7 @@ void ApplyForceTorquePrivate::UpdateVisuals() math::Vector3d worldTorque = this->linkWorldPose.Rot().RotateVector(this->torque); math::Vector3d applicationPoint = - this->linkWorldPose.Pos() + this->inertialPos; + inertialWorldPose.Pos(); double scale = applicationPoint.Distance(this->camera->WorldPose().Pos()) / 2.0; this->wrenchVis.UpdateVectorVisual( @@ -678,13 +742,13 @@ void ApplyForceTorquePrivate::UpdateVisuals() && this->force != math::Vector3d::Zero) { pos = - this->linkWorldPose.Pos() + this->inertialPos + + inertialWorldPose.Pos() + this->linkWorldPose.Rot().RotateVector(this->offset); } else if (this->activeVector == RotationToolVector::TORQUE && this->torque != math::Vector3d::Zero) { - pos = this->linkWorldPose.Pos() + this->inertialPos; + pos = inertialWorldPose.Pos(); } else { @@ -865,7 +929,8 @@ void ApplyForceTorquePrivate::HandleMouseEvents() } /// get start and end pos in world frame from 2d point - math::Vector3d pos = this->linkWorldPose.Pos() + this->inertialPos + + math::Pose3d inertialWorldPose = this->linkWorldPose * this->inertialPose; + math::Vector3d pos = inertialWorldPose.Pos() + this->linkWorldPose.Rot().RotateVector(this->offset); double d = pos.Dot(axis); math::Planed plane(axis, d); diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh b/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh index 7588915082..9c9e219834 100644 --- a/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -73,6 +74,14 @@ namespace sim NOTIFY ForceChanged ) + /// \brief Force magnitude + Q_PROPERTY( + double forceMag + READ ForceMag + WRITE SetForceMag + NOTIFY ForceMagChanged + ) + /// \brief Torque Q_PROPERTY( QVector3D torque @@ -81,6 +90,14 @@ namespace sim NOTIFY TorqueChanged ) + /// \brief Torque magnitude + Q_PROPERTY( + double torqueMag + READ TorqueMag + WRITE SetTorqueMag + NOTIFY TorqueMagChanged + ) + /// \brief Constructor public: ApplyForceTorque(); @@ -98,24 +115,28 @@ namespace sim EntityComponentManager &_ecm) override; /// \brief Get the name of the selected model + /// \return The model name public: Q_INVOKABLE QString ModelName() const; /// \brief Notify that the model name changed signals: void ModelNameChanged(); /// \brief Get the name of the links of the selected model + /// \return The list of link names public: Q_INVOKABLE QStringList LinkNameList() const; /// \brief Notify that the link list changed signals: void LinkNameListChanged(); /// \brief Get index of the link in the list + /// \return The link index public: Q_INVOKABLE int LinkIndex() const; /// \brief Notify that the link index changed signals: void LinkIndexChanged(); /// \brief Set the index of the link in the list + /// \param[in] _linkIndex The new link index public: Q_INVOKABLE void SetLinkIndex(int _linkIndex); /// \brief Get the force vector @@ -129,6 +150,18 @@ namespace sim /// \param[in] _force The new force vector public: Q_INVOKABLE void SetForce(QVector3D _force); + /// \brief Get the magnitude of the force vector + /// \return The force magnitude + public: Q_INVOKABLE double ForceMag() const; + + /// \brief Notify that the force magnitude changed + signals: void ForceMagChanged(); + + /// \brief Set the magnitude of the force vector, scaling it to + /// keep its direction + /// \param[in] _forceMag The new force magnitude + public: Q_INVOKABLE void SetForceMag(double _forceMag); + /// \brief Get the torque vector /// \return The torque vector public: Q_INVOKABLE QVector3D Torque() const; @@ -140,6 +173,24 @@ namespace sim /// \param[in] _torque The new torque vector public: Q_INVOKABLE void SetTorque(QVector3D _torque); + /// \brief Get the magnitude of the torque vector + /// \return The torque magnitude + public: Q_INVOKABLE double TorqueMag() const; + + /// \brief Notify that the torque magnitude changed + signals: void TorqueMagChanged(); + + /// \brief Set the magnitude of the torque vector, scaling it to + /// keep its direction + /// \param[in] _torqueMag The new torque magnitude + public: Q_INVOKABLE void SetTorqueMag(double _torqueMag); + + /// \brief Set components of offset vector + /// \param[in] _x X component of offset + /// \param[in] _y Y component of offset + /// \param[in] _z Z component of offset + public: Q_INVOKABLE void UpdateOffset(double _x, double _y, double _z); + /// \brief Apply the specified force public: Q_INVOKABLE void ApplyForce(); diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml index d3cc802760..7a24301135 100644 --- a/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml @@ -23,7 +23,7 @@ GridLayout { columns: 8 columnSpacing: 10 Layout.minimumWidth: 350 - Layout.minimumHeight: 700 + Layout.minimumHeight: 750 anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 @@ -37,8 +37,11 @@ GridLayout { // Precision of the GzSpinBoxes in decimal places property int decimalPlaces: 2 - // Step size of the GzSpinBoxes - property double step: 1.0 + // Step size of the force and torque + property double step: 100.0 + + // Step size of the offset + property double stepOffset: 1.0 Label { Layout.columnSpan: 8 @@ -59,21 +62,21 @@ GridLayout { Text { Layout.columnSpan: 2 id: modelText - color: "dimgrey" + color: "black" text: qsTr("Model:") } Text { Layout.columnSpan: 6 id: modelName - color: "dimgrey" + color: "black" text: ApplyForceTorque.modelName } Text { Layout.columnSpan: 2 id: linkText - color: "dimgrey" + color: "black" text: qsTr("Link:") } @@ -88,24 +91,33 @@ GridLayout { } } - // Force + // Force and offset Text { - Layout.columnSpan: 8 + Layout.columnSpan: 4 id: forceText - color: "dimgrey" - text: qsTr("Force (applied to the center of mass):") + color: "black" + text: qsTr("Force:") + } + + Text { + Layout.columnSpan: 4 + Layout.fillWidth: true + wrapMode: Text.WordWrap + id: offsetText + color: "black" + text: qsTr("Offset (from COM):") } Label { Layout.columnSpan: 2 horizontalAlignment: Text.AlignRight id: forceXText - color: "dimgrey" + color: "black" text: qsTr("X (N)") } GzSpinBox { - Layout.columnSpan: 6 + Layout.columnSpan: 2 Layout.fillWidth: true id: forceX maximumValue: maxValue @@ -116,16 +128,37 @@ GridLayout { onValueChanged: ApplyForceTorque.force.x = forceX.value } + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: offsetXText + color: "black" + text: qsTr("X (m)") + } + + GzSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: offsetX + maximumValue: maxValue + minimumValue: minValue + value: 0 + decimals: decimalPlaces + stepSize: stepOffset + onValueChanged: ApplyForceTorque.UpdateOffset( + offsetX.value, offsetY.value, offsetZ.value) + } + Label { Layout.columnSpan: 2 horizontalAlignment: Text.AlignRight id: forceYText - color: "dimgrey" + color: "black" text: qsTr("Y (N)") } GzSpinBox { - Layout.columnSpan: 6 + Layout.columnSpan: 2 Layout.fillWidth: true id: forceY maximumValue: maxValue @@ -136,16 +169,37 @@ GridLayout { onValueChanged: ApplyForceTorque.force.y = forceY.value } + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: offsetYText + color: "black" + text: qsTr("Y (m)") + } + + GzSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: offsetY + maximumValue: maxValue + minimumValue: minValue + value: 0 + decimals: decimalPlaces + stepSize: stepOffset + onValueChanged: ApplyForceTorque.UpdateOffset( + offsetX.value, offsetY.value, offsetZ.value) + } + Label { Layout.columnSpan: 2 horizontalAlignment: Text.AlignRight id: forceZText - color: "dimgrey" + color: "black" text: qsTr("Z (N)") } GzSpinBox { - Layout.columnSpan: 6 + Layout.columnSpan: 2 Layout.fillWidth: true id: forceZ maximumValue: maxValue @@ -156,6 +210,47 @@ GridLayout { onValueChanged: ApplyForceTorque.force.z = forceZ.value } + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: offsetZText + color: "black" + text: qsTr("Z (m)") + } + + GzSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: offsetZ + maximumValue: maxValue + minimumValue: minValue + value: 0 + decimals: decimalPlaces + stepSize: stepOffset + onValueChanged: ApplyForceTorque.UpdateOffset( + offsetX.value, offsetY.value, offsetZ.value) + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: forceMagText + color: "black" + text: qsTr("Mag. (N)") + } + + GzSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: forceMag + maximumValue: maxValue + minimumValue: 0 + value: ApplyForceTorque.forceMag + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.forceMag = forceMag.value + } + Button { text: qsTr("Apply Force") Layout.columnSpan: 8 @@ -169,7 +264,7 @@ GridLayout { Text { Layout.columnSpan: 8 id: torqueText - color: "dimgrey" + color: "black" text: qsTr("Torque:") } @@ -177,7 +272,7 @@ GridLayout { Layout.columnSpan: 2 horizontalAlignment: Text.AlignRight id: torqueXText - color: "dimgrey" + color: "black" text: qsTr("X (N.m)") } @@ -197,7 +292,7 @@ GridLayout { Layout.columnSpan: 2 horizontalAlignment: Text.AlignRight id: torqueYText - color: "dimgrey" + color: "black" text: qsTr("Y (N.m)") } @@ -217,7 +312,7 @@ GridLayout { Layout.columnSpan: 2 horizontalAlignment: Text.AlignRight id: torqueZText - color: "dimgrey" + color: "black" text: qsTr("Z (N.m)") } @@ -233,6 +328,26 @@ GridLayout { onValueChanged: ApplyForceTorque.torque.z = torqueZ.value } + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: torqueMagText + color: "black" + text: qsTr("Mag. (N.m)") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: torqueMag + maximumValue: maxValue + minimumValue: 0 + value: ApplyForceTorque.torqueMag + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.torqueMag = torqueMag.value + } + Button { text: qsTr("Apply Torque") Layout.columnSpan: 8 diff --git a/src/rendering/WrenchVisualizer.cc b/src/rendering/WrenchVisualizer.cc index 0adab74c16..61203f501f 100644 --- a/src/rendering/WrenchVisualizer.cc +++ b/src/rendering/WrenchVisualizer.cc @@ -15,10 +15,14 @@ * */ +#include + #include #include +#include #include #include +#include #include #include "gz/sim/rendering/WrenchVisualizer.hh" diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index 6ecf5d7d3b..bdda4d04b5 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -80,6 +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 expressed in the link +/// frame. /// \param[out] Torque to apply. /// \param[out] Offset of the force application point expressed in the link /// frame.