Skip to content

Commit

Permalink
Force offset and vector magnitude support in ApplyForceTorque (#2056)
Browse files Browse the repository at this point in the history 
Adds support for specifying the offset of the force application point through the GUI in the ApplyForceTorque plugin. This offset is also reflected in the visualization of the force vector.

Also adds fields for the magnitude of the force and torque vectors on the interface. Changing the components of a vector automatically updates the value of its magnitude, while changing the vector's magnitude automatically scales its components, maintaining its direction.

---------

Signed-off-by: Henrique-BO <[email protected]>
  • Loading branch information
@Henrique-BO
Henrique-BO authored Aug 25, 2023
1 parent 46c5df3 commit 2d64e1e
Show file tree
Hide file tree
Showing 5 changed files with 274 additions and 37 deletions.
99 changes: 82 additions & 17 deletions src/gui/plugins/apply_force_torque/ApplyForceTorque.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
*
*/

#include <memory>
#include <mutex>
#include <string>

Expand All @@ -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>
Expand Down Expand Up @@ -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};

Expand Down Expand Up @@ -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*/)
Expand Down Expand Up @@ -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();
}
}

Expand Down Expand Up @@ -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();
}

/////////////////////////////////////////////////
Expand All @@ -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);
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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(
Expand All @@ -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(
Expand All @@ -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
{
Expand Down Expand Up @@ -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);
Expand Down
51 changes: 51 additions & 0 deletions src/gui/plugins/apply_force_torque/ApplyForceTorque.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <QStringList>
#include <QObject>
#include <QEvent>
#include <QVector3D>

#include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/gui/GuiSystem.hh>
Expand Down Expand Up @@ -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
Expand All @@ -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();

Expand All @@ -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
Expand All @@ -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;
Expand All @@ -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();

Expand Down
Loading

0 comments on commit 2d64e1e

Please sign in to comment.