Skip to content

Commit

Permalink
New WrenchVisualizer class and fix rotation tool bugs
Browse files Browse the repository at this point in the history 
Signed-off-by: Henrique-BO <henrique.barrosoliveira@usp.br>
  • Loading branch information
@Henrique-BO
Henrique-BO committed Aug 3, 2023
1 parent 86be94c commit bfbb1bd
Show file tree
Hide file tree
Showing 5 changed files with 280 additions and 86 deletions.
83 changes: 83 additions & 0 deletions include/gz/sim/rendering/WrenchVisualizer.hh
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
/*
* Copyright (C) 2023 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef GZ_SIM_WRENCHVISUALIZER_HH_
#define GZ_SIM_WRENCHVISUALIZER_HH_

#include <memory>

#include <gz/sim/config.hh>
#include <gz/sim/rendering/Export.hh>

#include <gz/math/Vector3.hh>
#include <gz/rendering/RenderTypes.hh>

namespace gz
{
namespace sim
{
// Inline bracket to help doxygen filtering.
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace detail{
// Forward declare private data class.
class WrenchVisualizerPrivate;

/// \brief Creates, deletes, and maintains force and torque visuals
class GZ_SIM_RENDERING_VISIBLE WrenchVisualizer
{
/// \brief Constructor
public: WrenchVisualizer();

/// \brief Destructor
public: virtual ~WrenchVisualizer();

/// \brief Initialize the Wrench visualizer
void Init(rendering::ScenePtr _scene);

/// \brief Create a new force visual
/// \param[in] _material The material used for the visual
/// \return Pointer to the created ArrowVisual
public: rendering::ArrowVisualPtr CreateForceVisual(
rendering::MaterialPtr _material);

/// \brief Create a new torque visual
/// \param[in] _material The material used for the visual
/// \return Pointer to the created Visual
public: rendering::VisualPtr CreateTorqueVisual(
rendering::MaterialPtr _material);

/// \brief Update the visual of a vector to match its direction and position
/// \param[in] _visual Pointer to the vector visual
/// \param[in] _direction Direction of the vector
/// \param[in] _position Position of the arrow
/// \param[in] _size Size of the arrow in meters
/// \param[in] _tip True if _position specifies the tip of the vector,
/// false if it specifies tha base of the vector
public: void UpdateVectorVisual(rendering::VisualPtr _visual,
math::Vector3d _direction,
math::Vector3d _position,
double _size,
bool _tip = false);

/// \internal
/// \brief Private data pointer
private: std::unique_ptr<WrenchVisualizerPrivate> dataPtr;
};
}
}
}
}
#endif
153 changes: 67 additions & 86 deletions src/gui/plugins/apply_force_torque/ApplyForceTorque.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
#include <gz/sim/components/SystemPluginInfo.hh>
#include <gz/sim/components/World.hh>
#include <gz/sim/gui/GuiEvents.hh>
#include <gz/sim/rendering/WrenchVisualizer.hh>
#include <gz/transport/Node.hh>

#include "ApplyForceTorque.hh"
Expand Down Expand Up @@ -166,14 +167,17 @@ namespace sim
public: math::Vector3d initialVector;

/// \brief Vector currently being rotated
public: RotationToolVector vec = RotationToolVector::NONE;
public: RotationToolVector vec{RotationToolVector::NONE};

/// \brief Active transformation axis on the gizmo
public: rendering::TransformAxis activeAxis{rendering::TA_NONE};

/// \brief Holds the latest mouse event
public: gz::common::MouseEvent mouseEvent;

/// \brief Wrench visualizer
public: detail::WrenchVisualizer wrenchVis;

/// \brief Arrow for visualizing force.
public: rendering::ArrowVisualPtr forceVisual{nullptr};

Expand Down Expand Up @@ -228,8 +232,6 @@ void ApplyForceTorque::LoadConfig(const tinyxml2::XMLElement */*_pluginElem*/)
/////////////////////////////////////////////////
bool ApplyForceTorque::eventFilter(QObject *_obj, QEvent *_event)
{
// std::lock_guard<std::mutex> lock(this->dataPtr->mutex);

if (_event->type() == gz::gui::events::Render::kType)
{
this->dataPtr->OnRender();
Expand All @@ -245,7 +247,7 @@ bool ApplyForceTorque::eventFilter(QObject *_obj, QEvent *_event)
}
else if (_event->type() == gz::sim::gui::events::EntitiesSelected::kType)
{
if (!this->dataPtr->blockOrbit)
if (!this->dataPtr->blockOrbit && !this->dataPtr->mouseEvent.Dragging())
{
gz::sim::gui::events::EntitiesSelected *_e =
static_cast<gz::sim::gui::events::EntitiesSelected*>(_event);
Expand All @@ -255,7 +257,7 @@ bool ApplyForceTorque::eventFilter(QObject *_obj, QEvent *_event)
}
else if (_event->type() == gz::sim::gui::events::DeselectAllEntities::kType)
{
if (!this->dataPtr->blockOrbit)
if (!this->dataPtr->blockOrbit && !this->dataPtr->mouseEvent.Dragging())
{
this->dataPtr->selectedEntity.reset();
this->dataPtr->changedEntity = true;
Expand Down Expand Up @@ -293,6 +295,7 @@ void ApplyForceTorque::Update(const UpdateInfo &/*_info*/,
std::lock_guard<std::mutex> lock(this->dataPtr->mutex);

// Load the ApplyLinkWrench system
// TODO(anyone): should this be checked on every Update instead?
if (!this->dataPtr->systemLoaded)
{
const std::string name{"gz::sim::systems::ApplyLinkWrench"};
Expand Down Expand Up @@ -574,35 +577,10 @@ void ApplyForceTorquePrivate::OnRender()
mat->SetDepthCheckEnabled(false);
mat->SetDepthWriteEnabled(false);

// Create force visual
this->forceVisual = this->scene->CreateArrowVisual();
this->forceVisual->SetMaterial(mat);
this->forceVisual->ShowArrowHead(true);
this->forceVisual->ShowArrowShaft(true);
this->forceVisual->ShowArrowRotation(false);

// Create torque visual
common::MeshManager *meshMgr = common::MeshManager::Instance();
std::string meshName{"torque_tube"};
if (!meshMgr->HasMesh(meshName))
meshMgr->CreateTube(meshName, 0.28f, 0.3f, 0.2f, 1, 32);
rendering::VisualPtr torqueTube = this->scene->CreateVisual();
torqueTube->AddGeometry(this->scene->CreateMesh(meshName));
torqueTube->SetOrigin(0, 0, -0.9f);
torqueTube->SetLocalPosition(0, 0, 0);

rendering::VisualPtr cylinder = this->scene->CreateVisual();
cylinder->AddGeometry(this->scene->CreateCylinder());
cylinder->SetOrigin(0, 0, -0.5);
cylinder->SetLocalScale(0.01, 0.01, 0.8);
cylinder->SetLocalPosition(0, 0, 0);

this->torqueVisual = this->scene->CreateVisual();
this->torqueVisual->AddChild(torqueTube);
this->torqueVisual->AddChild(cylinder);
this->torqueVisual->SetMaterial(mat);

// Create gizmo visual
// Create visuals
this->wrenchVis.Init(this->scene);
this->forceVisual = this->wrenchVis.CreateForceVisual(mat);
this->torqueVisual = this->wrenchVis.CreateTorqueVisual(mat);
this->gizmoVisual = this->scene->CreateGizmoVisual();
this->scene->RootVisual()->AddChild(this->gizmoVisual);
}
Expand All @@ -627,30 +605,18 @@ void ApplyForceTorquePrivate::OnRender()
/////////////////////////////////////////////////
void ApplyForceTorquePrivate::UpdateVisuals()
{
math::Vector3d applicationPoint = this->linkWorldPose.Pos() +
this->inertialPos + this->linkWorldPose.Rot().RotateVector(this->offset);
double scale = applicationPoint.Distance(this->camera->WorldPose().Pos())
/ 3.0;
// Update force visualization
if (this->force != math::Vector3d::Zero &&
this->selectedEntity.has_value())
{
math::Vector3d worldForce =
this->linkWorldPose.Rot().RotateVector(this->force);
math::Vector3d u = worldForce.Normalized();
math::Vector3d v = gz::math::Vector3d::UnitZ;
double angle = acos(v.Dot(u));
math::Quaterniond quat;
// check the parallel case
if (math::equal(angle, GZ_PI))
quat.SetFromAxisAngle(u.Perpendicular(), angle);
else
quat.SetFromAxisAngle((v.Cross(u)).Normalize(), angle);

this->forceVisual->SetLocalScale(scale);
this->forceVisual->SetLocalPosition(applicationPoint - 0.75 * scale * u);
this->forceVisual->SetLocalRotation(quat);
this->forceVisual->SetVisible(true);
math::Vector3d applicationPoint = this->linkWorldPose.Pos() +
this->inertialPos + this->linkWorldPose.Rot().RotateVector(this->offset);
double scale = applicationPoint.Distance(this->camera->WorldPose().Pos())
/ 2.0;
this->wrenchVis.UpdateVectorVisual(
this->forceVisual, worldForce, applicationPoint, scale, true);
}
else
{
Expand All @@ -663,21 +629,12 @@ void ApplyForceTorquePrivate::UpdateVisuals()
{
math::Vector3d worldTorque =
this->linkWorldPose.Rot().RotateVector(this->torque);
math::Vector3d u = worldTorque.Normalized();
math::Vector3d v = gz::math::Vector3d::UnitZ;
double angle = acos(v.Dot(u));
math::Quaterniond quat;
// check the parallel case
if (math::equal(angle, GZ_PI))
quat.SetFromAxisAngle(u.Perpendicular(), angle);
else
quat.SetFromAxisAngle((v.Cross(u)).Normalize(), angle);

this->torqueVisual->SetLocalScale(scale);
this->torqueVisual->SetLocalPosition(this->linkWorldPose.Pos() +
this->inertialPos);
this->torqueVisual->SetLocalRotation(quat);
this->torqueVisual->SetVisible(true);
math::Vector3d applicationPoint =
this->linkWorldPose.Pos() + this->inertialPos;
double scale = applicationPoint.Distance(this->camera->WorldPose().Pos())
/ 2.0;
this->wrenchVis.UpdateVectorVisual(
this->torqueVisual, worldTorque, applicationPoint, scale, false);
}
else
{
Expand All @@ -691,14 +648,18 @@ void ApplyForceTorquePrivate::UpdateVisuals()
math::Vector3d u;
if (this->vec == RotationToolVector::FORCE)
{
pos = applicationPoint;
pos =
this->linkWorldPose.Pos() + this->inertialPos +
this->linkWorldPose.Rot().RotateVector(this->offset);
u = this->force;
}
else if (this->vec == RotationToolVector::TORQUE)
{
pos = this->linkWorldPose.Pos() + this->inertialPos;
u = this->torque;
}
double scale = pos.Distance(this->camera->WorldPose().Pos())
/ 2.0;

// Update gizmo visual rotation so that they are always facing the
// eye position
Expand Down Expand Up @@ -730,7 +691,7 @@ void ApplyForceTorquePrivate::UpdateVisuals()
circleVis->SetWorldRotation(
lookAt.Rotation() * math::Quaterniond(circleRotOffset));

this->gizmoVisual->SetLocalScale(scale, scale, scale);
this->gizmoVisual->SetLocalScale(1.5 * scale);
this->gizmoVisual->SetTransformMode(rendering::TransformMode::TM_ROTATION);
this->gizmoVisual->SetLocalPosition(pos);

Expand Down Expand Up @@ -759,7 +720,9 @@ void ApplyForceTorquePrivate::HandleMouseEvents()
// handle mouse movements
if (this->mouseEvent.Button() == common::MouseEvent::LEFT)
{
if (this->mouseEvent.Type() == common::MouseEvent::PRESS)
// Rotating vector around the clicked axis
if (this->mouseEvent.Type() == common::MouseEvent::PRESS
&& this->vec != RotationToolVector::NONE)
{
this->mousePressPos = this->mouseEvent.Pos();

Expand All @@ -770,7 +733,6 @@ void ApplyForceTorquePrivate::HandleMouseEvents()

if (visual)
{
auto id = visual->Id();
// check if the visual is an axis in the gizmo visual
auto axis = this->gizmoVisual->AxisById(visual->Id());
if (axis == rendering::TransformAxis::TA_ROTATION_Y
Expand All @@ -786,22 +748,11 @@ void ApplyForceTorquePrivate::HandleMouseEvents()
this->sendBlockOrbit = true;
this->activeAxis = axis;
}
else if (this->forceVisual->Id() == id ||
this->forceVisual->ChildById(id))
{
this->vec = RotationToolVector::FORCE;
}
else if (this->torqueVisual->Id() == id ||
this->torqueVisual->ChildById(id))
{
this->vec = RotationToolVector::TORQUE;
}
else
{
this->blockOrbit = false;
this->sendBlockOrbit = true;
this->activeAxis = rendering::TransformAxis::TA_NONE;
this->vec = RotationToolVector::NONE;
return;
}
}
Expand All @@ -811,6 +762,36 @@ void ApplyForceTorquePrivate::HandleMouseEvents()
this->blockOrbit = false;
this->sendBlockOrbit = true;
this->activeAxis = rendering::TransformAxis::TA_NONE;

if (!this->mouseEvent.Dragging())
{
// get the visual at mouse position
rendering::VisualPtr visual = this->scene->VisualAt(
this->camera,
this->mouseEvent.Pos());
if (!visual)
return;

// Select a vector for the rotation tool
auto id = visual->Id();
if ((this->forceVisual->Id() == id ||
this->forceVisual->ChildById(id))
&& this->vec != RotationToolVector::FORCE)
{
this->vec = RotationToolVector::FORCE;
}
else if ((this->torqueVisual->Id() == id ||
this->torqueVisual->ChildById(id))
&& this->vec != RotationToolVector::TORQUE)
{
this->vec = RotationToolVector::TORQUE;
}
else if (this->gizmoVisual->AxisById(visual->Id()) ==
rendering::TransformAxis::TA_NONE)
{
this->vec = RotationToolVector::NONE;
}
}
}
}
if (this->mouseEvent.Type() == common::MouseEvent::MOVE
Expand Down Expand Up @@ -840,16 +821,16 @@ void ApplyForceTorquePrivate::HandleMouseEvents()

math::Vector3d startPos;
this->ray->SetFromCamera(this->camera, start);
if (auto v = plane.Intersection(
this->ray->Origin(), this->ray->Direction()))
if (auto v{plane.Intersection(
this->ray->Origin(), this->ray->Direction())})
startPos = *v;
else
return;

math::Vector3d endPos;
this->ray->SetFromCamera(this->camera, end);
if (auto v = plane.Intersection(
this->ray->Origin(), this->ray->Direction()))
if (auto v{plane.Intersection(
this->ray->Origin(), this->ray->Direction())})
endPos = *v;
else
return;
Expand Down
2 changes: 2 additions & 0 deletions src/gui/plugins/apply_force_torque/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,6 @@
gz_add_gui_plugin(ApplyForceTorque
SOURCES ApplyForceTorque.cc
QT_HEADERS ApplyForceTorque.hh
PUBLIC_LINK_LIBS
${PROJECT_LIBRARY_TARGET_NAME}-rendering
)
1 change: 1 addition & 0 deletions src/rendering/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@ set (rendering_comp_sources
MarkerManager.cc
RenderUtil.cc
SceneManager.cc
WrenchVisualizer.cc
)

if (MSVC)
Expand Down
Loading

0 comments on commit bfbb1bd

Please sign in to comment.