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();