Force offset and vector magnitude support in ApplyForceTorque

src/gui/plugins/apply_force_torque/ApplyForceTorque.cc

@@ -15,6 +15,7 @@
  *
 */
 
+#include <memory>
 #include <mutex>
 #include <string>
 
@@ -26,6 +27,7 @@
 #include <gz/gui/MainWindow.hh>
 #include <gz/math/Pose3.hh>
 #include <gz/math/Quaternion.hh>
+#include <gz/math/Vector2.hh>
 #include <gz/math/Vector3.hh>
 #include <gz/msgs/entity_plugin_v.pb.h>
 #include <gz/msgs/entity_wrench.pb.h>
@@ -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};
 
@@ -443,9 +445,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 +500,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 +543,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 +602,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 +687,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 +715,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 +735,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 +922,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);

src/gui/plugins/apply_force_torque/ApplyForceTorque.hh

@@ -24,6 +24,7 @@
 #include <QStringList>
 #include <QObject>
 #include <QEvent>
+#include <QVector3D>
 
 #include <gz/sim/EntityComponentManager.hh>
 #include <gz/sim/gui/GuiSystem.hh>
@@ -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();