From 142d5a18bf588853315956edb2ee5d1468680307 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Sat, 1 Jul 2023 15:14:19 +0200 Subject: [PATCH 01/20] Create MouseDrag GUI plugin Signed-off-by: Henrique-BO --- src/gui/plugins/CMakeLists.txt | 1 + src/gui/plugins/mouse_drag/CMakeLists.txt | 4 + src/gui/plugins/mouse_drag/MouseDrag.cc | 196 ++++++++++++++++++++++ src/gui/plugins/mouse_drag/MouseDrag.hh | 66 ++++++++ src/gui/plugins/mouse_drag/MouseDrag.qml | 53 ++++++ src/gui/plugins/mouse_drag/MouseDrag.qrc | 5 + 6 files changed, 325 insertions(+) create mode 100644 src/gui/plugins/mouse_drag/CMakeLists.txt create mode 100644 src/gui/plugins/mouse_drag/MouseDrag.cc create mode 100644 src/gui/plugins/mouse_drag/MouseDrag.hh create mode 100644 src/gui/plugins/mouse_drag/MouseDrag.qml create mode 100644 src/gui/plugins/mouse_drag/MouseDrag.qrc diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 956158f39c..1a6665b882 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -139,6 +139,7 @@ add_subdirectory(environment_loader) add_subdirectory(environment_visualization) add_subdirectory(joint_position_controller) add_subdirectory(lights) +add_subdirectory(mouse_drag) add_subdirectory(playback_scrubber) add_subdirectory(plot_3d) add_subdirectory(plotting) diff --git a/src/gui/plugins/mouse_drag/CMakeLists.txt b/src/gui/plugins/mouse_drag/CMakeLists.txt new file mode 100644 index 0000000000..9d34ef4653 --- /dev/null +++ b/src/gui/plugins/mouse_drag/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_gui_plugin(MouseDrag + SOURCES MouseDrag.cc + QT_HEADERS MouseDrag.hh +) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc new file mode 100644 index 0000000000..9a33dad1e2 --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -0,0 +1,196 @@ +/* + * 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. + * +*/ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "MouseDrag.hh" + +namespace gz +{ +namespace sim +{ + class MouseDragPrivate + { + /// \brief Handle mouse events + public: void HandleMouseEvents(); + + /// \brief Transport node + public: transport::Node node; + + /// \brief Publisher for EntityWrench messages + public: transport::Node::Publisher pub; + + /// \brief Holds the latest mouse event + public: gz::common::MouseEvent mouseEvent; + + /// \brief True if there are new mouse events to process. + public: bool mouseDirty{false}; + + /// \brief True if the force should be applied to the center of mass + public: bool applyCOM{false}; + + /// \brief Block orbit + public: bool blockOrbit{false}; + }; +} +} + +using namespace gz; +using namespace sim; + +///////////////////////////////////////////////// +MouseDrag::MouseDrag() + : GuiSystem(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +MouseDrag::~MouseDrag() = default; + +///////////////////////////////////////////////// +void MouseDrag::LoadConfig(const tinyxml2::XMLElement */*_pluginElem*/) +{ + if (this->title.empty()) + this->title = "Mouse drag"; + + // Create wrench publisher + auto worldNames = gz::gui::worldNames(); + if (!worldNames.empty()) + { + auto topic = transport::TopicUtils::AsValidTopic( + "/world/" + worldNames[0].toStdString() + "/wrench"); + if (topic == "") + { + gzerr << "Unable to create publisher" << std::endl; + return; + } + this->dataPtr->pub = + this->dataPtr->node.Advertise(topic); + gzdbg << "Created publisher to " << topic << std::endl; + } + + gz::gui::App()->findChild + ()->installEventFilter(this); + gz::gui::App()->findChild + ()->QuickWindow()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool MouseDrag::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == gz::gui::events::Render::kType) + { + this->dataPtr->HandleMouseEvents(); + + gzdbg << "Block orbit: " << this->dataPtr->blockOrbit << std::endl; + gz::gui::events::BlockOrbit blockOrbitEvent(this->dataPtr->blockOrbit); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &blockOrbitEvent); + } + else if (_event->type() == gz::gui::events::MousePressOnScene::kType) + { + auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == gz::gui::events::DragOnScene::kType) + { + auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + + this->dataPtr->HandleMouseEvents(); + + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void MouseDrag::Update(const UpdateInfo &/*_info*/, + EntityComponentManager &/* _ecm */) +{ +} + +///////////////////////////////////////////////// +void MouseDrag::OnSwitchCOM(const bool _checked) +{ + this->dataPtr->applyCOM = _checked; + gzdbg << "Apply force to COM: " << this->dataPtr->applyCOM << std::endl; +} + +///////////////////////////////////////////////// +void MouseDragPrivate::HandleMouseEvents() +{ + // Check for mouse events + if (!this->mouseDirty) + { + return; + } + this->mouseDirty = false; + + if (this->mouseEvent.Button() == common::MouseEvent::LEFT) + { + if (this->mouseEvent.Type() == common::MouseEvent::PRESS && + this->mouseEvent.Control()) + { + gzdbg << "Ctrl-Left click press" << std::endl; + this->blockOrbit = true; + } + else if (this->mouseEvent.Type() == common::MouseEvent::RELEASE) + { + gzdbg << "Left click release" << std::endl; + this->blockOrbit = true; + } + } + else if (this->mouseEvent.Button() == common::MouseEvent::RIGHT) + { + if (this->mouseEvent.Type() == common::MouseEvent::PRESS && + this->mouseEvent.Control()) + { + gzdbg << "Ctrl-Right click press" << std::endl; + this->blockOrbit = true; + } + else if (this->mouseEvent.Type() == common::MouseEvent::RELEASE) + { + gzdbg << "Right click release" << std::endl; + this->blockOrbit = true; + } + } + + if (this->mouseEvent.Type() == common::MouseEvent::MOVE) + { + gzdbg << "Mouse Move [" << this->mouseEvent.Pos() << "]" << std::endl; + } +} + +// Register this plugin +GZ_ADD_PLUGIN(MouseDrag, gz::gui::Plugin); diff --git a/src/gui/plugins/mouse_drag/MouseDrag.hh b/src/gui/plugins/mouse_drag/MouseDrag.hh new file mode 100644 index 0000000000..3c72187db3 --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag.hh @@ -0,0 +1,66 @@ +/* + * 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_GUI_MOUSEDRAG_HH +#define GZ_GUI_MOUSEDRAG_HH + +#include + +#include + +namespace gz +{ +namespace sim +{ + class MouseDragPrivate; + + /// \brief Translate and rotate links by dragging them with the mouse. + /// Requires the ApplyLinkWrench system to be loaded. + /// + /// ## Configuration + /// This plugin doesn't accept any custom configuration. + class MouseDrag : public gz::sim::GuiSystem + { + Q_OBJECT + + /// \brief Constructor + public: MouseDrag(); + + /// \brief Destructor + public: ~MouseDrag() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + /// \brief Callback when echo button is pressed + public slots: void OnSwitchCOM(const bool _checked); + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} + +#endif diff --git a/src/gui/plugins/mouse_drag/MouseDrag.qml b/src/gui/plugins/mouse_drag/MouseDrag.qml new file mode 100644 index 0000000000..20a1b2df23 --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag.qml @@ -0,0 +1,53 @@ +/* + * 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. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + +GridLayout { + columns: 8 + columnSpacing: 10 + Layout.minimumWidth: 350 + Layout.minimumHeight: 200 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + + Text { + Layout.columnSpan: 8 + id: rotationText + color: "dimgrey" + text: qsTr("Ctrl+Left-Click: rotation") + } + + Text { + Layout.columnSpan: 8 + id: translationText + color: "dimgrey" + text: qsTr("Ctrl+Right-Click: translation") + } + + Switch { + Layout.columnSpan: 8 + objectName: "switchCOM" + text: qsTr("Apply force to center of mass") + onToggled: { + MouseDrag.OnSwitchCOM(checked); + } + } +} diff --git a/src/gui/plugins/mouse_drag/MouseDrag.qrc b/src/gui/plugins/mouse_drag/MouseDrag.qrc new file mode 100644 index 0000000000..89e46ce6cb --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag.qrc @@ -0,0 +1,5 @@ + + + MouseDrag.qml + + From 9449842f480fd78c056fd495d56582c5b149c3b9 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Sat, 1 Jul 2023 17:09:03 +0200 Subject: [PATCH 02/20] Track mouse position and the clicked link Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 129 +++++++++++++++++++----- 1 file changed, 102 insertions(+), 27 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index 9a33dad1e2..6ac870bcaa 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -19,15 +19,24 @@ #include #include +#include +#include +#include +#include #include #include #include #include +#include +#include #include +#include +#include #include #include #include #include +#include #include "MouseDrag.hh" @@ -46,6 +55,12 @@ namespace sim /// \brief Publisher for EntityWrench messages public: transport::Node::Publisher pub; + /// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene{nullptr}; + + /// \brief User camera + public: rendering::CameraPtr camera{nullptr}; + /// \brief Holds the latest mouse event public: gz::common::MouseEvent mouseEvent; @@ -53,10 +68,17 @@ namespace sim public: bool mouseDirty{false}; /// \brief True if the force should be applied to the center of mass - public: bool applyCOM{false}; + public: bool applyCOM{true}; /// \brief Block orbit public: bool blockOrbit{false}; + + /// \brief Visual of the Link to which apply the wrenches + public: Entity visualId; + + /// \brief Offset of the force application point relative to the + /// center of mass + public: math::Vector3d offset{0.0, 0.0, 0.0}; }; } } @@ -106,9 +128,6 @@ bool MouseDrag::eventFilter(QObject *_obj, QEvent *_event) { if (_event->type() == gz::gui::events::Render::kType) { - this->dataPtr->HandleMouseEvents(); - - gzdbg << "Block orbit: " << this->dataPtr->blockOrbit << std::endl; gz::gui::events::BlockOrbit blockOrbitEvent(this->dataPtr->blockOrbit); gz::gui::App()->sendEvent( gz::gui::App()->findChild(), @@ -136,15 +155,41 @@ bool MouseDrag::eventFilter(QObject *_obj, QEvent *_event) ///////////////////////////////////////////////// void MouseDrag::Update(const UpdateInfo &/*_info*/, - EntityComponentManager &/* _ecm */) + EntityComponentManager &_ecm) { + if (this->dataPtr->visualId == kNullEntity) + { + return; + } + + // Get Link corresponding to clicked Visual + Link link; + auto linkId = + _ecm.ComponentData(this->dataPtr->visualId); + if (linkId) + { + link = Link(*linkId); + if (!link.Valid(_ecm)) + { + return; + } + } + + auto inertial = _ecm.Component(*linkId); + if (!inertial) + { + gzdbg << "Link must have an inertial component" << std::endl; + return; + } + auto centerOfMass = inertial->Data().Pose().Pos(); } ///////////////////////////////////////////////// -void MouseDrag::OnSwitchCOM(const bool _checked) +void MouseDrag::OnSwitchCOM(const bool /* _checked */) { - this->dataPtr->applyCOM = _checked; - gzdbg << "Apply force to COM: " << this->dataPtr->applyCOM << std::endl; + // this->dataPtr->applyCOM = _checked; + gzdbg << "Only CoM application is currently supported" + << this->dataPtr->applyCOM << std::endl; } ///////////////////////////////////////////////// @@ -156,37 +201,67 @@ void MouseDragPrivate::HandleMouseEvents() return; } this->mouseDirty = false; - - if (this->mouseEvent.Button() == common::MouseEvent::LEFT) + + // Get scene and user camera + if (nullptr == this->scene) { - if (this->mouseEvent.Type() == common::MouseEvent::PRESS && - this->mouseEvent.Control()) + this->scene = rendering::sceneFromFirstRenderEngine(); + if (nullptr == this->scene) { - gzdbg << "Ctrl-Left click press" << std::endl; - this->blockOrbit = true; + return; } - else if (this->mouseEvent.Type() == common::MouseEvent::RELEASE) + + for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) { - gzdbg << "Left click release" << std::endl; - this->blockOrbit = true; + auto cam = std::dynamic_pointer_cast( + this->scene->NodeByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + gzdbg << "MouseDrag plugin is using camera [" + << this->camera->Name() << "]" << std::endl; + break; + } } } - else if (this->mouseEvent.Button() == common::MouseEvent::RIGHT) + + if (this->mouseEvent.Type() == common::MouseEvent::PRESS && + this->mouseEvent.Control()) { - if (this->mouseEvent.Type() == common::MouseEvent::PRESS && - this->mouseEvent.Control()) + this->blockOrbit = true; + + // Get the visual at mouse position + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + this->visualId = kNullEntity; + try { - gzdbg << "Ctrl-Right click press" << std::endl; - this->blockOrbit = true; + this->visualId = std::get(visual->UserData("gazebo-entity")); } - else if (this->mouseEvent.Type() == common::MouseEvent::RELEASE) + catch(std::bad_variant_access &e) { - gzdbg << "Right click release" << std::endl; - this->blockOrbit = true; + // It's ok to get here } - } - if (this->mouseEvent.Type() == common::MouseEvent::MOVE) + if (this->mouseEvent.Button() == common::MouseEvent::LEFT) + { + gzdbg << "Ctrl-Left click press" << std::endl; + } + else if (this->mouseEvent.Button() == common::MouseEvent::RIGHT) + { + gzdbg << "Ctrl-Right click press" << std::endl; + } + } + else if (this->mouseEvent.Type() == common::MouseEvent::RELEASE) + { + this->blockOrbit = false; + this->visualId = kNullEntity; + gzdbg << "Click release" << std::endl; + } + else if (this->mouseEvent.Type() == common::MouseEvent::MOVE) { gzdbg << "Mouse Move [" << this->mouseEvent.Pos() << "]" << std::endl; } From 508a0ef14a8e693c8a3b838f0b42a8bd436d1bb6 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Sun, 2 Jul 2023 17:22:49 +0200 Subject: [PATCH 03/20] Implement calculation of plane of wrench application and translation mode Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 188 ++++++++++++++++++++---- 1 file changed, 161 insertions(+), 27 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index 6ac870bcaa..1868a1ef16 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -23,6 +23,8 @@ #include #include #include +#include +#include #include #include #include @@ -30,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -37,6 +40,7 @@ #include #include #include +#include #include "MouseDrag.hh" @@ -44,11 +48,26 @@ namespace gz { namespace sim { + /// \enum MouseDragMode + /// \brief Unique identifiers for mouse dragging modes + enum MouseDragMode + { + /// \brief Inactive state + NONE = 0, + /// \brief Rotation mode + ROTATE = 1, + /// \brief Translation mode + TRANSLATE = 2, + }; + class MouseDragPrivate { /// \brief Handle mouse events public: void HandleMouseEvents(); + /// \brief Perform rendering calls in the rendering thread. + public: void OnRender(); + /// \brief Transport node public: transport::Node node; @@ -61,12 +80,18 @@ namespace sim /// \brief User camera public: rendering::CameraPtr camera{nullptr}; + /// \brief Ray query for mouse clicks + public: rendering::RayQueryPtr rayQuery{nullptr}; + /// \brief Holds the latest mouse event public: gz::common::MouseEvent mouseEvent; - /// \brief True if there are new mouse events to process. + /// \brief True if there are new mouse events to process public: bool mouseDirty{false}; + /// \brief True if there are new dragging mode changes to process + public: bool modeDirty{false}; + /// \brief True if the force should be applied to the center of mass public: bool applyCOM{true}; @@ -79,6 +104,15 @@ namespace sim /// \brief Offset of the force application point relative to the /// center of mass public: math::Vector3d offset{0.0, 0.0, 0.0}; + + /// \brief Plane of force application + public: math::Planed plane; + + /// \brief Initial position of a mouse click + public: math::Vector2i mousePressPos = math::Vector2i::Zero; + + /// \brief Current dragging mode + public: MouseDragMode mode = MouseDragMode::NONE; }; } } @@ -128,10 +162,7 @@ bool MouseDrag::eventFilter(QObject *_obj, QEvent *_event) { if (_event->type() == gz::gui::events::Render::kType) { - gz::gui::events::BlockOrbit blockOrbitEvent(this->dataPtr->blockOrbit); - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), - &blockOrbitEvent); + this->dataPtr->OnRender(); } else if (_event->type() == gz::gui::events::MousePressOnScene::kType) { @@ -157,7 +188,8 @@ bool MouseDrag::eventFilter(QObject *_obj, QEvent *_event) void MouseDrag::Update(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { - if (this->dataPtr->visualId == kNullEntity) + if (this->dataPtr->mode == MouseDragMode::NONE || + this->dataPtr->visualId == kNullEntity) { return; } @@ -176,37 +208,103 @@ void MouseDrag::Update(const UpdateInfo &/*_info*/, } auto inertial = _ecm.Component(*linkId); + math::Pose3d linkWorldPose = worldPose(*linkId, _ecm); if (!inertial) { gzdbg << "Link must have an inertial component" << std::endl; return; } - auto centerOfMass = inertial->Data().Pose().Pos(); + auto centerOfMass = + linkWorldPose.Pos() + + linkWorldPose.Rot().RotateVector(inertial->Data().Pose().Pos()); + math::Vector3d applicationPoint = centerOfMass; + if (!this->dataPtr->applyCOM) + { + applicationPoint += + linkWorldPose.Rot().RotateVector(this->dataPtr->offset); + } + + // Recalculate the plane of application when necessary + if (this->dataPtr->modeDirty) + { + this->dataPtr->modeDirty = false; + + // Make a ray query at the center of the image + this->dataPtr->rayQuery->SetFromCamera( + this->dataPtr->camera, math::Vector2d(0, 0)); + + math::Vector3d direction = this->dataPtr->rayQuery->Direction(); + + double planeOffset = direction.AbsDot(applicationPoint); + this->dataPtr->plane = math::Planed(-direction, planeOffset); + + gzdbg << "Mode " << this->dataPtr->mode + << " Entity " << *linkId + << " Point " << applicationPoint + << " Normal " << -direction << std::endl; + } + + math::Vector3d force; + math::Vector3d torque; + + if (this->dataPtr->mode == MouseDragMode::ROTATE) + { + // TODO(anyone): implement + torque = math::Vector3d::Zero; + } + else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) + { + // Normalize point on the image + double width = this->dataPtr->camera->ImageWidth(); + double height = this->dataPtr->camera->ImageHeight(); + + double nx = 2.0 * this->dataPtr->mouseEvent.Pos().X() / width - 1.0; + double ny = 1.0 - 2.0 * this->dataPtr->mouseEvent.Pos().Y() / height; + + // Make a ray query at the mouse position + this->dataPtr->rayQuery->SetFromCamera( + this->dataPtr->camera, math::Vector2d(nx, ny)); + + math::Vector3d origin = this->dataPtr->rayQuery->Origin(); + math::Vector3d direction = this->dataPtr->rayQuery->Direction(); + + auto target = this->dataPtr->plane.Intersection(origin, direction); + if (!target) + { + gzdbg << "No intersection found" << std::endl; + return; + } + math::Vector3d displacement = *target - applicationPoint; + // gzdbg << "Displacement " << displacement << std::endl; + + static math::Vector3d displacementPrev; + + force = 1e3*displacement + 1e4*(displacement-displacementPrev); + displacementPrev = displacement; + } + + msgs::EntityWrench msg; + msg.mutable_entity()->set_id(*linkId); + msgs::Set(msg.mutable_wrench()->mutable_force(), force); + msgs::Set(msg.mutable_wrench()->mutable_torque(), torque); + + this->dataPtr->pub.Publish(msg); } ///////////////////////////////////////////////// -void MouseDrag::OnSwitchCOM(const bool /* _checked */) +void MouseDrag::OnSwitchCOM(const bool _checked) { - // this->dataPtr->applyCOM = _checked; - gzdbg << "Only CoM application is currently supported" - << this->dataPtr->applyCOM << std::endl; + this->dataPtr->applyCOM = _checked; } ///////////////////////////////////////////////// -void MouseDragPrivate::HandleMouseEvents() +void MouseDragPrivate::OnRender() { - // Check for mouse events - if (!this->mouseDirty) - { - return; - } - this->mouseDirty = false; - // Get scene and user camera - if (nullptr == this->scene) + if (!this->scene) { this->scene = rendering::sceneFromFirstRenderEngine(); - if (nullptr == this->scene) + if (!this->scene) { return; } @@ -224,19 +322,47 @@ void MouseDragPrivate::HandleMouseEvents() break; } } + + if (!this->camera) + { + gzerr << "MouseDrag camera is not available" << std::endl; + return; + } + this->rayQuery = this->camera->Scene()->CreateRayQuery(); } + gz::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &blockOrbitEvent); +} + +///////////////////////////////////////////////// +void MouseDragPrivate::HandleMouseEvents() +{ + // Check for mouse events + if (!this->mouseDirty) + { + return; + } + this->mouseDirty = false; + if (this->mouseEvent.Type() == common::MouseEvent::PRESS && this->mouseEvent.Control()) { - this->blockOrbit = true; + this->mousePressPos = this->mouseEvent.Pos(); // Get the visual at mouse position rendering::VisualPtr visual = this->scene->VisualAt( this->camera, this->mouseEvent.Pos()); - this->visualId = kNullEntity; + + if (!visual) + { + return; + } + try { this->visualId = std::get(visual->UserData("gazebo-entity")); @@ -248,22 +374,30 @@ void MouseDragPrivate::HandleMouseEvents() if (this->mouseEvent.Button() == common::MouseEvent::LEFT) { - gzdbg << "Ctrl-Left click press" << std::endl; + // TODO(anyone): revert to rotation mode + // this->mode = MouseDragMode::ROTATE; + this->mode = MouseDragMode::TRANSLATE; + this->modeDirty = true; } else if (this->mouseEvent.Button() == common::MouseEvent::RIGHT) { - gzdbg << "Ctrl-Right click press" << std::endl; + this->mode = MouseDragMode::TRANSLATE; + this->modeDirty = true; } } else if (this->mouseEvent.Type() == common::MouseEvent::RELEASE) { + this->mode = MouseDragMode::NONE; this->blockOrbit = false; this->visualId = kNullEntity; - gzdbg << "Click release" << std::endl; + gzdbg << "Released" << std::endl; } else if (this->mouseEvent.Type() == common::MouseEvent::MOVE) { - gzdbg << "Mouse Move [" << this->mouseEvent.Pos() << "]" << std::endl; + if (this->mode != MouseDragMode::NONE) + { + this->blockOrbit = true; + } } } From 28919d69e905e4cf38fd5d63de724788bb16b825 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Mon, 3 Jul 2023 19:24:50 +0200 Subject: [PATCH 04/20] Implement translation drag by arbitrary point in object Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 110 +++++++++++++++--------- 1 file changed, 70 insertions(+), 40 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index 1868a1ef16..3513d13982 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -93,16 +93,18 @@ namespace sim public: bool modeDirty{false}; /// \brief True if the force should be applied to the center of mass - public: bool applyCOM{true}; + public: bool applyCOM{false}; /// \brief Block orbit public: bool blockOrbit{false}; - /// \brief Visual of the Link to which apply the wrenches + /// \brief Visual of the Link to which the wrenches are applied public: Entity visualId; + math::Vector3d applicationPoint; + /// \brief Offset of the force application point relative to the - /// center of mass + /// link origin public: math::Vector3d offset{0.0, 0.0, 0.0}; /// \brief Plane of force application @@ -164,17 +166,31 @@ bool MouseDrag::eventFilter(QObject *_obj, QEvent *_event) { this->dataPtr->OnRender(); } + else if (_event->type() == gz::gui::events::LeftClickOnScene::kType) + { + auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == gz::gui::events::RightClickOnScene::kType) + { + auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } else if (_event->type() == gz::gui::events::MousePressOnScene::kType) { auto event = - static_cast(_event); + static_cast(_event); this->dataPtr->mouseEvent = event->Mouse(); this->dataPtr->mouseDirty = true; } else if (_event->type() == gz::gui::events::DragOnScene::kType) { auto event = - static_cast(_event); + static_cast(_event); this->dataPtr->mouseEvent = event->Mouse(); this->dataPtr->mouseDirty = true; } @@ -185,11 +201,12 @@ bool MouseDrag::eventFilter(QObject *_obj, QEvent *_event) } ///////////////////////////////////////////////// -void MouseDrag::Update(const UpdateInfo &/*_info*/, +void MouseDrag::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { if (this->dataPtr->mode == MouseDragMode::NONE || - this->dataPtr->visualId == kNullEntity) + this->dataPtr->visualId == kNullEntity || + _info.paused) { return; } @@ -207,44 +224,51 @@ void MouseDrag::Update(const UpdateInfo &/*_info*/, } } + auto linkWorldPose = worldPose(*linkId, _ecm); auto inertial = _ecm.Component(*linkId); - math::Pose3d linkWorldPose = worldPose(*linkId, _ecm); if (!inertial) { - gzdbg << "Link must have an inertial component" << std::endl; return; } - auto centerOfMass = - linkWorldPose.Pos() + - linkWorldPose.Rot().RotateVector(inertial->Data().Pose().Pos()); - math::Vector3d applicationPoint = centerOfMass; - if (!this->dataPtr->applyCOM) - { - applicationPoint += - linkWorldPose.Rot().RotateVector(this->dataPtr->offset); - } - // Recalculate the plane of application when necessary if (this->dataPtr->modeDirty) { this->dataPtr->modeDirty = false; - // Make a ray query at the center of the image + if (this->dataPtr->mode == MouseDragMode::TRANSLATE) + { + // Calculate offset of force application from link origin + if (this->dataPtr->applyCOM) + { + this->dataPtr->offset = inertial->Data().Pose().Pos(); + this->dataPtr->applicationPoint = + linkWorldPose.Pos() + + linkWorldPose.Rot().RotateVector(this->dataPtr->offset); + } + else + { + this->dataPtr->offset = linkWorldPose.Rot().RotateVectorReverse( + this->dataPtr->applicationPoint - linkWorldPose.Pos()); + } + } + + // The plane of wrench application should be normal to the center + // of the camera view and pass through the application point this->dataPtr->rayQuery->SetFromCamera( this->dataPtr->camera, math::Vector2d(0, 0)); - math::Vector3d direction = this->dataPtr->rayQuery->Direction(); - - double planeOffset = direction.AbsDot(applicationPoint); - this->dataPtr->plane = math::Planed(-direction, planeOffset); - - gzdbg << "Mode " << this->dataPtr->mode - << " Entity " << *linkId - << " Point " << applicationPoint - << " Normal " << -direction << std::endl; + double planeOffset = direction.Dot(this->dataPtr->applicationPoint); + this->dataPtr->plane = math::Planed(direction, planeOffset); + } + else + { + this->dataPtr->applicationPoint = + linkWorldPose.Pos() + + linkWorldPose.Rot().RotateVector(this->dataPtr->offset); } math::Vector3d force; + math::Vector3d offsetFromCoM; math::Vector3d torque; if (this->dataPtr->mode == MouseDragMode::ROTATE) @@ -271,21 +295,22 @@ void MouseDrag::Update(const UpdateInfo &/*_info*/, auto target = this->dataPtr->plane.Intersection(origin, direction); if (!target) { - gzdbg << "No intersection found" << std::endl; return; } - math::Vector3d displacement = *target - applicationPoint; - // gzdbg << "Displacement " << displacement << std::endl; + math::Vector3d displacement = *target - this->dataPtr->applicationPoint; static math::Vector3d displacementPrev; force = 1e3*displacement + 1e4*(displacement-displacementPrev); displacementPrev = displacement; + + offsetFromCoM = this->dataPtr->offset - inertial->Data().Pose().Pos(); } msgs::EntityWrench msg; msg.mutable_entity()->set_id(*linkId); msgs::Set(msg.mutable_wrench()->mutable_force(), force); + // msgs::Set(msg.mutable_wrench()->mutable_force_offset(), offsetFromCoM); msgs::Set(msg.mutable_wrench()->mutable_torque(), torque); this->dataPtr->pub.Publish(msg); @@ -328,7 +353,7 @@ void MouseDragPrivate::OnRender() gzerr << "MouseDrag camera is not available" << std::endl; return; } - this->rayQuery = this->camera->Scene()->CreateRayQuery(); + this->rayQuery = this->scene->CreateRayQuery(); } gz::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); @@ -348,9 +373,11 @@ void MouseDragPrivate::HandleMouseEvents() this->mouseDirty = false; if (this->mouseEvent.Type() == common::MouseEvent::PRESS && - this->mouseEvent.Control()) + this->mouseEvent.Control() && + this->mouseEvent.Button() != common::MouseEvent::MIDDLE) { this->mousePressPos = this->mouseEvent.Pos(); + this->blockOrbit = true; // Get the visual at mouse position rendering::VisualPtr visual = this->scene->VisualAt( @@ -363,6 +390,14 @@ void MouseDragPrivate::HandleMouseEvents() return; } + if (!this->applyCOM) + { + this->applicationPoint = rendering::screenToScene( + this->mousePressPos, this->camera, + this->rayQuery); + } + this->modeDirty = true; + try { this->visualId = std::get(visual->UserData("gazebo-entity")); @@ -374,15 +409,11 @@ void MouseDragPrivate::HandleMouseEvents() if (this->mouseEvent.Button() == common::MouseEvent::LEFT) { - // TODO(anyone): revert to rotation mode - // this->mode = MouseDragMode::ROTATE; - this->mode = MouseDragMode::TRANSLATE; - this->modeDirty = true; + this->mode = MouseDragMode::ROTATE; } else if (this->mouseEvent.Button() == common::MouseEvent::RIGHT) { this->mode = MouseDragMode::TRANSLATE; - this->modeDirty = true; } } else if (this->mouseEvent.Type() == common::MouseEvent::RELEASE) @@ -390,7 +421,6 @@ void MouseDragPrivate::HandleMouseEvents() this->mode = MouseDragMode::NONE; this->blockOrbit = false; this->visualId = kNullEntity; - gzdbg << "Released" << std::endl; } else if (this->mouseEvent.Type() == common::MouseEvent::MOVE) { From 603e949dd1cf28ae733125a5a448b7d43add2930 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Sat, 8 Jul 2023 15:55:38 -0300 Subject: [PATCH 05/20] Implement rotation dragging Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 131 ++++++++++++++++-------- 1 file changed, 91 insertions(+), 40 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index 3513d13982..090b8704fd 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -39,8 +39,10 @@ #include #include #include +#include #include #include +#include #include "MouseDrag.hh" @@ -101,17 +103,21 @@ namespace sim /// \brief Visual of the Link to which the wrenches are applied public: Entity visualId; + /// \brief Application point of the force in world coordinates math::Vector3d applicationPoint; /// \brief Offset of the force application point relative to the - /// link origin + /// link origin, written in the link-fixed frame public: math::Vector3d offset{0.0, 0.0, 0.0}; /// \brief Plane of force application public: math::Planed plane; /// \brief Initial position of a mouse click - public: math::Vector2i mousePressPos = math::Vector2i::Zero; + public: math::Vector2i mousePressPos{0, 0}; + + /// \brief Initial world rotation of the link during mouse click + public: math::Quaterniond initialRot; /// \brief Current dragging mode public: MouseDragMode mode = MouseDragMode::NONE; @@ -235,7 +241,12 @@ void MouseDrag::Update(const UpdateInfo &_info, { this->dataPtr->modeDirty = false; - if (this->dataPtr->mode == MouseDragMode::TRANSLATE) + if (this->dataPtr->mode == MouseDragMode::ROTATE) + { + this->dataPtr->mousePressPos = this->dataPtr->mouseEvent.Pos(); + this->dataPtr->initialRot = linkWorldPose.Rot(); + } + else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) { // Calculate offset of force application from link origin if (this->dataPtr->applyCOM) @@ -254,17 +265,39 @@ void MouseDrag::Update(const UpdateInfo &_info, // The plane of wrench application should be normal to the center // of the camera view and pass through the application point - this->dataPtr->rayQuery->SetFromCamera( - this->dataPtr->camera, math::Vector2d(0, 0)); - math::Vector3d direction = this->dataPtr->rayQuery->Direction(); + math::Vector3d direction = this->dataPtr->camera->WorldPose().Rot().XAxis(); double planeOffset = direction.Dot(this->dataPtr->applicationPoint); this->dataPtr->plane = math::Planed(direction, planeOffset); } else { - this->dataPtr->applicationPoint = - linkWorldPose.Pos() + - linkWorldPose.Rot().RotateVector(this->dataPtr->offset); + // TODO(anyone): can this become an else if? + if (this->dataPtr->mode == MouseDragMode::TRANSLATE) + { + this->dataPtr->applicationPoint = + linkWorldPose.Pos() + + linkWorldPose.Rot().RotateVector(this->dataPtr->offset); + } + } + + // Normalize point on the image + double width = this->dataPtr->camera->ImageWidth(); + double height = this->dataPtr->camera->ImageHeight(); + + double nx = 2.0 * this->dataPtr->mouseEvent.Pos().X() / width - 1.0; + double ny = 1.0 - 2.0 * this->dataPtr->mouseEvent.Pos().Y() / height; + + // Make a ray query at the mouse position + this->dataPtr->rayQuery->SetFromCamera( + this->dataPtr->camera, math::Vector2d(nx, ny)); + + math::Vector3d origin = this->dataPtr->rayQuery->Origin(); + math::Vector3d direction = this->dataPtr->rayQuery->Direction(); + + auto target = this->dataPtr->plane.Intersection(origin, direction); + if (!target) + { + return; } math::Vector3d force; @@ -273,35 +306,55 @@ void MouseDrag::Update(const UpdateInfo &_info, if (this->dataPtr->mode == MouseDragMode::ROTATE) { - // TODO(anyone): implement - torque = math::Vector3d::Zero; + // Calculate rotation axes + math::Pose3d camPose = this->dataPtr->camera->WorldPose(); + math::Vector3d verticalAxis(camPose.Rot().ZAxis()); + math::Vector3d horizontalAxis(-camPose.Rot().YAxis()); + + // Map mouse displacement with desired pose + math::Vector3d targetHorizontal = + (*target - this->dataPtr->applicationPoint).Dot(horizontalAxis) * + horizontalAxis + this->dataPtr->applicationPoint; + math::Vector3d targetVertical = + (*target - this->dataPtr->applicationPoint).Dot(verticalAxis) * + verticalAxis + this->dataPtr->applicationPoint; + + math::Vector3d start = + (this->dataPtr->applicationPoint - camPose.Pos()).Normalized(); + math::Vector3d endHorizontal = + (targetHorizontal - camPose.Pos()).Normalized(); + math::Vector3d endVertical = (targetVertical - camPose.Pos()).Normalized(); + + double angleHorizontal = atan2( + start.Cross(endHorizontal).Length(), start.Dot(endHorizontal)); + if (this->dataPtr->mouseEvent.Pos().X() < this->dataPtr->mousePressPos.X()) + angleHorizontal = -angleHorizontal; + + double angleVertical = atan2( + start.Cross(endVertical).Length(), start.Dot(endVertical)); + if (this->dataPtr->mouseEvent.Pos().Y() < this->dataPtr->mousePressPos.Y()) + angleVertical = -angleVertical; + + math::Quaterniond desiredRot = + math::Quaterniond(verticalAxis, angleHorizontal) * + math::Quaterniond(horizontalAxis, angleVertical); + + // Calculate the necessary rotation and torque + math::Quaterniond displacementRot = + linkWorldPose.Rot().Inverse() * (this->dataPtr->initialRot * desiredRot); + math::Vector3d displacementVec = displacementRot.Euler(); + static math::Vector3d displacementVecPrev; + + torque = + 1e3 * displacementVec + 1e4 * (displacementVec - displacementVecPrev); + displacementVecPrev = displacementVec; } else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) { - // Normalize point on the image - double width = this->dataPtr->camera->ImageWidth(); - double height = this->dataPtr->camera->ImageHeight(); - - double nx = 2.0 * this->dataPtr->mouseEvent.Pos().X() / width - 1.0; - double ny = 1.0 - 2.0 * this->dataPtr->mouseEvent.Pos().Y() / height; - - // Make a ray query at the mouse position - this->dataPtr->rayQuery->SetFromCamera( - this->dataPtr->camera, math::Vector2d(nx, ny)); - - math::Vector3d origin = this->dataPtr->rayQuery->Origin(); - math::Vector3d direction = this->dataPtr->rayQuery->Direction(); - - auto target = this->dataPtr->plane.Intersection(origin, direction); - if (!target) - { - return; - } math::Vector3d displacement = *target - this->dataPtr->applicationPoint; - static math::Vector3d displacementPrev; - force = 1e3*displacement + 1e4*(displacement-displacementPrev); + force = 1e3 * displacement + 1e4 * (displacement - displacementPrev); displacementPrev = displacement; offsetFromCoM = this->dataPtr->offset - inertial->Data().Pose().Pos(); @@ -311,6 +364,8 @@ void MouseDrag::Update(const UpdateInfo &_info, msg.mutable_entity()->set_id(*linkId); msgs::Set(msg.mutable_wrench()->mutable_force(), force); // msgs::Set(msg.mutable_wrench()->mutable_force_offset(), offsetFromCoM); + torque += linkWorldPose.Rot().RotateVector( + offsetFromCoM + inertial->Data().Pose().Pos()).Cross(force); msgs::Set(msg.mutable_wrench()->mutable_torque(), torque); this->dataPtr->pub.Publish(msg); @@ -376,8 +431,8 @@ void MouseDragPrivate::HandleMouseEvents() this->mouseEvent.Control() && this->mouseEvent.Button() != common::MouseEvent::MIDDLE) { - this->mousePressPos = this->mouseEvent.Pos(); this->blockOrbit = true; + this->modeDirty = true; // Get the visual at mouse position rendering::VisualPtr visual = this->scene->VisualAt( @@ -390,13 +445,9 @@ void MouseDragPrivate::HandleMouseEvents() return; } - if (!this->applyCOM) - { - this->applicationPoint = rendering::screenToScene( - this->mousePressPos, this->camera, - this->rayQuery); - } - this->modeDirty = true; + this->applicationPoint = rendering::screenToScene( + this->mouseEvent.Pos(), this->camera, + this->rayQuery); try { From 13836358386a47835a817a9945f8957b1c7a126e Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Tue, 11 Jul 2023 11:05:16 -0300 Subject: [PATCH 06/20] Improve rotation calculation and implement velocity damping Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 85 ++++++++++++------------- 1 file changed, 40 insertions(+), 45 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index 090b8704fd..80eae77050 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -103,18 +103,18 @@ namespace sim /// \brief Visual of the Link to which the wrenches are applied public: Entity visualId; - /// \brief Application point of the force in world coordinates + /// \brief Application point of the wrench in world coordinates math::Vector3d applicationPoint; /// \brief Offset of the force application point relative to the - /// link origin, written in the link-fixed frame + /// link origin, expressed in the link-fixed frame public: math::Vector3d offset{0.0, 0.0, 0.0}; /// \brief Plane of force application public: math::Planed plane; /// \brief Initial position of a mouse click - public: math::Vector2i mousePressPos{0, 0}; + public: math::Vector2i mousePressPos; /// \brief Initial world rotation of the link during mouse click public: math::Quaterniond initialRot; @@ -229,6 +229,7 @@ void MouseDrag::Update(const UpdateInfo &_info, return; } } + link.EnableVelocityChecks(_ecm, true); auto linkWorldPose = worldPose(*linkId, _ecm); auto inertial = _ecm.Component(*linkId); @@ -271,7 +272,6 @@ void MouseDrag::Update(const UpdateInfo &_info, } else { - // TODO(anyone): can this become an else if? if (this->dataPtr->mode == MouseDragMode::TRANSLATE) { this->dataPtr->applicationPoint = @@ -280,7 +280,7 @@ void MouseDrag::Update(const UpdateInfo &_info, } } - // Normalize point on the image + // Normalize mouse position on the image double width = this->dataPtr->camera->ImageWidth(); double height = this->dataPtr->camera->ImageHeight(); @@ -307,65 +307,59 @@ void MouseDrag::Update(const UpdateInfo &_info, if (this->dataPtr->mode == MouseDragMode::ROTATE) { // Calculate rotation axes - math::Pose3d camPose = this->dataPtr->camera->WorldPose(); - math::Vector3d verticalAxis(camPose.Rot().ZAxis()); - math::Vector3d horizontalAxis(-camPose.Rot().YAxis()); - - // Map mouse displacement with desired pose - math::Vector3d targetHorizontal = - (*target - this->dataPtr->applicationPoint).Dot(horizontalAxis) * - horizontalAxis + this->dataPtr->applicationPoint; - math::Vector3d targetVertical = - (*target - this->dataPtr->applicationPoint).Dot(verticalAxis) * - verticalAxis + this->dataPtr->applicationPoint; - + math::Vector3d camPos = this->dataPtr->camera->WorldPose().Pos(); math::Vector3d start = - (this->dataPtr->applicationPoint - camPose.Pos()).Normalized(); - math::Vector3d endHorizontal = - (targetHorizontal - camPose.Pos()).Normalized(); - math::Vector3d endVertical = (targetVertical - camPose.Pos()).Normalized(); - - double angleHorizontal = atan2( - start.Cross(endHorizontal).Length(), start.Dot(endHorizontal)); - if (this->dataPtr->mouseEvent.Pos().X() < this->dataPtr->mousePressPos.X()) - angleHorizontal = -angleHorizontal; + (this->dataPtr->applicationPoint - camPos).Normalized(); + math::Vector3d end = (*target - camPos).Normalized(); - double angleVertical = atan2( - start.Cross(endVertical).Length(), start.Dot(endVertical)); - if (this->dataPtr->mouseEvent.Pos().Y() < this->dataPtr->mousePressPos.Y()) - angleVertical = -angleVertical; + math::Vector3d axis = start.Cross(end); + double angle = -atan2(axis.Length(), start.Dot(end)); + axis -= axis.Dot(this->dataPtr->plane.Normal()) * + this->dataPtr->plane.Normal(); + axis.Normalize(); - math::Quaterniond desiredRot = - math::Quaterniond(verticalAxis, angleHorizontal) * - math::Quaterniond(horizontalAxis, angleVertical); + // Desired change in rotation relative to the rotation on mouse click + math::Quaterniond desiredRot = math::Quaterniond(axis, angle); // Calculate the necessary rotation and torque math::Quaterniond displacementRot = linkWorldPose.Rot().Inverse() * (this->dataPtr->initialRot * desiredRot); - math::Vector3d displacementVec = displacementRot.Euler(); - static math::Vector3d displacementVecPrev; + math::Vector3d angularVel; + if (auto v = link.WorldAngularVelocity(_ecm)) + { + angularVel = *v; + gzdbg << "angularVel [" << angularVel << "]" << std::endl; + } torque = - 1e3 * displacementVec + 1e4 * (displacementVec - displacementVecPrev); - displacementVecPrev = displacementVec; + 1e3 * displacementRot.Euler() - 5e2 * angularVel; } else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) { math::Vector3d displacement = *target - this->dataPtr->applicationPoint; - static math::Vector3d displacementPrev; - - force = 1e3 * displacement + 1e4 * (displacement - displacementPrev); - displacementPrev = displacement; + math::Vector3d linearVel; + math::Vector3d angularVel; + if (auto v = link.WorldLinearVelocity(_ecm)) + { + linearVel = *v; + gzdbg << "linearVel [" << linearVel << "]" << std::endl; + } + if (auto v = link.WorldAngularVelocity(_ecm)) + { + angularVel = *v; + gzdbg << "angularVel [" << angularVel << "]" << std::endl; + } - offsetFromCoM = this->dataPtr->offset - inertial->Data().Pose().Pos(); + force = 1e3 * displacement - 1e2 * linearVel; + torque = + linkWorldPose.Rot().RotateVector(this->dataPtr->offset).Cross(force) - + 1e1 * angularVel; } + // Publish wrench msgs::EntityWrench msg; msg.mutable_entity()->set_id(*linkId); msgs::Set(msg.mutable_wrench()->mutable_force(), force); - // msgs::Set(msg.mutable_wrench()->mutable_force_offset(), offsetFromCoM); - torque += linkWorldPose.Rot().RotateVector( - offsetFromCoM + inertial->Data().Pose().Pos()).Cross(force); msgs::Set(msg.mutable_wrench()->mutable_torque(), torque); this->dataPtr->pub.Publish(msg); @@ -445,6 +439,7 @@ void MouseDragPrivate::HandleMouseEvents() return; } + // Get the 3D coordinates of the clicked point this->applicationPoint = rendering::screenToScene( this->mouseEvent.Pos(), this->camera, this->rayQuery); From 0f685702696c535115705dcd2e67690e407cb224 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Fri, 14 Jul 2023 13:14:53 -0300 Subject: [PATCH 07/20] Use PID for wrench calculation and implement visualization Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 276 +++++++++++++++++------- 1 file changed, 198 insertions(+), 78 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index 090b8704fd..e6657f82d3 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -25,10 +25,12 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -43,6 +45,8 @@ #include #include #include +#include +#include #include "MouseDrag.hh" @@ -70,6 +74,16 @@ namespace sim /// \brief Perform rendering calls in the rendering thread. public: void OnRender(); + /// \brief Update the PID loop on 3 coordinates and return the command value + public: math::Vector3d CalculatePID( + math::PID _pid[3], + const math::Vector3d &_error, + const std::chrono::duration &_dt); + + /// \brief Update the PID gains with critical damping + public: void UpdateGains(math::Pose3d _linkWorldPose, + math::Inertiald _inertial); + /// \brief Transport node public: transport::Node node; @@ -100,27 +114,48 @@ namespace sim /// \brief Block orbit public: bool blockOrbit{false}; + /// \brief True if BlockOrbit events should be sent + public: bool sendBlockOrbit{false}; + /// \brief Visual of the Link to which the wrenches are applied public: Entity visualId; - /// \brief Application point of the force in world coordinates + /// \brief Application point of the wrench in world coordinates math::Vector3d applicationPoint; + /// \brief Point to which the link is dragged to, in translation mode + math::Vector3d target; + /// \brief Offset of the force application point relative to the - /// link origin, written in the link-fixed frame + /// link origin, expressed in the link-fixed frame public: math::Vector3d offset{0.0, 0.0, 0.0}; /// \brief Plane of force application public: math::Planed plane; /// \brief Initial position of a mouse click - public: math::Vector2i mousePressPos{0, 0}; + public: math::Vector2i mousePressPos; /// \brief Initial world rotation of the link during mouse click public: math::Quaterniond initialRot; /// \brief Current dragging mode public: MouseDragMode mode = MouseDragMode::NONE; + + /// \brief PID controllers for translation + public: math::PID posPid[3]; + + /// \brief PID controllers for rotation + public: math::PID rotPid[3]; + + /// \brief Spring stiffness for translation, in (m/s²)/m + public: double posStiffness = 100; + + /// \brief Spring stiffness for rotation + public: double rotStiffness = 100; + + /// \brief Arrow for visualizing wrench + public: rendering::ArrowVisualPtr arrowVisual; }; } } @@ -211,9 +246,9 @@ void MouseDrag::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { if (this->dataPtr->mode == MouseDragMode::NONE || - this->dataPtr->visualId == kNullEntity || _info.paused) { + this->dataPtr->mode = MouseDragMode::NONE; return; } @@ -236,6 +271,7 @@ void MouseDrag::Update(const UpdateInfo &_info, { return; } + this->dataPtr->UpdateGains(linkWorldPose, inertial->Data()); if (this->dataPtr->modeDirty) { @@ -268,19 +304,23 @@ void MouseDrag::Update(const UpdateInfo &_info, math::Vector3d direction = this->dataPtr->camera->WorldPose().Rot().XAxis(); double planeOffset = direction.Dot(this->dataPtr->applicationPoint); this->dataPtr->plane = math::Planed(direction, planeOffset); - } - else - { - // TODO(anyone): can this become an else if? - if (this->dataPtr->mode == MouseDragMode::TRANSLATE) + + gzdbg << "Resetting PIDs" << std::endl; + for (auto i : {0, 1, 2}) { - this->dataPtr->applicationPoint = - linkWorldPose.Pos() + - linkWorldPose.Rot().RotateVector(this->dataPtr->offset); + this->dataPtr->posPid[i].Reset(); + this->dataPtr->rotPid[i].Reset(); } } + // Track the application point in translation mode + else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) + { + this->dataPtr->applicationPoint = + linkWorldPose.Pos() + + linkWorldPose.Rot().RotateVector(this->dataPtr->offset); + } - // Normalize point on the image + // Normalize mouse position on the image double width = this->dataPtr->camera->ImageWidth(); double height = this->dataPtr->camera->ImageHeight(); @@ -294,78 +334,53 @@ void MouseDrag::Update(const UpdateInfo &_info, math::Vector3d origin = this->dataPtr->rayQuery->Origin(); math::Vector3d direction = this->dataPtr->rayQuery->Direction(); - auto target = this->dataPtr->plane.Intersection(origin, direction); - if (!target) - { + if (auto t = this->dataPtr->plane.Intersection(origin, direction)) + this->dataPtr->target = *t; + else return; - } math::Vector3d force; - math::Vector3d offsetFromCoM; math::Vector3d torque; + math::Quaterniond errorRot; + math::Vector3d errorPos; if (this->dataPtr->mode == MouseDragMode::ROTATE) { - // Calculate rotation axes - math::Pose3d camPose = this->dataPtr->camera->WorldPose(); - math::Vector3d verticalAxis(camPose.Rot().ZAxis()); - math::Vector3d horizontalAxis(-camPose.Rot().YAxis()); - - // Map mouse displacement with desired pose - math::Vector3d targetHorizontal = - (*target - this->dataPtr->applicationPoint).Dot(horizontalAxis) * - horizontalAxis + this->dataPtr->applicationPoint; - math::Vector3d targetVertical = - (*target - this->dataPtr->applicationPoint).Dot(verticalAxis) * - verticalAxis + this->dataPtr->applicationPoint; - + // Calculate rotation angle from mouse displacement + math::Vector3d camPos = this->dataPtr->camera->WorldPose().Pos(); math::Vector3d start = - (this->dataPtr->applicationPoint - camPose.Pos()).Normalized(); - math::Vector3d endHorizontal = - (targetHorizontal - camPose.Pos()).Normalized(); - math::Vector3d endVertical = (targetVertical - camPose.Pos()).Normalized(); - - double angleHorizontal = atan2( - start.Cross(endHorizontal).Length(), start.Dot(endHorizontal)); - if (this->dataPtr->mouseEvent.Pos().X() < this->dataPtr->mousePressPos.X()) - angleHorizontal = -angleHorizontal; + (this->dataPtr->applicationPoint - camPos).Normalized(); + math::Vector3d end = (this->dataPtr->target - camPos).Normalized(); + math::Vector3d axis = start.Cross(end); + double angle = -atan2(axis.Length(), start.Dot(end)); - double angleVertical = atan2( - start.Cross(endVertical).Length(), start.Dot(endVertical)); - if (this->dataPtr->mouseEvent.Pos().Y() < this->dataPtr->mousePressPos.Y()) - angleVertical = -angleVertical; - - math::Quaterniond desiredRot = - math::Quaterniond(verticalAxis, angleHorizontal) * - math::Quaterniond(horizontalAxis, angleVertical); + // Project rotation axis onto plane + axis -= axis.Dot(this->dataPtr->plane.Normal()) * + this->dataPtr->plane.Normal(); + axis.Normalize(); // Calculate the necessary rotation and torque - math::Quaterniond displacementRot = - linkWorldPose.Rot().Inverse() * (this->dataPtr->initialRot * desiredRot); - math::Vector3d displacementVec = displacementRot.Euler(); - static math::Vector3d displacementVecPrev; - - torque = - 1e3 * displacementVec + 1e4 * (displacementVec - displacementVecPrev); - displacementVecPrev = displacementVec; + errorRot = + (this->dataPtr->initialRot * math::Quaterniond(axis, angle)).Inverse() * + linkWorldPose.Rot(); + torque = this->dataPtr->CalculatePID( + this->dataPtr->rotPid, errorRot.Euler(), _info.dt); } else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) { - math::Vector3d displacement = *target - this->dataPtr->applicationPoint; - static math::Vector3d displacementPrev; - - force = 1e3 * displacement + 1e4 * (displacement - displacementPrev); - displacementPrev = displacement; - - offsetFromCoM = this->dataPtr->offset - inertial->Data().Pose().Pos(); + errorPos = this->dataPtr->applicationPoint - this->dataPtr->target; + force = this->dataPtr->CalculatePID( + this->dataPtr->posPid, errorPos, _info.dt); + torque = + linkWorldPose.Rot().RotateVector(this->dataPtr->offset).Cross(force) + + this->dataPtr->CalculatePID( + this->dataPtr->rotPid, linkWorldPose.Rot().Euler(), _info.dt); } + // Publish wrench msgs::EntityWrench msg; msg.mutable_entity()->set_id(*linkId); msgs::Set(msg.mutable_wrench()->mutable_force(), force); - // msgs::Set(msg.mutable_wrench()->mutable_force_offset(), offsetFromCoM); - torque += linkWorldPose.Rot().RotateVector( - offsetFromCoM + inertial->Data().Pose().Pos()).Cross(force); msgs::Set(msg.mutable_wrench()->mutable_torque(), torque); this->dataPtr->pub.Publish(msg); @@ -409,12 +424,68 @@ void MouseDragPrivate::OnRender() return; } this->rayQuery = this->scene->CreateRayQuery(); + this->arrowVisual = this->scene->CreateArrowVisual(); + this->arrowVisual->SetMaterial("Default/TransYellow"); } - gz::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), - &blockOrbitEvent); + // Update the visualization + if (this->mode == MouseDragMode::NONE) + { + this->arrowVisual->ShowArrowHead(false); + this->arrowVisual->ShowArrowShaft(false); + this->arrowVisual->ShowArrowRotation(false); + } + else + { + math::Vector3d axisDir = this->target - this->applicationPoint; + math::Vector3d u = axisDir.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); + + if (this->mode == MouseDragMode::ROTATE) + { + quat = + math::Quaterniond(this->camera->WorldPose().Rot().XAxis(), GZ_PI/2) * + quat; + double scale = axisDir.Length() / 0.075f; + + this->arrowVisual->SetLocalPosition(this->applicationPoint); + this->arrowVisual->SetLocalRotation(quat); + this->arrowVisual->SetLocalScale(scale, scale, 0); + + this->arrowVisual->ShowArrowHead(false); + this->arrowVisual->ShowArrowShaft(false); + this->arrowVisual->ShowArrowRotation(true); + } + if (this->mode == MouseDragMode::TRANSLATE) + { + this->arrowVisual->SetLocalPosition(this->applicationPoint); + this->arrowVisual->SetLocalRotation(quat); + this->arrowVisual->SetLocalScale(1, 1, axisDir.Length()); + + this->arrowVisual->ShowArrowHead(true); + this->arrowVisual->ShowArrowShaft(true); + this->arrowVisual->ShowArrowRotation(false); + } + } + + if (this->sendBlockOrbit) + { + // Events with false should only be sent once + if (!this->blockOrbit) + this->sendBlockOrbit = false; + + gz::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &blockOrbitEvent); + } } ///////////////////////////////////////////////// @@ -432,32 +503,39 @@ void MouseDragPrivate::HandleMouseEvents() this->mouseEvent.Button() != common::MouseEvent::MIDDLE) { this->blockOrbit = true; + this->sendBlockOrbit = true; this->modeDirty = true; + gui::events::DeselectAllEntities event(true); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &event); + // Get the visual at mouse position rendering::VisualPtr visual = this->scene->VisualAt( this->camera, this->mouseEvent.Pos()); - this->visualId = kNullEntity; if (!visual) { + this->mode = MouseDragMode::NONE; return; } - - this->applicationPoint = rendering::screenToScene( - this->mouseEvent.Pos(), this->camera, - this->rayQuery); - try { this->visualId = std::get(visual->UserData("gazebo-entity")); } catch(std::bad_variant_access &e) { - // It's ok to get here + this->mode = MouseDragMode::NONE; + return; } + // Get the 3D coordinates of the clicked point + this->applicationPoint = rendering::screenToScene( + this->mouseEvent.Pos(), this->camera, + this->rayQuery); + if (this->mouseEvent.Button() == common::MouseEvent::LEFT) { this->mode = MouseDragMode::ROTATE; @@ -471,13 +549,55 @@ void MouseDragPrivate::HandleMouseEvents() { this->mode = MouseDragMode::NONE; this->blockOrbit = false; - this->visualId = kNullEntity; + this->sendBlockOrbit = true; } else if (this->mouseEvent.Type() == common::MouseEvent::MOVE) { if (this->mode != MouseDragMode::NONE) { this->blockOrbit = true; + this->sendBlockOrbit = true; + } + } +} + +///////////////////////////////////////////////// +math::Vector3d MouseDragPrivate::CalculatePID( + math::PID _pid[3], + const math::Vector3d &_error, + const std::chrono::duration &_dt) +{ + math::Vector3d result; + for (auto i : {0, 1, 2}) + { + result[i] = _pid[i].Update(_error[i], _dt); + } + return result; +} + +///////////////////////////////////////////////// +void MouseDragPrivate::UpdateGains(math::Pose3d _linkWorldPose, + math::Inertiald _inertial) +{ + math::Matrix3d R(_linkWorldPose.Rot() * _inertial.Pose().Rot()); + math::Matrix3d inertia = R * _inertial.Moi() * R.Transposed(); + if (this->mode == MouseDragMode::ROTATE) + { + for (auto i : {0, 1, 2}) + { + this->rotPid[i].SetPGain(this->rotStiffness * inertia(i, i)); + this->rotPid[i].SetDGain(2 * sqrt(this->rotStiffness) * inertia(i, i)); + } + } + else if (this->mode == MouseDragMode::TRANSLATE) + { + double mass = _inertial.MassMatrix().Mass(); + for (auto i : {0, 1, 2}) + { + this->posPid[i].SetPGain(this->posStiffness * mass); + this->posPid[i].SetDGain(2 * sqrt(this->posStiffness) * mass); + this->rotPid[i].SetPGain(0); + this->rotPid[i].SetDGain(sqrt(this->rotStiffness) * inertia(i, i)); } } } From 2b36d85532bce38e148f108f0b90c3fca4d23046 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Mon, 17 Jul 2023 16:07:33 -0300 Subject: [PATCH 08/20] Improve visualization for rotation Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 149 +++++++++++++----------- 1 file changed, 84 insertions(+), 65 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index f214bbe569..9eb4369b1c 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -19,34 +19,34 @@ #include #include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include #include -#include #include +#include #include -#include #include +#include +#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include "MouseDrag.hh" @@ -117,15 +117,24 @@ namespace sim /// \brief True if BlockOrbit events should be sent public: bool sendBlockOrbit{false}; + /// \brief True if the dragging mode is active + public: bool dragActive{false}; + /// \brief Visual of the Link to which the wrenches are applied public: Entity visualId; /// \brief Application point of the wrench in world coordinates math::Vector3d applicationPoint; + /// \brief Initial world rotation of the link during mouse click + public: math::Quaterniond initialRot; + /// \brief Point to which the link is dragged to, in translation mode math::Vector3d target; + /// \brief Goal pose for rotation mode + public: math::Pose3d goalPose; + /// \brief Offset of the force application point relative to the /// link origin, expressed in the link-fixed frame public: math::Vector3d offset{0.0, 0.0, 0.0}; @@ -136,9 +145,6 @@ namespace sim /// \brief Initial position of a mouse click public: math::Vector2i mousePressPos; - /// \brief Initial world rotation of the link during mouse click - public: math::Quaterniond initialRot; - /// \brief Current dragging mode public: MouseDragMode mode = MouseDragMode::NONE; @@ -154,8 +160,15 @@ namespace sim /// \brief Spring stiffness for rotation public: double rotStiffness = 100; - /// \brief Arrow for visualizing wrench + /// \brief Arrow for visualizing force in translation mode. + /// This arrow goes from the application point to the target point. public: rendering::ArrowVisualPtr arrowVisual; + + /// \brief Box for visualizing rotation mode + public: rendering::VisualPtr boxVisual; + + /// \brief Size of the bounding box of the selected link + public: math::Vector3d bboxSize; }; } } @@ -196,8 +209,6 @@ void MouseDrag::LoadConfig(const tinyxml2::XMLElement */*_pluginElem*/) gz::gui::App()->findChild ()->installEventFilter(this); - gz::gui::App()->findChild - ()->QuickWindow()->installEventFilter(this); } ///////////////////////////////////////////////// @@ -259,12 +270,13 @@ void MouseDrag::Update(const UpdateInfo &_info, if (linkId) { link = Link(*linkId); - if (!link.Valid(_ecm)) + auto model = link.ParentModel(_ecm); + if (!link.Valid(_ecm) || model->Static(_ecm)) { + this->dataPtr->blockOrbit = false; return; } } - link.EnableVelocityChecks(_ecm, true); auto linkWorldPose = worldPose(*linkId, _ecm); auto inertial = _ecm.Component(*linkId); @@ -272,6 +284,8 @@ void MouseDrag::Update(const UpdateInfo &_info, { return; } + + this->dataPtr->dragActive = true; this->dataPtr->UpdateGains(linkWorldPose, inertial->Data()); if (this->dataPtr->modeDirty) @@ -342,8 +356,6 @@ void MouseDrag::Update(const UpdateInfo &_info, math::Vector3d force; math::Vector3d torque; - math::Quaterniond errorRot; - math::Vector3d errorPos; if (this->dataPtr->mode == MouseDragMode::ROTATE) { @@ -361,15 +373,18 @@ void MouseDrag::Update(const UpdateInfo &_info, axis.Normalize(); // Calculate the necessary rotation and torque - errorRot = - (this->dataPtr->initialRot * math::Quaterniond(axis, angle)).Inverse() * - linkWorldPose.Rot(); + this->dataPtr->goalPose.Rot() = + this->dataPtr->initialRot * math::Quaterniond(axis, angle); + this->dataPtr->goalPose.Pos() = linkWorldPose.Pos(); + math::Quaterniond errorRot = + this->dataPtr->goalPose.Rot().Inverse() * linkWorldPose.Rot(); torque = this->dataPtr->CalculatePID( this->dataPtr->rotPid, errorRot.Euler(), _info.dt); } else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) { - errorPos = this->dataPtr->applicationPoint - this->dataPtr->target; + math::Vector3d errorPos = + this->dataPtr->applicationPoint - this->dataPtr->target; force = this->dataPtr->CalculatePID( this->dataPtr->posPid, errorPos, _info.dt); torque = @@ -425,18 +440,37 @@ void MouseDragPrivate::OnRender() return; } this->rayQuery = this->scene->CreateRayQuery(); + this->arrowVisual = this->scene->CreateArrowVisual(); - this->arrowVisual->SetMaterial("Default/TransYellow"); + this->arrowVisual->SetMaterial("Default/TransRed"); + this->arrowVisual->ShowArrowHead(true); + this->arrowVisual->ShowArrowShaft(true); + this->arrowVisual->ShowArrowRotation(false); + this->arrowVisual->SetVisible(false); + + this->boxVisual = scene->CreateVisual(); + this->boxVisual->AddGeometry(scene->CreateBox()); + this->boxVisual->SetInheritScale(false); + this->boxVisual->SetMaterial("Default/TransRed"); + this->boxVisual->SetUserData("gui-only", static_cast(true)); + this->boxVisual->SetVisible(false); } // Update the visualization - if (this->mode == MouseDragMode::NONE) + if (!this->dragActive) { - this->arrowVisual->ShowArrowHead(false); - this->arrowVisual->ShowArrowShaft(false); - this->arrowVisual->ShowArrowRotation(false); + this->arrowVisual->SetVisible(false); + this->boxVisual->SetVisible(false); } - else + else if (this->mode == MouseDragMode::ROTATE) + { + this->boxVisual->SetLocalPose(this->goalPose); + this->boxVisual->SetLocalScale(1.2 * this->bboxSize); + + this->arrowVisual->SetVisible(false); + this->boxVisual->SetVisible(true); + } + else if (this->mode == MouseDragMode::TRANSLATE) { math::Vector3d axisDir = this->target - this->applicationPoint; math::Vector3d u = axisDir.Normalized(); @@ -448,32 +482,14 @@ void MouseDragPrivate::OnRender() quat.SetFromAxisAngle(u.Perpendicular(), angle); else quat.SetFromAxisAngle((v.Cross(u)).Normalize(), angle); + double scale = 2 * this->bboxSize.Length(); - if (this->mode == MouseDragMode::ROTATE) - { - quat = - math::Quaterniond(this->camera->WorldPose().Rot().XAxis(), GZ_PI/2) * - quat; - double scale = axisDir.Length() / 0.075f; - - this->arrowVisual->SetLocalPosition(this->applicationPoint); - this->arrowVisual->SetLocalRotation(quat); - this->arrowVisual->SetLocalScale(scale, scale, 0); - - this->arrowVisual->ShowArrowHead(false); - this->arrowVisual->ShowArrowShaft(false); - this->arrowVisual->ShowArrowRotation(true); - } - if (this->mode == MouseDragMode::TRANSLATE) - { - this->arrowVisual->SetLocalPosition(this->applicationPoint); - this->arrowVisual->SetLocalRotation(quat); - this->arrowVisual->SetLocalScale(1, 1, axisDir.Length()); + this->arrowVisual->SetLocalPosition(this->applicationPoint); + this->arrowVisual->SetLocalRotation(quat); + this->arrowVisual->SetLocalScale(scale, scale, axisDir.Length() / 0.75); - this->arrowVisual->ShowArrowHead(true); - this->arrowVisual->ShowArrowShaft(true); - this->arrowVisual->ShowArrowRotation(false); - } + this->arrowVisual->SetVisible(true); + this->boxVisual->SetVisible(false); } if (this->sendBlockOrbit) @@ -537,6 +553,8 @@ void MouseDragPrivate::HandleMouseEvents() this->mouseEvent.Pos(), this->camera, this->rayQuery); + this->bboxSize = visual->LocalBoundingBox().Size(); + if (this->mouseEvent.Button() == common::MouseEvent::LEFT) { this->mode = MouseDragMode::ROTATE; @@ -549,6 +567,7 @@ void MouseDragPrivate::HandleMouseEvents() else if (this->mouseEvent.Type() == common::MouseEvent::RELEASE) { this->mode = MouseDragMode::NONE; + this->dragActive = false; this->blockOrbit = false; this->sendBlockOrbit = true; } @@ -598,7 +617,7 @@ void MouseDragPrivate::UpdateGains(math::Pose3d _linkWorldPose, this->posPid[i].SetPGain(this->posStiffness * mass); this->posPid[i].SetDGain(2 * sqrt(this->posStiffness) * mass); this->rotPid[i].SetPGain(0); - this->rotPid[i].SetDGain(sqrt(this->rotStiffness) * inertia(i, i)); + this->rotPid[i].SetDGain(0.5 * sqrt(this->rotStiffness) * inertia(i, i)); } } } From 54a64c0fe81f04f790e6f97917b755b14e3cf7d1 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Wed, 19 Jul 2023 10:28:58 -0300 Subject: [PATCH 09/20] Fix rotation mode Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 81 ++++++++++++------------- 1 file changed, 38 insertions(+), 43 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index 9eb4369b1c..2ff141c29f 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -102,6 +102,9 @@ namespace sim /// \brief Holds the latest mouse event public: gz::common::MouseEvent mouseEvent; + /// \brief Initial position of a mouse click + public: math::Vector2i mousePressPos; + /// \brief True if there are new mouse events to process public: bool mouseDirty{false}; @@ -111,17 +114,23 @@ namespace sim /// \brief True if the force should be applied to the center of mass public: bool applyCOM{false}; - /// \brief Block orbit + /// \brief True if camera orbit should be blocked public: bool blockOrbit{false}; /// \brief True if BlockOrbit events should be sent public: bool sendBlockOrbit{false}; - /// \brief True if the dragging mode is active + /// \brief True if dragging is active public: bool dragActive{false}; - /// \brief Visual of the Link to which the wrenches are applied - public: Entity visualId; + /// \brief Current dragging mode + public: MouseDragMode mode = MouseDragMode::NONE; + + /// \brief Link to which the wrenches are applied + public: Entity linkId; + + /// \brief Plane of force application + public: math::Planed plane; /// \brief Application point of the wrench in world coordinates math::Vector3d applicationPoint; @@ -139,15 +148,6 @@ namespace sim /// link origin, expressed in the link-fixed frame public: math::Vector3d offset{0.0, 0.0, 0.0}; - /// \brief Plane of force application - public: math::Planed plane; - - /// \brief Initial position of a mouse click - public: math::Vector2i mousePressPos; - - /// \brief Current dragging mode - public: MouseDragMode mode = MouseDragMode::NONE; - /// \brief PID controllers for translation public: math::PID posPid[3]; @@ -157,7 +157,7 @@ namespace sim /// \brief Spring stiffness for translation, in (m/s²)/m public: double posStiffness = 100; - /// \brief Spring stiffness for rotation + /// \brief Spring stiffness for rotation, in (rad/s²)/rad public: double rotStiffness = 100; /// \brief Arrow for visualizing force in translation mode. @@ -260,31 +260,29 @@ void MouseDrag::Update(const UpdateInfo &_info, _info.paused) { this->dataPtr->mode = MouseDragMode::NONE; + this->dataPtr->dragActive = false; return; } // Get Link corresponding to clicked Visual - Link link; - auto linkId = - _ecm.ComponentData(this->dataPtr->visualId); - if (linkId) + Link link(this->dataPtr->linkId); + auto model = link.ParentModel(_ecm); + if (!link.Valid(_ecm) || model->Static(_ecm)) { - link = Link(*linkId); - auto model = link.ParentModel(_ecm); - if (!link.Valid(_ecm) || model->Static(_ecm)) - { - this->dataPtr->blockOrbit = false; - return; - } + this->dataPtr->blockOrbit = false; + return; } - auto linkWorldPose = worldPose(*linkId, _ecm); - auto inertial = _ecm.Component(*linkId); + auto linkWorldPose = worldPose(this->dataPtr->linkId, _ecm); + auto inertial = _ecm.Component(this->dataPtr->linkId); if (!inertial) { return; } + if (this->dataPtr->blockOrbit) + this->dataPtr->sendBlockOrbit = true; + this->dataPtr->dragActive = true; this->dataPtr->UpdateGains(linkWorldPose, inertial->Data()); @@ -292,6 +290,11 @@ void MouseDrag::Update(const UpdateInfo &_info, { this->dataPtr->modeDirty = false; + gui::events::DeselectAllEntities event(true); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &event); + if (this->dataPtr->mode == MouseDragMode::ROTATE) { this->dataPtr->mousePressPos = this->dataPtr->mouseEvent.Pos(); @@ -320,7 +323,6 @@ void MouseDrag::Update(const UpdateInfo &_info, double planeOffset = direction.Dot(this->dataPtr->applicationPoint); this->dataPtr->plane = math::Planed(direction, planeOffset); - gzdbg << "Resetting PIDs" << std::endl; for (auto i : {0, 1, 2}) { this->dataPtr->posPid[i].Reset(); @@ -374,10 +376,10 @@ void MouseDrag::Update(const UpdateInfo &_info, // Calculate the necessary rotation and torque this->dataPtr->goalPose.Rot() = - this->dataPtr->initialRot * math::Quaterniond(axis, angle); + math::Quaterniond(axis, angle) * this->dataPtr->initialRot; this->dataPtr->goalPose.Pos() = linkWorldPose.Pos(); math::Quaterniond errorRot = - this->dataPtr->goalPose.Rot().Inverse() * linkWorldPose.Rot(); + linkWorldPose.Rot() * this->dataPtr->goalPose.Rot().Inverse(); torque = this->dataPtr->CalculatePID( this->dataPtr->rotPid, errorRot.Euler(), _info.dt); } @@ -395,7 +397,7 @@ void MouseDrag::Update(const UpdateInfo &_info, // Publish wrench msgs::EntityWrench msg; - msg.mutable_entity()->set_id(*linkId); + msg.mutable_entity()->set_id(this->dataPtr->linkId); msgs::Set(msg.mutable_wrench()->mutable_force(), force); msgs::Set(msg.mutable_wrench()->mutable_torque(), torque); @@ -519,15 +521,6 @@ void MouseDragPrivate::HandleMouseEvents() this->mouseEvent.Control() && this->mouseEvent.Button() != common::MouseEvent::MIDDLE) { - this->blockOrbit = true; - this->sendBlockOrbit = true; - this->modeDirty = true; - - gui::events::DeselectAllEntities event(true); - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), - &event); - // Get the visual at mouse position rendering::VisualPtr visual = this->scene->VisualAt( this->camera, @@ -540,7 +533,8 @@ void MouseDragPrivate::HandleMouseEvents() } try { - this->visualId = std::get(visual->UserData("gazebo-entity")); + this->linkId = + std::get(visual->Parent()->UserData("gazebo-entity")); } catch(std::bad_variant_access &e) { @@ -548,6 +542,9 @@ void MouseDragPrivate::HandleMouseEvents() return; } + this->blockOrbit = true; + this->modeDirty = true; + // Get the 3D coordinates of the clicked point this->applicationPoint = rendering::screenToScene( this->mouseEvent.Pos(), this->camera, @@ -569,14 +566,12 @@ void MouseDragPrivate::HandleMouseEvents() this->mode = MouseDragMode::NONE; this->dragActive = false; this->blockOrbit = false; - this->sendBlockOrbit = true; } else if (this->mouseEvent.Type() == common::MouseEvent::MOVE) { if (this->mode != MouseDragMode::NONE) { this->blockOrbit = true; - this->sendBlockOrbit = true; } } } From 316f054424fd6cd5f4a0ffcf4ae3f24314334a3e Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Thu, 20 Jul 2023 12:24:17 -0300 Subject: [PATCH 10/20] Make arrows always visible and minor changes Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 33 ++++++++++++++++++------- src/gui/plugins/mouse_drag/MouseDrag.hh | 3 ++- 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index 2ff141c29f..646f20869b 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -74,13 +74,19 @@ namespace sim /// \brief Perform rendering calls in the rendering thread. public: void OnRender(); - /// \brief Update the PID loop on 3 coordinates and return the command value + /// \brief Update the PID loop on 3 coordinates + /// \param[in] _pid PID controllers on 3 coordinates to be updated + /// \param[in] _error Error vector since last call (p_state - p_target) + /// \param[in] _dt Change in time since last call + /// \return the command value public: math::Vector3d CalculatePID( math::PID _pid[3], const math::Vector3d &_error, const std::chrono::duration &_dt); /// \brief Update the PID gains with critical damping + /// \param[in] _linkWorldPose world pose of the link + /// \param[in] _inertial inertial of the link public: void UpdateGains(math::Pose3d _linkWorldPose, math::Inertiald _inertial); @@ -155,17 +161,17 @@ namespace sim public: math::PID rotPid[3]; /// \brief Spring stiffness for translation, in (m/s²)/m - public: double posStiffness = 100; + public: double posStiffness = 100.0; /// \brief Spring stiffness for rotation, in (rad/s²)/rad - public: double rotStiffness = 100; + public: double rotStiffness = 100.0; /// \brief Arrow for visualizing force in translation mode. /// This arrow goes from the application point to the target point. - public: rendering::ArrowVisualPtr arrowVisual; + public: rendering::ArrowVisualPtr arrowVisual{nullptr}; /// \brief Box for visualizing rotation mode - public: rendering::VisualPtr boxVisual; + public: rendering::VisualPtr boxVisual{nullptr}; /// \brief Size of the bounding box of the selected link public: math::Vector3d bboxSize; @@ -390,9 +396,14 @@ void MouseDrag::Update(const UpdateInfo &_info, force = this->dataPtr->CalculatePID( this->dataPtr->posPid, errorPos, _info.dt); torque = - linkWorldPose.Rot().RotateVector(this->dataPtr->offset).Cross(force) + - this->dataPtr->CalculatePID( + linkWorldPose.Rot().RotateVector(this->dataPtr->offset).Cross(force); + // Rotation damping on translation mode isn't done if the force + // is applied to the center of mass + if (!this->dataPtr->applyCOM) + { + torque += this->dataPtr->CalculatePID( this->dataPtr->rotPid, linkWorldPose.Rot().Euler(), _info.dt); + } } // Publish wrench @@ -443,8 +454,12 @@ void MouseDragPrivate::OnRender() } this->rayQuery = this->scene->CreateRayQuery(); + auto mat = this->scene->Material("Default/TransRed")->Clone(); + mat->SetDepthCheckEnabled(false); + mat->SetDepthWriteEnabled(false); + this->arrowVisual = this->scene->CreateArrowVisual(); - this->arrowVisual->SetMaterial("Default/TransRed"); + this->arrowVisual->SetMaterial(mat); this->arrowVisual->ShowArrowHead(true); this->arrowVisual->ShowArrowShaft(true); this->arrowVisual->ShowArrowRotation(false); @@ -453,7 +468,7 @@ void MouseDragPrivate::OnRender() this->boxVisual = scene->CreateVisual(); this->boxVisual->AddGeometry(scene->CreateBox()); this->boxVisual->SetInheritScale(false); - this->boxVisual->SetMaterial("Default/TransRed"); + this->boxVisual->SetMaterial(mat); this->boxVisual->SetUserData("gui-only", static_cast(true)); this->boxVisual->SetVisible(false); } diff --git a/src/gui/plugins/mouse_drag/MouseDrag.hh b/src/gui/plugins/mouse_drag/MouseDrag.hh index 3c72187db3..8e6cb11817 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.hh +++ b/src/gui/plugins/mouse_drag/MouseDrag.hh @@ -53,7 +53,8 @@ namespace sim public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm) override; - /// \brief Callback when echo button is pressed + /// \brief Callback when COM switch is pressed + /// \param[in] _checked True if force should be applied to center of mass public slots: void OnSwitchCOM(const bool _checked); /// \internal From e7f2ef2d2b70deefb1bb2ff659826f307f146d38 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Fri, 21 Jul 2023 15:41:06 -0300 Subject: [PATCH 11/20] Automatically load ApplyLinkWrench system and improve calculations Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 156 ++++++++++++++++++------ src/gui/plugins/mouse_drag/MouseDrag.hh | 2 +- 2 files changed, 118 insertions(+), 40 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index 646f20869b..f1bd00d82e 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -25,10 +25,12 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -44,7 +46,10 @@ #include #include #include +#include #include +#include +#include #include #include @@ -96,6 +101,9 @@ namespace sim /// \brief Publisher for EntityWrench messages public: transport::Node::Publisher pub; + /// \brief World name + public: std::string worldName; + /// \brief Pointer to the rendering scene public: rendering::ScenePtr scene{nullptr}; @@ -129,6 +137,9 @@ namespace sim /// \brief True if dragging is active public: bool dragActive{false}; + /// \brief True if the ApplyLinkWrench system is loaded + public: bool systemLoaded{false}; + /// \brief Current dragging mode public: MouseDragMode mode = MouseDragMode::NONE; @@ -147,8 +158,8 @@ namespace sim /// \brief Point to which the link is dragged to, in translation mode math::Vector3d target; - /// \brief Goal pose for rotation mode - public: math::Pose3d goalPose; + /// \brief Goal link rotation for rotation mode + public: math::Quaterniond goalRot; /// \brief Offset of the force application point relative to the /// link origin, expressed in the link-fixed frame @@ -201,8 +212,9 @@ void MouseDrag::LoadConfig(const tinyxml2::XMLElement */*_pluginElem*/) auto worldNames = gz::gui::worldNames(); if (!worldNames.empty()) { + this->dataPtr->worldName = worldNames[0].toStdString(); auto topic = transport::TopicUtils::AsValidTopic( - "/world/" + worldNames[0].toStdString() + "/wrench"); + "/world/" + this->dataPtr->worldName + "/wrench"); if (topic == "") { gzerr << "Unable to create publisher" << std::endl; @@ -262,6 +274,77 @@ bool MouseDrag::eventFilter(QObject *_obj, QEvent *_event) void MouseDrag::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { + // Load the ApplyLinkWrench system + if (!this->dataPtr->systemLoaded) + { + const std::string name{"gz::sim::systems::ApplyLinkWrench"}; + const std::string filename{"gz-sim-apply-link-wrench-system"}; + const std::string innerxml{"0"}; + + // Get world entity + Entity worldEntity; + _ecm.Each( + [&](const Entity &_entity, + const components::World */*_world*/, + const components::Name *_name)->bool + { + if (_name->Data() == this->dataPtr->worldName) + { + worldEntity = _entity; + return false; + } + return true; + }); + + // Check if already loaded + auto msg = _ecm.ComponentData(worldEntity); + if (!msg) + { + gzdbg << "Unable to find SystemPluginInfo component for entity " + << worldEntity << std::endl; + return; + } + for (const auto &plugin : msg->plugins()) + { + if (plugin.filename() == filename) + { + this->dataPtr->systemLoaded = true; + gzdbg << "ApplyLinkWrench system already loaded" << std::endl; + break; + } + } + + // Request to load system + if (!this->dataPtr->systemLoaded) + { + msgs::EntityPlugin_V req; + req.mutable_entity()->set_id(worldEntity); + auto plugin = req.add_plugins(); + plugin->set_name(name); + plugin->set_filename(filename); + plugin->set_innerxml(innerxml); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/" + this->dataPtr->worldName + + "/entity/system/add"}; + if (this->dataPtr->node.Request(service, req, timeout, res, result)) + { + this->dataPtr->systemLoaded = true; + gzdbg << "ApplyLinkWrench system has been loaded" << std::endl; + } + else + { + gzerr << "Error adding new system to entity: " + << worldEntity << "\n" + << "Name: " << name << "\n" + << "Filename: " << filename << "\n" + << "Inner XML: " << innerxml << std::endl; + } + } + } + if (this->dataPtr->mode == MouseDragMode::NONE || _info.paused) { @@ -301,26 +384,21 @@ void MouseDrag::Update(const UpdateInfo &_info, gz::gui::App()->findChild(), &event); - if (this->dataPtr->mode == MouseDragMode::ROTATE) + this->dataPtr->mousePressPos = this->dataPtr->mouseEvent.Pos(); + this->dataPtr->initialRot = linkWorldPose.Rot(); + + // Calculate offset of force application from link origin + if (this->dataPtr->applyCOM || this->dataPtr->mode == MouseDragMode::ROTATE) { - this->dataPtr->mousePressPos = this->dataPtr->mouseEvent.Pos(); - this->dataPtr->initialRot = linkWorldPose.Rot(); + this->dataPtr->offset = inertial->Data().Pose().Pos(); + this->dataPtr->applicationPoint = + linkWorldPose.Pos() + + linkWorldPose.Rot().RotateVector(this->dataPtr->offset); } - else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) + else { - // Calculate offset of force application from link origin - if (this->dataPtr->applyCOM) - { - this->dataPtr->offset = inertial->Data().Pose().Pos(); - this->dataPtr->applicationPoint = - linkWorldPose.Pos() + - linkWorldPose.Rot().RotateVector(this->dataPtr->offset); - } - else - { - this->dataPtr->offset = linkWorldPose.Rot().RotateVectorReverse( - this->dataPtr->applicationPoint - linkWorldPose.Pos()); - } + this->dataPtr->offset = linkWorldPose.Rot().RotateVectorReverse( + this->dataPtr->applicationPoint - linkWorldPose.Pos()); } // The plane of wrench application should be normal to the center @@ -335,8 +413,8 @@ void MouseDrag::Update(const UpdateInfo &_info, this->dataPtr->rotPid[i].Reset(); } } - // Track the application point in translation mode - else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) + // Track the application point + else { this->dataPtr->applicationPoint = linkWorldPose.Pos() + @@ -353,25 +431,19 @@ void MouseDrag::Update(const UpdateInfo &_info, // Make a ray query at the mouse position this->dataPtr->rayQuery->SetFromCamera( this->dataPtr->camera, math::Vector2d(nx, ny)); + math::Vector3d end = this->dataPtr->rayQuery->Direction(); - math::Vector3d origin = this->dataPtr->rayQuery->Origin(); - math::Vector3d direction = this->dataPtr->rayQuery->Direction(); - - if (auto t = this->dataPtr->plane.Intersection(origin, direction)) - this->dataPtr->target = *t; - else - return; - + // Wrench in world coordinates, applied at the link origin math::Vector3d force; math::Vector3d torque; - if (this->dataPtr->mode == MouseDragMode::ROTATE) { // Calculate rotation angle from mouse displacement - math::Vector3d camPos = this->dataPtr->camera->WorldPose().Pos(); - math::Vector3d start = - (this->dataPtr->applicationPoint - camPos).Normalized(); - math::Vector3d end = (this->dataPtr->target - camPos).Normalized(); + nx = 2.0 * this->dataPtr->mousePressPos.X() / width - 1.0; + ny = 1.0 - 2.0 * this->dataPtr->mousePressPos.Y() / height; + this->dataPtr->rayQuery->SetFromCamera( + this->dataPtr->camera, math::Vector2d(nx, ny)); + math::Vector3d start = this->dataPtr->rayQuery->Direction(); math::Vector3d axis = start.Cross(end); double angle = -atan2(axis.Length(), start.Dot(end)); @@ -381,16 +453,21 @@ void MouseDrag::Update(const UpdateInfo &_info, axis.Normalize(); // Calculate the necessary rotation and torque - this->dataPtr->goalPose.Rot() = + this->dataPtr->goalRot = math::Quaterniond(axis, angle) * this->dataPtr->initialRot; - this->dataPtr->goalPose.Pos() = linkWorldPose.Pos(); math::Quaterniond errorRot = - linkWorldPose.Rot() * this->dataPtr->goalPose.Rot().Inverse(); + linkWorldPose.Rot() * this->dataPtr->goalRot.Inverse(); torque = this->dataPtr->CalculatePID( this->dataPtr->rotPid, errorRot.Euler(), _info.dt); } else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) { + math::Vector3d origin = this->dataPtr->rayQuery->Origin(); + if (auto t = this->dataPtr->plane.Intersection(origin, end)) + this->dataPtr->target = *t; + else + return; + math::Vector3d errorPos = this->dataPtr->applicationPoint - this->dataPtr->target; force = this->dataPtr->CalculatePID( @@ -481,7 +558,8 @@ void MouseDragPrivate::OnRender() } else if (this->mode == MouseDragMode::ROTATE) { - this->boxVisual->SetLocalPose(this->goalPose); + this->boxVisual->SetLocalPosition(this->applicationPoint); + this->boxVisual->SetLocalRotation(this->goalRot); this->boxVisual->SetLocalScale(1.2 * this->bboxSize); this->arrowVisual->SetVisible(false); diff --git a/src/gui/plugins/mouse_drag/MouseDrag.hh b/src/gui/plugins/mouse_drag/MouseDrag.hh index 8e6cb11817..b8b095a5d4 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.hh +++ b/src/gui/plugins/mouse_drag/MouseDrag.hh @@ -29,7 +29,7 @@ namespace sim class MouseDragPrivate; /// \brief Translate and rotate links by dragging them with the mouse. - /// Requires the ApplyLinkWrench system to be loaded. + /// Automatically loads the ApplyLinkWrench system. /// /// ## Configuration /// This plugin doesn't accept any custom configuration. From 91b1f1aa0f5f2be26851e8d5100bbfd8ea6638f4 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Thu, 27 Jul 2023 12:14:11 -0300 Subject: [PATCH 12/20] Configurable stiffnesses in MouseDrag Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 42 +++++++++++++++++- src/gui/plugins/mouse_drag/MouseDrag.hh | 42 +++++++++++++++++- src/gui/plugins/mouse_drag/MouseDrag.qml | 55 ++++++++++++++++++++++-- 3 files changed, 134 insertions(+), 5 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index f1bd00d82e..bbdba692dd 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -203,7 +203,7 @@ MouseDrag::MouseDrag() MouseDrag::~MouseDrag() = default; ///////////////////////////////////////////////// -void MouseDrag::LoadConfig(const tinyxml2::XMLElement */*_pluginElem*/) +void MouseDrag::LoadConfig(const tinyxml2::XMLElement *_pluginElem) { if (this->title.empty()) this->title = "Mouse drag"; @@ -225,6 +225,22 @@ void MouseDrag::LoadConfig(const tinyxml2::XMLElement */*_pluginElem*/) gzdbg << "Created publisher to " << topic << std::endl; } + // Read configuration + if (_pluginElem) + { + if (auto elem = _pluginElem->FirstChildElement("rotation_stiffness")) + { + elem->QueryDoubleText(&this->dataPtr->rotStiffness); + emit this->RotStiffnessChanged(); + } + + if (auto elem = _pluginElem->FirstChildElement("position_stiffness")) + { + elem->QueryDoubleText(&this->dataPtr->posStiffness); + emit this->PosStiffnessChanged(); + } + } + gz::gui::App()->findChild ()->installEventFilter(this); } @@ -498,6 +514,30 @@ void MouseDrag::OnSwitchCOM(const bool _checked) this->dataPtr->applyCOM = _checked; } +///////////////////////////////////////////////// +double MouseDrag::RotStiffness() const +{ + return this->dataPtr->rotStiffness; +} + +///////////////////////////////////////////////// +void MouseDrag::SetRotStiffness(double _rotStiffness) +{ + this->dataPtr->rotStiffness = _rotStiffness; +} + +///////////////////////////////////////////////// +double MouseDrag::PosStiffness() const +{ + return this->dataPtr->posStiffness; +} + +///////////////////////////////////////////////// +void MouseDrag::SetPosStiffness(double _posStiffness) +{ + this->dataPtr->posStiffness = _posStiffness; +} + ///////////////////////////////////////////////// void MouseDragPrivate::OnRender() { diff --git a/src/gui/plugins/mouse_drag/MouseDrag.hh b/src/gui/plugins/mouse_drag/MouseDrag.hh index b8b095a5d4..6f61a31974 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.hh +++ b/src/gui/plugins/mouse_drag/MouseDrag.hh @@ -32,11 +32,29 @@ namespace sim /// Automatically loads the ApplyLinkWrench system. /// /// ## Configuration - /// This plugin doesn't accept any custom configuration. + /// + /// * \ : Stiffness of rotation mode, defaults to 100 + /// * \ : Stiffness of translation mode, defaults to 100 class MouseDrag : public gz::sim::GuiSystem { Q_OBJECT + /// \brief Stiffness of rotation mode + Q_PROPERTY( + double rotStiffness + READ RotStiffness + WRITE SetRotStiffness + NOTIFY RotStiffnessChanged + ) + + /// \brief Stiffness of translation mode + Q_PROPERTY( + double posStiffness + READ PosStiffness + WRITE SetPosStiffness + NOTIFY PosStiffnessChanged + ) + /// \brief Constructor public: MouseDrag(); @@ -57,6 +75,28 @@ namespace sim /// \param[in] _checked True if force should be applied to center of mass public slots: void OnSwitchCOM(const bool _checked); + /// \brief Get the rotational stiffness + /// \return The rotational stiffness + public: Q_INVOKABLE double RotStiffness() const; + + /// \brief Notify that the rotational stiffness changed + signals: void RotStiffnessChanged(); + + /// \brief Set the rotational stiffness + /// \param[in] _rotStiffness The new rotational stiffness + public: Q_INVOKABLE void SetRotStiffness(double _rotStiffness); + + /// \brief Get the translational stiffness + /// \return The translational stiffness + public: Q_INVOKABLE double PosStiffness() const; + + /// \brief Notify that the translational stiffness changed + signals: void PosStiffnessChanged(); + + /// \brief Set the translational stiffness + /// \param[in] _posStiffness The new translational stiffness + public: Q_INVOKABLE void SetPosStiffness(double _posStiffness); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/mouse_drag/MouseDrag.qml b/src/gui/plugins/mouse_drag/MouseDrag.qml index 20a1b2df23..1f457debee 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.qml +++ b/src/gui/plugins/mouse_drag/MouseDrag.qml @@ -23,23 +23,72 @@ GridLayout { columns: 8 columnSpacing: 10 Layout.minimumWidth: 350 - Layout.minimumHeight: 200 + Layout.minimumHeight: 300 anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 + // Maximum value for GzSpinBoxes + property int maxValue: 1000000 + + // Precision of the GzSpinBoxes in decimal places + property int decimalPlaces: 1 + + // Step size of the GzSpinBoxes + property double step: 10.0 + Text { Layout.columnSpan: 8 id: rotationText color: "dimgrey" - text: qsTr("Ctrl+Left-Click: rotation") + text: qsTr("Rotation (Ctrl+Left-Click)") + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: rotStiffText + color: "dimgrey" + text: qsTr("Rotation stiffness") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: rotStiffness + maximumValue: maxValue + minimumValue: 0 + value: MouseDrag.rotStiffness + decimals: decimalPlaces + stepSize: step + onValueChanged: MouseDrag.rotStiffness = rotStiffness.value } Text { Layout.columnSpan: 8 id: translationText color: "dimgrey" - text: qsTr("Ctrl+Right-Click: translation") + text: qsTr("Translation (Ctrl+Right-Click)") + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: posStiffText + color: "dimgrey" + text: qsTr("Position stiffness") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: posStiffness + maximumValue: maxValue + minimumValue: 0 + value: MouseDrag.posStiffness + decimals: decimalPlaces + stepSize: step + onValueChanged: MouseDrag.posStiffness = posStiffness.value } Switch { From 60f788f5022962329bb9b691d1096f99fdfaf6c8 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Mon, 31 Jul 2023 15:30:09 -0300 Subject: [PATCH 13/20] Minor fixes Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index bbdba692dd..bffa36e72e 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -144,19 +144,19 @@ namespace sim public: MouseDragMode mode = MouseDragMode::NONE; /// \brief Link to which the wrenches are applied - public: Entity linkId; + public: Entity linkId{kNullEntity}; /// \brief Plane of force application public: math::Planed plane; /// \brief Application point of the wrench in world coordinates - math::Vector3d applicationPoint; + public: math::Vector3d applicationPoint; /// \brief Initial world rotation of the link during mouse click public: math::Quaterniond initialRot; /// \brief Point to which the link is dragged to, in translation mode - math::Vector3d target; + public: math::Vector3d target; /// \brief Goal link rotation for rotation mode public: math::Quaterniond goalRot; @@ -215,7 +215,7 @@ void MouseDrag::LoadConfig(const tinyxml2::XMLElement *_pluginElem) this->dataPtr->worldName = worldNames[0].toStdString(); auto topic = transport::TopicUtils::AsValidTopic( "/world/" + this->dataPtr->worldName + "/wrench"); - if (topic == "") + if (topic.empty()) { gzerr << "Unable to create publisher" << std::endl; return; @@ -343,9 +343,10 @@ void MouseDrag::Update(const UpdateInfo &_info, msgs::Boolean res; bool result; unsigned int timeout = 5000; - std::string service{"/world/" + this->dataPtr->worldName + - "/entity/system/add"}; - if (this->dataPtr->node.Request(service, req, timeout, res, result)) + auto service = transport::TopicUtils::AsValidTopic( + "/world/" + this->dataPtr->worldName + "/entity/system/add"); + if (!service.empty() && + this->dataPtr->node.Request(service, req, timeout, res, result)) { this->dataPtr->systemLoaded = true; gzdbg << "ApplyLinkWrench system has been loaded" << std::endl; From 965a9e0fd273c3e371b0aa09f912da9ce285002c Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Tue, 1 Aug 2023 12:06:46 -0300 Subject: [PATCH 14/20] Change wrench calculation Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 161 +++++++++++++----------- 1 file changed, 90 insertions(+), 71 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index f1bd00d82e..4e49e6551f 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -79,21 +79,25 @@ namespace sim /// \brief Perform rendering calls in the rendering thread. public: void OnRender(); - /// \brief Update the PID loop on 3 coordinates - /// \param[in] _pid PID controllers on 3 coordinates to be updated - /// \param[in] _error Error vector since last call (p_state - p_target) + /// \brief Calculate the wrench to be applied to the link, + /// based on a spring-damper control law. + /// + /// This attempts to minimize the error (in position or + /// rotation, depending on the mode) with a proportional control, while + /// damping the rate of change of the link's pose. + /// + /// \param[in] _error Position error in translation mode, Rotation error + /// (in Euler angles) in rotation mode /// \param[in] _dt Change in time since last call - /// \return the command value - public: math::Vector3d CalculatePID( - math::PID _pid[3], - const math::Vector3d &_error, - const std::chrono::duration &_dt); - - /// \brief Update the PID gains with critical damping - /// \param[in] _linkWorldPose world pose of the link + /// \param[in] _linkWorldPose World pose of the link /// \param[in] _inertial inertial of the link - public: void UpdateGains(math::Pose3d _linkWorldPose, - math::Inertiald _inertial); + /// \return The EntityWrench message containing the calculated + /// force and torque + public: msgs::EntityWrench CalculateWrench( + const math::Vector3d &_error, + const std::chrono::duration &_dt, + const math::Pose3d &_linkWorldPose, + const math::Inertiald &_inertial); /// \brief Transport node public: transport::Node node; @@ -155,7 +159,7 @@ namespace sim /// \brief Initial world rotation of the link during mouse click public: math::Quaterniond initialRot; - /// \brief Point to which the link is dragged to, in translation mode + /// \brief Point to which the link is dragged in translation mode math::Vector3d target; /// \brief Goal link rotation for rotation mode @@ -165,17 +169,14 @@ namespace sim /// link origin, expressed in the link-fixed frame public: math::Vector3d offset{0.0, 0.0, 0.0}; - /// \brief PID controllers for translation - public: math::PID posPid[3]; - - /// \brief PID controllers for rotation - public: math::PID rotPid[3]; + /// \brief Link world pose in previous Update call. Used for damping + public: math::Pose3d poseLast; /// \brief Spring stiffness for translation, in (m/s²)/m public: double posStiffness = 100.0; /// \brief Spring stiffness for rotation, in (rad/s²)/rad - public: double rotStiffness = 100.0; + public: double rotStiffness = 200.0; /// \brief Arrow for visualizing force in translation mode. /// This arrow goes from the application point to the target point. @@ -373,7 +374,6 @@ void MouseDrag::Update(const UpdateInfo &_info, this->dataPtr->sendBlockOrbit = true; this->dataPtr->dragActive = true; - this->dataPtr->UpdateGains(linkWorldPose, inertial->Data()); if (this->dataPtr->modeDirty) { @@ -386,6 +386,7 @@ void MouseDrag::Update(const UpdateInfo &_info, this->dataPtr->mousePressPos = this->dataPtr->mouseEvent.Pos(); this->dataPtr->initialRot = linkWorldPose.Rot(); + this->dataPtr->poseLast = linkWorldPose; // Calculate offset of force application from link origin if (this->dataPtr->applyCOM || this->dataPtr->mode == MouseDragMode::ROTATE) @@ -406,12 +407,6 @@ void MouseDrag::Update(const UpdateInfo &_info, math::Vector3d direction = this->dataPtr->camera->WorldPose().Rot().XAxis(); double planeOffset = direction.Dot(this->dataPtr->applicationPoint); this->dataPtr->plane = math::Planed(direction, planeOffset); - - for (auto i : {0, 1, 2}) - { - this->dataPtr->posPid[i].Reset(); - this->dataPtr->rotPid[i].Reset(); - } } // Track the application point else @@ -434,8 +429,7 @@ void MouseDrag::Update(const UpdateInfo &_info, math::Vector3d end = this->dataPtr->rayQuery->Direction(); // Wrench in world coordinates, applied at the link origin - math::Vector3d force; - math::Vector3d torque; + msgs::EntityWrench msg; if (this->dataPtr->mode == MouseDragMode::ROTATE) { // Calculate rotation angle from mouse displacement @@ -457,8 +451,8 @@ void MouseDrag::Update(const UpdateInfo &_info, math::Quaterniond(axis, angle) * this->dataPtr->initialRot; math::Quaterniond errorRot = linkWorldPose.Rot() * this->dataPtr->goalRot.Inverse(); - torque = this->dataPtr->CalculatePID( - this->dataPtr->rotPid, errorRot.Euler(), _info.dt); + msg = this->dataPtr->CalculateWrench( + errorRot.Euler(), _info.dt, linkWorldPose, inertial->Data()); } else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) { @@ -470,25 +464,11 @@ void MouseDrag::Update(const UpdateInfo &_info, math::Vector3d errorPos = this->dataPtr->applicationPoint - this->dataPtr->target; - force = this->dataPtr->CalculatePID( - this->dataPtr->posPid, errorPos, _info.dt); - torque = - linkWorldPose.Rot().RotateVector(this->dataPtr->offset).Cross(force); - // Rotation damping on translation mode isn't done if the force - // is applied to the center of mass - if (!this->dataPtr->applyCOM) - { - torque += this->dataPtr->CalculatePID( - this->dataPtr->rotPid, linkWorldPose.Rot().Euler(), _info.dt); - } + msg = this->dataPtr->CalculateWrench( + errorPos, _info.dt, linkWorldPose, inertial->Data()); } // Publish wrench - msgs::EntityWrench msg; - msg.mutable_entity()->set_id(this->dataPtr->linkId); - msgs::Set(msg.mutable_wrench()->mutable_force(), force); - msgs::Set(msg.mutable_wrench()->mutable_torque(), torque); - this->dataPtr->pub.Publish(msg); } @@ -670,44 +650,83 @@ void MouseDragPrivate::HandleMouseEvents() } ///////////////////////////////////////////////// -math::Vector3d MouseDragPrivate::CalculatePID( - math::PID _pid[3], +msgs::EntityWrench MouseDragPrivate::CalculateWrench( const math::Vector3d &_error, - const std::chrono::duration &_dt) -{ - math::Vector3d result; - for (auto i : {0, 1, 2}) - { - result[i] = _pid[i].Update(_error[i], _dt); - } - return result; -} - -///////////////////////////////////////////////// -void MouseDragPrivate::UpdateGains(math::Pose3d _linkWorldPose, - math::Inertiald _inertial) + const std::chrono::duration &_dt, + const math::Pose3d &_linkWorldPose, + const math::Inertiald &_inertial) { - math::Matrix3d R(_linkWorldPose.Rot() * _inertial.Pose().Rot()); - math::Matrix3d inertia = R * _inertial.Moi() * R.Transposed(); + math::Vector3d force, torque; if (this->mode == MouseDragMode::ROTATE) { + // TODO(anyone): is this the best way to scale rotation gains? + double avgInertia = _inertial.MassMatrix().PrincipalMoments().Sum() / 3; + + math::Vector3d dErrorRot = + (_linkWorldPose.Rot() * this->poseLast.Rot().Inverse()).Euler() + / _dt.count(); for (auto i : {0, 1, 2}) { - this->rotPid[i].SetPGain(this->rotStiffness * inertia(i, i)); - this->rotPid[i].SetDGain(2 * sqrt(this->rotStiffness) * inertia(i, i)); + double pGainRot = this->rotStiffness * avgInertia; + double dGainRot = 2 * sqrt(this->rotStiffness) * avgInertia; + + // Correct angle discontinuity around 0/2pi + if (dErrorRot[i] > GZ_PI) + dErrorRot[i] -= 2 * GZ_PI; + else if (dErrorRot[i] < -GZ_PI) + dErrorRot[i] += 2 * GZ_PI; + + torque[i] = - pGainRot * _error[i] - dGainRot * dErrorRot[i]; } } else if (this->mode == MouseDragMode::TRANSLATE) { double mass = _inertial.MassMatrix().Mass(); - for (auto i : {0, 1, 2}) + double pGainPos = this->posStiffness * mass; + double dGainPos = 0.5 * sqrt(this->posStiffness) * mass; + + math::Vector3d dErrorPos = + (_linkWorldPose.Pos() - this->poseLast.Pos()) / _dt.count(); + + force = - pGainPos * _error - dGainPos * dErrorPos; + torque = + _linkWorldPose.Rot().RotateVector(this->offset).Cross(force); + + // If the force is not applied to the center of mass, slightly damp the + // resulting rotation + if (!this->applyCOM) { - this->posPid[i].SetPGain(this->posStiffness * mass); - this->posPid[i].SetDGain(2 * sqrt(this->posStiffness) * mass); - this->rotPid[i].SetPGain(0); - this->rotPid[i].SetDGain(0.5 * sqrt(this->rotStiffness) * inertia(i, i)); + double avgInertia = _inertial.MassMatrix().PrincipalMoments().Sum() / 3; + + math::Vector3d dErrorRot = + (_linkWorldPose.Rot() * this->poseLast.Rot().Inverse()).Euler() + / _dt.count(); + for (auto i : {0, 1, 2}) + { + double dGainRot = 0.5 * sqrt(this->rotStiffness) * avgInertia; + + // Correct angle discontinuity around 0/2pi + if (dErrorRot[i] > GZ_PI) + dErrorRot[i] -= 2 * GZ_PI; + else if (dErrorRot[i] < -GZ_PI) + dErrorRot[i] += 2 * GZ_PI; + + torque -= dGainRot * dErrorRot[i]; + } } } + else + { + return msgs::EntityWrench(); + } + + this->poseLast = _linkWorldPose; + + msgs::EntityWrench msg; + msg.mutable_entity()->set_id(this->linkId); + msgs::Set(msg.mutable_wrench()->mutable_force(), force); + msgs::Set(msg.mutable_wrench()->mutable_torque(), torque); + return msg; } // Register this plugin From 7c819644b28237fd9c9fb864f6249e4de6060219 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Tue, 1 Aug 2023 17:46:36 -0300 Subject: [PATCH 15/20] Visualizing plane and optimize calculations Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 124 ++++++++++++++++-------- 1 file changed, 85 insertions(+), 39 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index 4e49e6551f..a7bb70e8f7 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -79,6 +79,10 @@ namespace sim /// \brief Perform rendering calls in the rendering thread. public: void OnRender(); + /// \brief Update the gains for the controller + /// \param[in] _inertial Inertial of the link + public: void UpdateGains(const math::Inertiald &_inertial); + /// \brief Calculate the wrench to be applied to the link, /// based on a spring-damper control law. /// @@ -96,8 +100,7 @@ namespace sim public: msgs::EntityWrench CalculateWrench( const math::Vector3d &_error, const std::chrono::duration &_dt, - const math::Pose3d &_linkWorldPose, - const math::Inertiald &_inertial); + const math::Pose3d &_linkWorldPose); /// \brief Transport node public: transport::Node node; @@ -173,10 +176,22 @@ namespace sim public: math::Pose3d poseLast; /// \brief Spring stiffness for translation, in (m/s²)/m - public: double posStiffness = 100.0; + public: double posStiffness{100.0}; + + /// \brief P-gain for translation + public: double pGainPos{0.0}; + + /// \brief D-gain for translation + public: double dGainPos{0.0}; /// \brief Spring stiffness for rotation, in (rad/s²)/rad - public: double rotStiffness = 200.0; + public: double rotStiffness{200.0}; + + /// \brief P-gain for rotation + public: double pGainRot{0.0}; + + /// \brief D-gain for rotation + public: double dGainRot{0.0}; /// \brief Arrow for visualizing force in translation mode. /// This arrow goes from the application point to the target point. @@ -185,6 +200,9 @@ namespace sim /// \brief Box for visualizing rotation mode public: rendering::VisualPtr boxVisual{nullptr}; + /// \brief Plane for visualizing the wrench application plane + public: rendering::VisualPtr planeVisual{nullptr}; + /// \brief Size of the bounding box of the selected link public: math::Vector3d bboxSize; }; @@ -387,6 +405,7 @@ void MouseDrag::Update(const UpdateInfo &_info, this->dataPtr->mousePressPos = this->dataPtr->mouseEvent.Pos(); this->dataPtr->initialRot = linkWorldPose.Rot(); this->dataPtr->poseLast = linkWorldPose; + this->dataPtr->UpdateGains(inertial->Data()); // Calculate offset of force application from link origin if (this->dataPtr->applyCOM || this->dataPtr->mode == MouseDragMode::ROTATE) @@ -452,7 +471,7 @@ void MouseDrag::Update(const UpdateInfo &_info, math::Quaterniond errorRot = linkWorldPose.Rot() * this->dataPtr->goalRot.Inverse(); msg = this->dataPtr->CalculateWrench( - errorRot.Euler(), _info.dt, linkWorldPose, inertial->Data()); + errorRot.Euler(), _info.dt, linkWorldPose); } else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) { @@ -464,8 +483,7 @@ void MouseDrag::Update(const UpdateInfo &_info, math::Vector3d errorPos = this->dataPtr->applicationPoint - this->dataPtr->target; - msg = this->dataPtr->CalculateWrench( - errorPos, _info.dt, linkWorldPose, inertial->Data()); + msg = this->dataPtr->CalculateWrench(errorPos, _info.dt, linkWorldPose); } // Publish wrench @@ -520,14 +538,31 @@ void MouseDragPrivate::OnRender() this->arrowVisual->ShowArrowHead(true); this->arrowVisual->ShowArrowShaft(true); this->arrowVisual->ShowArrowRotation(false); - this->arrowVisual->SetVisible(false); this->boxVisual = scene->CreateVisual(); this->boxVisual->AddGeometry(scene->CreateBox()); this->boxVisual->SetInheritScale(false); this->boxVisual->SetMaterial(mat); this->boxVisual->SetUserData("gui-only", static_cast(true)); - this->boxVisual->SetVisible(false); + + auto planeMat = this->scene->Material("trans-gray"); + if (!planeMat) + { + planeMat = this->scene->CreateMaterial("trans-gray"); + planeMat->SetAmbient(1.0, 1.0, 1.0); + planeMat->SetDiffuse(1.0, 1.0, 1.0); + planeMat->SetSpecular(1.0, 1.0, 1.0); + planeMat->SetTransparency(0.7); + planeMat->SetCastShadows(false); + planeMat->SetReceiveShadows(false); + planeMat->SetLightingEnabled(false); + } + + this->planeVisual = scene->CreateVisual(); + this->planeVisual->AddGeometry(scene->CreatePlane()); + this->planeVisual->SetInheritScale(false); + this->planeVisual->SetMaterial(planeMat); + this->planeVisual->SetUserData("gui-only", static_cast(true)); } // Update the visualization @@ -535,6 +570,7 @@ void MouseDragPrivate::OnRender() { this->arrowVisual->SetVisible(false); this->boxVisual->SetVisible(false); + this->planeVisual->SetVisible(false); } else if (this->mode == MouseDragMode::ROTATE) { @@ -544,27 +580,27 @@ void MouseDragPrivate::OnRender() this->arrowVisual->SetVisible(false); this->boxVisual->SetVisible(true); + this->planeVisual->SetVisible(false); } else if (this->mode == MouseDragMode::TRANSLATE) { math::Vector3d axisDir = this->target - this->applicationPoint; - math::Vector3d u = axisDir.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); + quat.SetFrom2Axes(math::Vector3d::UnitZ, axisDir); double scale = 2 * this->bboxSize.Length(); - this->arrowVisual->SetLocalPosition(this->applicationPoint); this->arrowVisual->SetLocalRotation(quat); this->arrowVisual->SetLocalScale(scale, scale, axisDir.Length() / 0.75); + quat.SetFrom2Axes(math::Vector3d::UnitZ, -this->plane.Normal()); + scale = this->applicationPoint.Distance(this->camera->WorldPose().Pos()); + this->planeVisual->SetLocalRotation(quat); + this->planeVisual->SetLocalPosition(this->applicationPoint); + this->planeVisual->SetLocalScale(scale); + this->arrowVisual->SetVisible(true); this->boxVisual->SetVisible(false); + this->planeVisual->SetVisible(true); } if (this->sendBlockOrbit) @@ -649,46 +685,61 @@ void MouseDragPrivate::HandleMouseEvents() } } +///////////////////////////////////////////////// +void MouseDragPrivate::UpdateGains(const math::Inertiald &_inertial) +{ + if (this->mode == MouseDragMode::ROTATE) + { + // TODO(anyone): is this the best way to scale rotation gains? + double avgInertia = _inertial.MassMatrix().PrincipalMoments().Sum() / 3; + this->pGainRot = this->rotStiffness * avgInertia; + this->dGainRot = 2 * sqrt(this->rotStiffness) * avgInertia; + this->pGainPos = 0.0; + this->dGainPos = 0.0; + } + else if (this->mode == MouseDragMode::TRANSLATE) + { + double mass = _inertial.MassMatrix().Mass(); + this->pGainPos = this->posStiffness * mass; + this->dGainPos = 0.5 * sqrt(this->posStiffness) * mass; + this->pGainRot = 0.0; + + if (!this->applyCOM) + { + double avgInertia = _inertial.MassMatrix().PrincipalMoments().Sum() / 3; + this->dGainRot = 0.5 * sqrt(this->rotStiffness) * avgInertia; + } + } +} + ///////////////////////////////////////////////// msgs::EntityWrench MouseDragPrivate::CalculateWrench( const math::Vector3d &_error, const std::chrono::duration &_dt, - const math::Pose3d &_linkWorldPose, - const math::Inertiald &_inertial) + const math::Pose3d &_linkWorldPose) { math::Vector3d force, torque; if (this->mode == MouseDragMode::ROTATE) { - // TODO(anyone): is this the best way to scale rotation gains? - double avgInertia = _inertial.MassMatrix().PrincipalMoments().Sum() / 3; - math::Vector3d dErrorRot = (_linkWorldPose.Rot() * this->poseLast.Rot().Inverse()).Euler() / _dt.count(); for (auto i : {0, 1, 2}) { - double pGainRot = this->rotStiffness * avgInertia; - double dGainRot = 2 * sqrt(this->rotStiffness) * avgInertia; - // Correct angle discontinuity around 0/2pi if (dErrorRot[i] > GZ_PI) dErrorRot[i] -= 2 * GZ_PI; else if (dErrorRot[i] < -GZ_PI) dErrorRot[i] += 2 * GZ_PI; - - torque[i] = - pGainRot * _error[i] - dGainRot * dErrorRot[i]; } + torque = - this->pGainRot * _error - this->dGainRot * dErrorRot; } else if (this->mode == MouseDragMode::TRANSLATE) { - double mass = _inertial.MassMatrix().Mass(); - double pGainPos = this->posStiffness * mass; - double dGainPos = 0.5 * sqrt(this->posStiffness) * mass; - math::Vector3d dErrorPos = (_linkWorldPose.Pos() - this->poseLast.Pos()) / _dt.count(); - force = - pGainPos * _error - dGainPos * dErrorPos; + force = - this->pGainPos * _error - this->dGainPos * dErrorPos; torque = _linkWorldPose.Rot().RotateVector(this->offset).Cross(force); @@ -696,23 +747,18 @@ msgs::EntityWrench MouseDragPrivate::CalculateWrench( // resulting rotation if (!this->applyCOM) { - double avgInertia = _inertial.MassMatrix().PrincipalMoments().Sum() / 3; - math::Vector3d dErrorRot = (_linkWorldPose.Rot() * this->poseLast.Rot().Inverse()).Euler() / _dt.count(); for (auto i : {0, 1, 2}) { - double dGainRot = 0.5 * sqrt(this->rotStiffness) * avgInertia; - // Correct angle discontinuity around 0/2pi if (dErrorRot[i] > GZ_PI) dErrorRot[i] -= 2 * GZ_PI; else if (dErrorRot[i] < -GZ_PI) dErrorRot[i] += 2 * GZ_PI; - - torque -= dGainRot * dErrorRot[i]; } + torque -= this->dGainRot * dErrorRot; } } else From f6920d3fc055743034a3d6e7983669f5558d79ac Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Thu, 10 Aug 2023 10:45:06 -0300 Subject: [PATCH 16/20] Add mutex Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index a7bb70e8f7..0bb7f767a1 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -15,10 +15,12 @@ * */ +#include #include #include #include +#include #include #include #include @@ -108,6 +110,9 @@ namespace sim /// \brief Publisher for EntityWrench messages public: transport::Node::Publisher pub; + /// \brief To synchronize member access + public: std::mutex mutex; + /// \brief World name public: std::string worldName; @@ -293,6 +298,7 @@ bool MouseDrag::eventFilter(QObject *_obj, QEvent *_event) void MouseDrag::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { + GZ_PROFILE("MouseDrag::Update"); // Load the ApplyLinkWrench system if (!this->dataPtr->systemLoaded) { @@ -364,6 +370,8 @@ void MouseDrag::Update(const UpdateInfo &_info, } } + std::lock_guard lock(this->dataPtr->mutex); + if (this->dataPtr->mode == MouseDragMode::NONE || _info.paused) { @@ -565,6 +573,8 @@ void MouseDragPrivate::OnRender() this->planeVisual->SetUserData("gui-only", static_cast(true)); } + std::lock_guard lock(this->mutex); + // Update the visualization if (!this->dragActive) { @@ -626,6 +636,8 @@ void MouseDragPrivate::HandleMouseEvents() } this->mouseDirty = false; + std::lock_guard lock(this->mutex); + if (this->mouseEvent.Type() == common::MouseEvent::PRESS && this->mouseEvent.Control() && this->mouseEvent.Button() != common::MouseEvent::MIDDLE) From ba95572f8ec5426716d039fdc32ed26306e9b809 Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Thu, 10 Aug 2023 17:06:52 -0300 Subject: [PATCH 17/20] Started MouseDrag unit and integration tests Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/CMakeLists.txt | 1 + src/gui/plugins/mouse_drag/MouseDrag_TEST.cc | 91 ++++++++++ test/integration/CMakeLists.txt | 8 + test/integration/mouse_drag.cc | 176 +++++++++++++++++++ 4 files changed, 276 insertions(+) create mode 100644 src/gui/plugins/mouse_drag/MouseDrag_TEST.cc create mode 100644 test/integration/mouse_drag.cc diff --git a/src/gui/plugins/mouse_drag/CMakeLists.txt b/src/gui/plugins/mouse_drag/CMakeLists.txt index 9d34ef4653..d7d9ad9cf6 100644 --- a/src/gui/plugins/mouse_drag/CMakeLists.txt +++ b/src/gui/plugins/mouse_drag/CMakeLists.txt @@ -1,4 +1,5 @@ gz_add_gui_plugin(MouseDrag SOURCES MouseDrag.cc QT_HEADERS MouseDrag.hh + TEST_SOURCES MouseDrag_TEST.cc ) diff --git a/src/gui/plugins/mouse_drag/MouseDrag_TEST.cc b/src/gui/plugins/mouse_drag/MouseDrag_TEST.cc new file mode 100644 index 0000000000..3ff469469e --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag_TEST.cc @@ -0,0 +1,91 @@ +/* + * 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. + * +*/ + +#include +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#ifdef _MSC_VER +#pragma warning(pop) +#endif +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "test_config.hh" +#include "../../../../test/helpers/EnvTestFixture.hh" + +#include "../../GuiRunner.hh" + +#include "MouseDrag.hh" + +int g_argc = 1; +char **g_argv; + +using namespace gz; + +/// \brief Tests for the mouse drag GU I plugin +class MouseDragGui : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(MouseDragGui, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) +{ + // Create app + auto app = std::make_unique(g_argc, g_argv); + ASSERT_NE(nullptr, app); + app->AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + + // Create GUI runner to handle sim::gui plugins + auto runner = new sim::GuiRunner("default"); + runner->setParent(gui::App()); + + // Add plugin + EXPECT_TRUE(app->LoadPlugin("MouseDrag")); + + // Get main window + auto win = app->findChild(); + ASSERT_NE(nullptr, win); + + // Get plugin + auto plugins = win->findChildren< + sim::MouseDrag *>(); + EXPECT_EQ(plugins.size(), 1); + + auto plugin = plugins[0]; + + EXPECT_EQ(plugin->Title(), "Mouse drag"); + + transport::Node node; + auto topic = transport::TopicUtils::AsValidTopic("/world/default/wrench"); + EXPECT_FALSE(topic.empty()); + + std::vector topics; + node.TopicList(topics); + EXPECT_NE(std::find(topics.begin(), topics.end(), topic), topics.end()); + + // Cleanup + plugins.clear(); +} diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 59eae5c5c4..9d405bcc99 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -51,6 +51,7 @@ set(tests model.cc model_photo_shoot_default_joints.cc model_photo_shoot_random_joints.cc + mouse_drag.cc multicopter.cc multiple_servers.cc navsat_system.cc @@ -210,6 +211,13 @@ if(TARGET INTEGRATION_reset_sensors) ) endif() +if(TARGET INTEGRATION_mouse_drag) + target_link_libraries(INTEGRATION_mouse_drag + # gz-sim7-gui + ${PROJECT_LIBRARY_TARGET_NAME}-gui + ) +endif() + # The default timeout (240s) doesn't seem to be enough for this test. if(TARGET INTEGRATION_tracked_vehicle_system ) set_tests_properties(INTEGRATION_tracked_vehicle_system PROPERTIES TIMEOUT 300) diff --git a/test/integration/mouse_drag.cc b/test/integration/mouse_drag.cc new file mode 100644 index 0000000000..6a01d2cba6 --- /dev/null +++ b/test/integration/mouse_drag.cc @@ -0,0 +1,176 @@ +/* + * 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. + * + */ + +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gz/sim/TestFixture.hh" +#include "test_config.hh" +#include "../helpers/EnvTestFixture.hh" + +#include "gz/sim/gui/Gui.hh" + +int g_argc = 1; +char **g_argv; + +using namespace gz; +using namespace sim; +using namespace std::chrono_literals; + +/// \brief Test fixture for MouseDrag plugin +class MouseDragTestFixture : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(MouseDragTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(ApplyLinkWrenchLoaded)) +{ + // Run server in headless mode + const std::string kGzCommand = + common::joinPaths(getInstallPrefix(), "bin", "gz"); + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "shapes.sdf"); + std::vector env = { + "LD_LIBRARY_PATH=" + common::joinPaths(getInstallPrefix(), "lib"), + "HOME=/home/henrique", + }; + auto serverProc = utils::Subprocess( + {kGzCommand, "sim", "-s", "-r", "-v4", sdfFile}, env); + EXPECT_TRUE(serverProc.Alive()); + gzdbg << "Server alive " << serverProc.Alive() << std::endl; + + // Create app + auto app = gz::sim::gui::createGui( + g_argc, g_argv, nullptr, nullptr, false, nullptr); + gzmsg << "GUI created" << std::endl; + ASSERT_NE(nullptr, app); + + // Add plugin + app->AddPluginPath(common::joinPaths(PROJECT_BINARY_PATH, "lib")); + EXPECT_TRUE(app->LoadPlugin("MouseDrag")); + gzmsg << "MouseDrag plugin loaded" << std::endl; + + // Setup ECM state callback + EntityComponentManager ecm; + std::function cb = + [&](const msgs::SerializedStepMap &_msg) + { + // gzdbg << "Get state" << std::endl; + ecm.SetState(_msg.state()); + }; + std::string id = std::to_string(gz::gui::App()->applicationPid()); + transport::Node node; + std::string reqSrv = transport::TopicUtils::AsValidTopic( + common::joinPaths(node.Options().NameSpace(), id, "state_async")); + EXPECT_FALSE(reqSrv.empty()); + EXPECT_TRUE(node.Advertise(reqSrv, cb)); + gzmsg << "Callback on " << reqSrv << std::endl; + + // Wait for state_async service + std::string stateAsyncTopic = transport::TopicUtils::AsValidTopic( + "/world/default/state_async"); + EXPECT_FALSE(stateAsyncTopic.empty()); + std::vector services; + node.ServiceList(services); + while (std::find(services.begin(), services.end(), stateAsyncTopic) == + services.end()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + node.ServiceList(services); + } + + // Request initial ECM state + msgs::StringMsg req; + req.set_data(reqSrv); + unsigned int timeout = 100; + msgs::Empty res; + bool result{false}; + gzmsg << "Requesting " << stateAsyncTopic << std::endl; + node.Request(stateAsyncTopic, req, timeout, res, result); + + // Subscribe to future ECM updates + std::string stateTopic = transport::TopicUtils::AsValidTopic( + "/world/default/state"); + EXPECT_FALSE(stateTopic.empty()); + EXPECT_TRUE(node.Subscribe(stateTopic, cb)); + + // Get world entity + Entity worldEntity; + ecm.Each( + [&](const Entity &_entity, + const components::World */*_world*/, + const components::Name *_name)->bool + { + if (_name->Data() == "default") + { + worldEntity = _entity; + return false; + } + return true; + }); + gzmsg << "worldEntity " << worldEntity << std::endl; + + // Check for ApplyLinkWrench system + int sleep = 0; + int maxSleep = 30; + bool systemLoaded{false}; + while (!systemLoaded && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + + // Check if ApplyLinkWrench system is loaded + if (auto msg = ecm.ComponentData(worldEntity)) + { + for (const auto &plugin : msg->plugins()) + { + if (plugin.filename() == "gz-sim-apply-link-wrench-system") + { + systemLoaded = true; + break; + } + } + } + + sleep++; + } + EXPECT_LT(sleep, maxSleep); + gzmsg << "System loaded " << systemLoaded << std::endl; + + // Cleanup + serverProc.Terminate(); + gzdbg << "Server stdout:\n" << serverProc.Stdout() << std::endl; + gzdbg << "Server stderr:\n" << serverProc.Stderr() << std::endl; +} From ee833a2d3672918ddcb1b388fbe072d236c48bed Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Tue, 15 Aug 2023 12:24:06 -0300 Subject: [PATCH 18/20] Use events to test translation mode in MouseDrag Signed-off-by: Henrique-BO --- test/integration/CMakeLists.txt | 22 ++- test/integration/TestHelper.hh | 68 +++++++ test/integration/mouse_drag.cc | 303 ++++++++++++++++++++++++-------- test/worlds/mouse_drag.sdf | 138 +++++++++++++++ 4 files changed, 457 insertions(+), 74 deletions(-) create mode 100644 test/integration/TestHelper.hh create mode 100644 test/worlds/mouse_drag.sdf diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 9d405bcc99..f11c03bf7b 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -103,6 +103,7 @@ set(tests_needing_display distortion_camera.cc gpu_lidar.cc mesh_uri.cc + mouse_drag.cc optical_tactile_plugin.cc reset_sensors.cc rgbd_camera.cc @@ -212,9 +213,28 @@ if(TARGET INTEGRATION_reset_sensors) endif() if(TARGET INTEGRATION_mouse_drag) + find_package(Qt5Test REQUIRED) + + set (qt_headers + TestHelper.hh + ) + + QT5_WRAP_CPP(headers_MOC ${qt_headers}) + + add_library(${PROJECT_NAME}_test_helpers SHARED + # TestHelper.cc + ${headers_MOC} + ) + target_compile_definitions(${PROJECT_NAME}_test_helpers PRIVATE TestHelper_EXPORTS) + target_link_libraries(${PROJECT_NAME}_test_helpers + PUBLIC + ${PROJECT_LIBRARY_TARGET_NAME} + ) + target_link_libraries(INTEGRATION_mouse_drag - # gz-sim7-gui ${PROJECT_LIBRARY_TARGET_NAME}-gui + Qt5::Test + ${PROJECT_NAME}_test_helpers ) endif() diff --git a/test/integration/TestHelper.hh b/test/integration/TestHelper.hh new file mode 100644 index 0000000000..e86ed3cb0a --- /dev/null +++ b/test/integration/TestHelper.hh @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2021 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_GUI_TESTHELPER_HH_ +#define GZ_GUI_TESTHELPER_HH_ + +#include +#include +#include + +#ifndef _WIN32 +# define TestHelper_EXPORTS_API +#else +# if (defined(TestHelper_EXPORTS)) +# define TestHelper_EXPORTS_API __declspec(dllexport) +# else +# define TestHelper_EXPORTS_API __declspec(dllimport) +# endif +#endif + +namespace gz +{ +namespace sim +{ +/// \brief +class TestHelper_EXPORTS_API TestHelper : public QObject +{ + Q_OBJECT + + /// \brief Constructor + public: inline TestHelper() + { + gz::gui::App()->findChild()-> + installEventFilter(this); + } + + /// \brief Destructor + public: ~TestHelper() = default; + + /// \brief Documentation inherited + public: inline bool eventFilter(QObject *_obj, QEvent *_event) override + { + if (this->forwardEvent) + this->forwardEvent(_event); + + // Standard event processing + return QObject::eventFilter(_obj, _event); + } + + public: std::function forwardEvent; +}; +} +} + +#endif diff --git a/test/integration/mouse_drag.cc b/test/integration/mouse_drag.cc index 6a01d2cba6..ac41e275b0 100644 --- a/test/integration/mouse_drag.cc +++ b/test/integration/mouse_drag.cc @@ -16,8 +16,7 @@ */ #include - -#include +#include #include #include @@ -27,23 +26,33 @@ #include #include #include +#include +#include +#include +#include +#include #include #include #include #include +#include #include +#include #include #include #include "gz/sim/TestFixture.hh" #include "test_config.hh" #include "../helpers/EnvTestFixture.hh" +#include "TestHelper.hh" #include "gz/sim/gui/Gui.hh" int g_argc = 1; char **g_argv; +double tol = 1e-1; + using namespace gz; using namespace sim; using namespace std::chrono_literals; @@ -51,6 +60,88 @@ using namespace std::chrono_literals; /// \brief Test fixture for MouseDrag plugin class MouseDragTestFixture : public InternalFixture<::testing::Test> { + protected: utils::Subprocess StartServer(const std::string &_sdfFile) + { + std::vector cmd = { + common::joinPaths(getInstallPrefix(), "bin", "gz"), + "sim", "-s", "-r", "-v4", /* "--iterations=1000", */_sdfFile}; + std::vector env = { + "LD_LIBRARY_PATH=" + common::joinPaths(getInstallPrefix(), "lib")}; + auto serverProc = utils::Subprocess(cmd, env); + EXPECT_TRUE(serverProc.Alive()); + + return serverProc; + } + + protected: std::unique_ptr StartGUI() + { + auto app = gz::sim::gui::createGui( + g_argc, g_argv, nullptr, nullptr, false, nullptr); + app->AddPluginPath(common::joinPaths(PROJECT_BINARY_PATH, "lib")); + gzdbg << "GUI created" << std::endl; + EXPECT_NE(nullptr, app); + + // Subscribe to future ECM updates + std::string stateTopic = transport::TopicUtils::AsValidTopic( + "/world/default/state"); + EXPECT_FALSE(stateTopic.empty()); + EXPECT_TRUE( + this->node.Subscribe(stateTopic, &MouseDragTestFixture::OnState, this)); + + return app; + } + + protected: void RequestState() + { + std::string stateAsyncTopic = transport::TopicUtils::AsValidTopic( + "/world/default/state_async"); + EXPECT_FALSE(stateAsyncTopic.empty()); + std::string id = std::to_string(gz::gui::App()->applicationPid()); + std::string reqSrv = transport::TopicUtils::AsValidTopic( + "/" + id + "/state_async"); + EXPECT_FALSE(reqSrv.empty()); + + auto advertised = this->node.AdvertisedServices(); + if (std::find(advertised.begin(), advertised.end(), reqSrv) == + advertised.end()) + { + EXPECT_TRUE( + this->node.Advertise(reqSrv, &MouseDragTestFixture::OnState, this)); + } + + msgs::StringMsg req; + req.set_data(reqSrv); + unsigned int timeout = 500; + msgs::Empty res; + bool result{false}; + gzdbg << "Requesting " << stateAsyncTopic << std::endl; + this->stateUpdated = false; + this->node.Request(stateAsyncTopic, req, timeout, res, result); + + // Wait for initial state + unsigned int sleep = 0; + unsigned int maxSleep = 30; + while (!this->stateUpdated && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++sleep; + } + EXPECT_LT(sleep, maxSleep); + } + + protected: void OnState(const msgs::SerializedStepMap &_msg) + { + if (!this->stateUpdated) + gzdbg << "State request answered" << std::endl; + this->stateUpdated = true; + this->ecm.SetState(_msg.state()); + } + + protected: transport::Node node; + + protected: EntityComponentManager ecm; + + protected: bool stateUpdated{false}; }; ///////////////////////////////////////////////// @@ -58,77 +149,16 @@ TEST_F(MouseDragTestFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(ApplyLinkWrenchLoaded)) { // Run server in headless mode - const std::string kGzCommand = - common::joinPaths(getInstallPrefix(), "bin", "gz"); const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), - "test", "worlds", "shapes.sdf"); - std::vector env = { - "LD_LIBRARY_PATH=" + common::joinPaths(getInstallPrefix(), "lib"), - "HOME=/home/henrique", - }; - auto serverProc = utils::Subprocess( - {kGzCommand, "sim", "-s", "-r", "-v4", sdfFile}, env); - EXPECT_TRUE(serverProc.Alive()); - gzdbg << "Server alive " << serverProc.Alive() << std::endl; - - // Create app - auto app = gz::sim::gui::createGui( - g_argc, g_argv, nullptr, nullptr, false, nullptr); - gzmsg << "GUI created" << std::endl; - ASSERT_NE(nullptr, app); - - // Add plugin - app->AddPluginPath(common::joinPaths(PROJECT_BINARY_PATH, "lib")); - EXPECT_TRUE(app->LoadPlugin("MouseDrag")); - gzmsg << "MouseDrag plugin loaded" << std::endl; - - // Setup ECM state callback - EntityComponentManager ecm; - std::function cb = - [&](const msgs::SerializedStepMap &_msg) - { - // gzdbg << "Get state" << std::endl; - ecm.SetState(_msg.state()); - }; - std::string id = std::to_string(gz::gui::App()->applicationPid()); - transport::Node node; - std::string reqSrv = transport::TopicUtils::AsValidTopic( - common::joinPaths(node.Options().NameSpace(), id, "state_async")); - EXPECT_FALSE(reqSrv.empty()); - EXPECT_TRUE(node.Advertise(reqSrv, cb)); - gzmsg << "Callback on " << reqSrv << std::endl; - - // Wait for state_async service - std::string stateAsyncTopic = transport::TopicUtils::AsValidTopic( - "/world/default/state_async"); - EXPECT_FALSE(stateAsyncTopic.empty()); - std::vector services; - node.ServiceList(services); - while (std::find(services.begin(), services.end(), stateAsyncTopic) == - services.end()) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - node.ServiceList(services); - } + "test", "worlds", "mouse_drag.sdf"); + auto serverProc = this->StartServer(sdfFile); // Request initial ECM state - msgs::StringMsg req; - req.set_data(reqSrv); - unsigned int timeout = 100; - msgs::Empty res; - bool result{false}; - gzmsg << "Requesting " << stateAsyncTopic << std::endl; - node.Request(stateAsyncTopic, req, timeout, res, result); - - // Subscribe to future ECM updates - std::string stateTopic = transport::TopicUtils::AsValidTopic( - "/world/default/state"); - EXPECT_FALSE(stateTopic.empty()); - EXPECT_TRUE(node.Subscribe(stateTopic, cb)); + this->RequestState(); // Get world entity Entity worldEntity; - ecm.Each( + this->ecm.Each( [&](const Entity &_entity, const components::World */*_world*/, const components::Name *_name)->bool @@ -140,19 +170,80 @@ TEST_F(MouseDragTestFixture, } return true; }); - gzmsg << "worldEntity " << worldEntity << std::endl; + gzdbg << "worldEntity " << worldEntity << std::endl; + + // Setup GUI + auto app = this->StartGUI(); + EXPECT_TRUE(app->LoadPlugin("MouseDrag")); + gzdbg << "MouseDrag plugin loaded" << std::endl; + + // Get main window + auto win = app->findChild(); + ASSERT_NE(nullptr, win); + + // Show, but don't exec, so we don't block + win->QuickWindow()->show(); + + // Catch rendering thread + rendering::ScenePtr scene; + rendering::CameraPtr camera; + std::optional event; + auto testHelper = std::make_unique(); + testHelper->forwardEvent = [&](QEvent *_event) + { + if (_event->type() == gz::gui::events::Render::kType) + { + gzdbg << "Render" << std::endl; + // Get scene and user camera + if (!scene) + { + scene = rendering::sceneFromFirstRenderEngine(); + EXPECT_NE(nullptr, scene); + for (unsigned int i = 0; i < scene->NodeCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + scene->NodeByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + camera = cam; + break; + } + } + EXPECT_NE(nullptr, camera); + } + // Send events + if (event) + { + app->sendEvent(app->findChild(), *event); + event.reset(); + } + } + }; + + // Wait for scene + unsigned int sleep = 0; + unsigned int maxSleep = 30; + while (!scene && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + ++sleep; + } + EXPECT_LT(sleep, maxSleep); // Check for ApplyLinkWrench system - int sleep = 0; - int maxSleep = 30; + sleep = 0; bool systemLoaded{false}; while (!systemLoaded && sleep < maxSleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); QCoreApplication::processEvents(); + gzdbg << "Searching for system" << std::endl; // Check if ApplyLinkWrench system is loaded - if (auto msg = ecm.ComponentData(worldEntity)) + if (auto msg = + this->ecm.ComponentData(worldEntity)) { for (const auto &plugin : msg->plugins()) { @@ -163,14 +254,80 @@ TEST_F(MouseDragTestFixture, } } } - + else + { + this->RequestState(); + } sleep++; } EXPECT_LT(sleep, maxSleep); - gzmsg << "System loaded " << systemLoaded << std::endl; + gzdbg << "System loaded " << systemLoaded << std::endl; + + // Get box initial position + Entity box = ecm.EntityByComponents(components::Model(), + components::Name("box")); + auto poseInitial = ecm.ComponentData(box); + EXPECT_TRUE(poseInitial.has_value()); + + // Translation + gzdbg << "Ctrl-Right Click" << std::endl; + common::MouseEvent mouse; + auto pos = camera->Project(poseInitial->Pos()); + mouse.SetPos(pos); + mouse.SetControl(true); + mouse.SetType(common::MouseEvent::PRESS); + mouse.SetButton(common::MouseEvent::RIGHT); + gz::gui::events::LeftClickOnScene leftClickEvent(mouse); + event = &leftClickEvent; + + // Wait for event to be sent + sleep = 0; + while (event && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + ++sleep; + } + EXPECT_LT(sleep, maxSleep); + + gzdbg << "Drag upwards" << std::endl; + mouse.SetPos(pos.X(), pos.Y()-100); + mouse.SetType(common::MouseEvent::MOVE); + gz::gui::events::DragOnScene dragEvent(mouse); + event = &dragEvent; + + // Wait for event to be sent + sleep = 0; + while (event && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + ++sleep; + } + EXPECT_LT(sleep, maxSleep); + + sleep = 0; + auto pose = ecm.ComponentData(box); + while (pose->Z() - poseInitial->Z() < tol && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + pose = ecm.ComponentData(box); + ++sleep; + } + EXPECT_LT(sleep, maxSleep); // Cleanup serverProc.Terminate(); + sleep = 0; + while (serverProc.Alive() && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++sleep; + } + EXPECT_LT(sleep, maxSleep); gzdbg << "Server stdout:\n" << serverProc.Stdout() << std::endl; gzdbg << "Server stderr:\n" << serverProc.Stderr() << std::endl; + + win->QuickWindow()->close(); } diff --git a/test/worlds/mouse_drag.sdf b/test/worlds/mouse_drag.sdf new file mode 100644 index 0000000000..f0ea301bcb --- /dev/null +++ b/test/worlds/mouse_drag.sdf @@ -0,0 +1,138 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 50 + 200 + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + \ No newline at end of file From 78f86a5ec41973228731b1111a019df9d1664f8b Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Wed, 16 Aug 2023 15:26:54 -0300 Subject: [PATCH 19/20] Refactoring and minor changes Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag.cc | 163 +++++++++++++++--------- 1 file changed, 101 insertions(+), 62 deletions(-) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc index 0bb7f767a1..fd0274e305 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -104,6 +104,17 @@ namespace sim const std::chrono::duration &_dt, const math::Pose3d &_linkWorldPose); + /// \brief Sets the RayQuery from the user camera to the given position + /// on the screen + /// \param[in] _pos Position on the screen + public: void SetRayFromCamera(const math::Vector2i &_pos); + + /// \brief Corrects an angle so that it is in the [-pi; pi] interval, + /// in order to eliminate the discontinuity around 0 and 2pi + /// \param[in] _angle The angle in radians + /// \return The corrected angle + public: double CorrectAngle(const double _angle); + /// \brief Transport node public: transport::Node node; @@ -233,11 +244,11 @@ void MouseDrag::LoadConfig(const tinyxml2::XMLElement */*_pluginElem*/) this->title = "Mouse drag"; // Create wrench publisher - auto worldNames = gz::gui::worldNames(); + const auto worldNames = gz::gui::worldNames(); if (!worldNames.empty()) { this->dataPtr->worldName = worldNames[0].toStdString(); - auto topic = transport::TopicUtils::AsValidTopic( + const auto topic = transport::TopicUtils::AsValidTopic( "/world/" + this->dataPtr->worldName + "/wrench"); if (topic == "") { @@ -248,6 +259,11 @@ void MouseDrag::LoadConfig(const tinyxml2::XMLElement */*_pluginElem*/) this->dataPtr->node.Advertise(topic); gzdbg << "Created publisher to " << topic << std::endl; } + else + { + gzerr << "Unable to find world" << std::endl; + return; + } gz::gui::App()->findChild ()->installEventFilter(this); @@ -262,28 +278,34 @@ bool MouseDrag::eventFilter(QObject *_obj, QEvent *_event) } else if (_event->type() == gz::gui::events::LeftClickOnScene::kType) { - auto event = + // Mutex can't be locked on the whole eventFilter because that causes + // the program to freeze, since the Update function sends GUI events + std::lock_guard lock(this->dataPtr->mutex); + const auto event = static_cast(_event); this->dataPtr->mouseEvent = event->Mouse(); this->dataPtr->mouseDirty = true; } else if (_event->type() == gz::gui::events::RightClickOnScene::kType) { - auto event = + std::lock_guard lock(this->dataPtr->mutex); + const auto event = static_cast(_event); this->dataPtr->mouseEvent = event->Mouse(); this->dataPtr->mouseDirty = true; } else if (_event->type() == gz::gui::events::MousePressOnScene::kType) { - auto event = + std::lock_guard lock(this->dataPtr->mutex); + const auto event = static_cast(_event); this->dataPtr->mouseEvent = event->Mouse(); this->dataPtr->mouseDirty = true; } else if (_event->type() == gz::gui::events::DragOnScene::kType) { - auto event = + std::lock_guard lock(this->dataPtr->mutex); + const auto event = static_cast(_event); this->dataPtr->mouseEvent = event->Mouse(); this->dataPtr->mouseDirty = true; @@ -322,7 +344,8 @@ void MouseDrag::Update(const UpdateInfo &_info, }); // Check if already loaded - auto msg = _ecm.ComponentData(worldEntity); + const auto msg = + _ecm.ComponentData(worldEntity); if (!msg) { gzdbg << "Unable to find SystemPluginInfo component for entity " @@ -351,9 +374,14 @@ void MouseDrag::Update(const UpdateInfo &_info, msgs::Boolean res; bool result; - unsigned int timeout = 5000; - std::string service{"/world/" + this->dataPtr->worldName + - "/entity/system/add"}; + const unsigned int timeout = 5000; + const auto service = transport::TopicUtils::AsValidTopic( + "/world/" + this->dataPtr->worldName + "/entity/system/add"); + if (service.empty()) + { + gzerr << "Unable to request " << service << std::endl; + return; + } if (this->dataPtr->node.Request(service, req, timeout, res, result)) { this->dataPtr->systemLoaded = true; @@ -366,6 +394,7 @@ void MouseDrag::Update(const UpdateInfo &_info, << "Name: " << name << "\n" << "Filename: " << filename << "\n" << "Inner XML: " << innerxml << std::endl; + return; } } } @@ -381,21 +410,18 @@ void MouseDrag::Update(const UpdateInfo &_info, } // Get Link corresponding to clicked Visual - Link link(this->dataPtr->linkId); - auto model = link.ParentModel(_ecm); - if (!link.Valid(_ecm) || model->Static(_ecm)) + const Link link(this->dataPtr->linkId); + const auto model = link.ParentModel(_ecm); + const auto linkWorldPose = worldPose(this->dataPtr->linkId, _ecm); + const auto inertial = + _ecm.Component(this->dataPtr->linkId); + if (!link.Valid(_ecm) || !inertial || + !model->Valid(_ecm) || model->Static(_ecm)) { this->dataPtr->blockOrbit = false; return; } - auto linkWorldPose = worldPose(this->dataPtr->linkId, _ecm); - auto inertial = _ecm.Component(this->dataPtr->linkId); - if (!inertial) - { - return; - } - if (this->dataPtr->blockOrbit) this->dataPtr->sendBlockOrbit = true; @@ -431,8 +457,10 @@ void MouseDrag::Update(const UpdateInfo &_info, // The plane of wrench application should be normal to the center // of the camera view and pass through the application point - math::Vector3d direction = this->dataPtr->camera->WorldPose().Rot().XAxis(); - double planeOffset = direction.Dot(this->dataPtr->applicationPoint); + const math::Vector3d direction = + this->dataPtr->camera->WorldPose().Rot().XAxis(); + const double planeOffset = + direction.Dot(this->dataPtr->applicationPoint); this->dataPtr->plane = math::Planed(direction, planeOffset); } // Track the application point @@ -443,30 +471,19 @@ void MouseDrag::Update(const UpdateInfo &_info, linkWorldPose.Rot().RotateVector(this->dataPtr->offset); } - // Normalize mouse position on the image - double width = this->dataPtr->camera->ImageWidth(); - double height = this->dataPtr->camera->ImageHeight(); - - double nx = 2.0 * this->dataPtr->mouseEvent.Pos().X() / width - 1.0; - double ny = 1.0 - 2.0 * this->dataPtr->mouseEvent.Pos().Y() / height; - // Make a ray query at the mouse position - this->dataPtr->rayQuery->SetFromCamera( - this->dataPtr->camera, math::Vector2d(nx, ny)); - math::Vector3d end = this->dataPtr->rayQuery->Direction(); + this->dataPtr->SetRayFromCamera(this->dataPtr->mouseEvent.Pos()); + const math::Vector3d end = this->dataPtr->rayQuery->Direction(); // Wrench in world coordinates, applied at the link origin msgs::EntityWrench msg; if (this->dataPtr->mode == MouseDragMode::ROTATE) { // Calculate rotation angle from mouse displacement - nx = 2.0 * this->dataPtr->mousePressPos.X() / width - 1.0; - ny = 1.0 - 2.0 * this->dataPtr->mousePressPos.Y() / height; - this->dataPtr->rayQuery->SetFromCamera( - this->dataPtr->camera, math::Vector2d(nx, ny)); - math::Vector3d start = this->dataPtr->rayQuery->Direction(); + this->dataPtr->SetRayFromCamera(this->dataPtr->mousePressPos); + const math::Vector3d start = this->dataPtr->rayQuery->Direction(); math::Vector3d axis = start.Cross(end); - double angle = -atan2(axis.Length(), start.Dot(end)); + const double angle = -atan2(axis.Length(), start.Dot(end)); // Project rotation axis onto plane axis -= axis.Dot(this->dataPtr->plane.Normal()) * @@ -476,20 +493,20 @@ void MouseDrag::Update(const UpdateInfo &_info, // Calculate the necessary rotation and torque this->dataPtr->goalRot = math::Quaterniond(axis, angle) * this->dataPtr->initialRot; - math::Quaterniond errorRot = + const math::Quaterniond errorRot = linkWorldPose.Rot() * this->dataPtr->goalRot.Inverse(); msg = this->dataPtr->CalculateWrench( errorRot.Euler(), _info.dt, linkWorldPose); } else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) { - math::Vector3d origin = this->dataPtr->rayQuery->Origin(); - if (auto t = this->dataPtr->plane.Intersection(origin, end)) + const math::Vector3d origin = this->dataPtr->rayQuery->Origin(); + if (const auto t = this->dataPtr->plane.Intersection(origin, end)) this->dataPtr->target = *t; else return; - math::Vector3d errorPos = + const math::Vector3d errorPos = this->dataPtr->applicationPoint - this->dataPtr->target; msg = this->dataPtr->CalculateWrench(errorPos, _info.dt, linkWorldPose); } @@ -507,6 +524,8 @@ void MouseDrag::OnSwitchCOM(const bool _checked) ///////////////////////////////////////////////// void MouseDragPrivate::OnRender() { + std::lock_guard lock(this->mutex); + // Get scene and user camera if (!this->scene) { @@ -518,7 +537,7 @@ void MouseDragPrivate::OnRender() for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) { - auto cam = std::dynamic_pointer_cast( + const auto cam = std::dynamic_pointer_cast( this->scene->NodeByIndex(i)); if (cam && cam->HasUserData("user-camera") && std::get(cam->UserData("user-camera"))) @@ -551,7 +570,7 @@ void MouseDragPrivate::OnRender() this->boxVisual->AddGeometry(scene->CreateBox()); this->boxVisual->SetInheritScale(false); this->boxVisual->SetMaterial(mat); - this->boxVisual->SetUserData("gui-only", static_cast(true)); + this->boxVisual->SetUserData("gui-only", true); auto planeMat = this->scene->Material("trans-gray"); if (!planeMat) @@ -564,6 +583,8 @@ void MouseDragPrivate::OnRender() planeMat->SetCastShadows(false); planeMat->SetReceiveShadows(false); planeMat->SetLightingEnabled(false); + planeMat->SetDepthCheckEnabled(false); + planeMat->SetDepthWriteEnabled(false); } this->planeVisual = scene->CreateVisual(); @@ -573,8 +594,6 @@ void MouseDragPrivate::OnRender() this->planeVisual->SetUserData("gui-only", static_cast(true)); } - std::lock_guard lock(this->mutex); - // Update the visualization if (!this->dragActive) { @@ -594,7 +613,7 @@ void MouseDragPrivate::OnRender() } else if (this->mode == MouseDragMode::TRANSLATE) { - math::Vector3d axisDir = this->target - this->applicationPoint; + const math::Vector3d axisDir = this->target - this->applicationPoint; math::Quaterniond quat; quat.SetFrom2Axes(math::Vector3d::UnitZ, axisDir); double scale = 2 * this->bboxSize.Length(); @@ -643,7 +662,7 @@ void MouseDragPrivate::HandleMouseEvents() this->mouseEvent.Button() != common::MouseEvent::MIDDLE) { // Get the visual at mouse position - rendering::VisualPtr visual = this->scene->VisualAt( + const rendering::VisualPtr visual = this->scene->VisualAt( this->camera, this->mouseEvent.Pos()); @@ -703,7 +722,8 @@ void MouseDragPrivate::UpdateGains(const math::Inertiald &_inertial) if (this->mode == MouseDragMode::ROTATE) { // TODO(anyone): is this the best way to scale rotation gains? - double avgInertia = _inertial.MassMatrix().PrincipalMoments().Sum() / 3; + const double avgInertia = + _inertial.MassMatrix().PrincipalMoments().Sum() / 3; this->pGainRot = this->rotStiffness * avgInertia; this->dGainRot = 2 * sqrt(this->rotStiffness) * avgInertia; this->pGainPos = 0.0; @@ -711,14 +731,16 @@ void MouseDragPrivate::UpdateGains(const math::Inertiald &_inertial) } else if (this->mode == MouseDragMode::TRANSLATE) { - double mass = _inertial.MassMatrix().Mass(); + const double mass = + _inertial.MassMatrix().Mass(); this->pGainPos = this->posStiffness * mass; this->dGainPos = 0.5 * sqrt(this->posStiffness) * mass; this->pGainRot = 0.0; if (!this->applyCOM) { - double avgInertia = _inertial.MassMatrix().PrincipalMoments().Sum() / 3; + const double avgInertia = + _inertial.MassMatrix().PrincipalMoments().Sum() / 3; this->dGainRot = 0.5 * sqrt(this->rotStiffness) * avgInertia; } } @@ -738,17 +760,13 @@ msgs::EntityWrench MouseDragPrivate::CalculateWrench( / _dt.count(); for (auto i : {0, 1, 2}) { - // Correct angle discontinuity around 0/2pi - if (dErrorRot[i] > GZ_PI) - dErrorRot[i] -= 2 * GZ_PI; - else if (dErrorRot[i] < -GZ_PI) - dErrorRot[i] += 2 * GZ_PI; + dErrorRot[i] = this->CorrectAngle(dErrorRot[i]); } torque = - this->pGainRot * _error - this->dGainRot * dErrorRot; } else if (this->mode == MouseDragMode::TRANSLATE) { - math::Vector3d dErrorPos = + const math::Vector3d dErrorPos = (_linkWorldPose.Pos() - this->poseLast.Pos()) / _dt.count(); force = - this->pGainPos * _error - this->dGainPos * dErrorPos; @@ -764,11 +782,7 @@ msgs::EntityWrench MouseDragPrivate::CalculateWrench( / _dt.count(); for (auto i : {0, 1, 2}) { - // Correct angle discontinuity around 0/2pi - if (dErrorRot[i] > GZ_PI) - dErrorRot[i] -= 2 * GZ_PI; - else if (dErrorRot[i] < -GZ_PI) - dErrorRot[i] += 2 * GZ_PI; + dErrorRot[i] = this->CorrectAngle(dErrorRot[i]); } torque -= this->dGainRot * dErrorRot; } @@ -787,5 +801,30 @@ msgs::EntityWrench MouseDragPrivate::CalculateWrench( return msg; } +///////////////////////////////////////////////// +void MouseDragPrivate::SetRayFromCamera(const math::Vector2i &_pos) +{ + // Normalize position on the image + const double width = this->camera->ImageWidth(); + const double height = this->camera->ImageHeight(); + + const double nx = 2.0 * _pos.X() / width - 1.0; + const double ny = 1.0 - 2.0 * _pos.Y() / height; + + // Make a ray query at the given position + this->rayQuery->SetFromCamera(this->camera, math::Vector2d(nx, ny)); +} + + +double MouseDragPrivate::CorrectAngle(const double _angle) +{ + double result = _angle; + if (_angle > GZ_PI) + result -= 2 * GZ_PI; + else if (_angle < -GZ_PI) + result += 2 * GZ_PI; + return result; +} + // Register this plugin GZ_ADD_PLUGIN(MouseDrag, gz::gui::Plugin); From 85f4d15ad7f293f34202b548a80d610ce3c1cace Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Wed, 23 Aug 2023 14:04:27 -0300 Subject: [PATCH 20/20] Refactor GuiRelay Signed-off-by: Henrique-BO --- src/gui/plugins/mouse_drag/MouseDrag_TEST.cc | 5 +- test/CMakeLists.txt | 1 + test/helpers/CMakeLists.txt | 15 +++++ .../TestHelper.hh => helpers/GuiRelay.hh} | 51 +++++++++++------ test/integration/CMakeLists.txt | 19 ------- test/integration/mouse_drag.cc | 56 ++++++++++--------- test/worlds/mouse_drag.sdf | 2 +- 7 files changed, 83 insertions(+), 66 deletions(-) create mode 100644 test/helpers/CMakeLists.txt rename test/{integration/TestHelper.hh => helpers/GuiRelay.hh} (57%) diff --git a/src/gui/plugins/mouse_drag/MouseDrag_TEST.cc b/src/gui/plugins/mouse_drag/MouseDrag_TEST.cc index 3ff469469e..2cd9716e9a 100644 --- a/src/gui/plugins/mouse_drag/MouseDrag_TEST.cc +++ b/src/gui/plugins/mouse_drag/MouseDrag_TEST.cc @@ -13,7 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ +*/ #include #ifdef _MSC_VER @@ -56,7 +56,8 @@ TEST_F(MouseDragGui, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) // Create app auto app = std::make_unique(g_argc, g_argv); ASSERT_NE(nullptr, app); - app->AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + app->AddPluginPath( + common::joinPaths(std::string(PROJECT_BINARY_PATH), "lib")); // Create GUI runner to handle sim::gui plugins auto runner = new sim::GuiRunner("default"); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 0436a5fc32..f2019471a6 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -7,6 +7,7 @@ include_directories ( add_subdirectory(gtest_vendor) add_subdirectory(benchmark) +add_subdirectory(helpers) add_subdirectory(integration) add_subdirectory(performance) add_subdirectory(plugins) diff --git a/test/helpers/CMakeLists.txt b/test/helpers/CMakeLists.txt new file mode 100644 index 0000000000..18c86fc019 --- /dev/null +++ b/test/helpers/CMakeLists.txt @@ -0,0 +1,15 @@ +find_package(Qt5Test REQUIRED) + +set (qt_headers + GuiRelay.hh +) + +QT5_WRAP_CPP(headers_MOC ${qt_headers}) + +add_library(${PROJECT_NAME}_test_helpers SHARED + ${headers_MOC} +) +target_link_libraries(${PROJECT_NAME}_test_helpers + PUBLIC + ${PROJECT_LIBRARY_TARGET_NAME} +) diff --git a/test/integration/TestHelper.hh b/test/helpers/GuiRelay.hh similarity index 57% rename from test/integration/TestHelper.hh rename to test/helpers/GuiRelay.hh index e86ed3cb0a..4ad347c6e7 100644 --- a/test/integration/TestHelper.hh +++ b/test/helpers/GuiRelay.hh @@ -14,44 +14,60 @@ * limitations under the License. * */ -#ifndef GZ_GUI_TESTHELPER_HH_ -#define GZ_GUI_TESTHELPER_HH_ +#ifndef GZ_GUI_GUIRELAY_HH_ +#define GZ_GUI_GUIRELAY_HH_ #include #include #include -#ifndef _WIN32 -# define TestHelper_EXPORTS_API -#else -# if (defined(TestHelper_EXPORTS)) -# define TestHelper_EXPORTS_API __declspec(dllexport) -# else -# define TestHelper_EXPORTS_API __declspec(dllimport) -# endif -#endif - namespace gz { namespace sim { -/// \brief -class TestHelper_EXPORTS_API TestHelper : public QObject +namespace test +{ +/// \brief Helper class to be used in internal tests. It allows receiving +/// events emitted in the GUI. +/// +/// ## Usage +/// +/// // Instantiate the class +/// test::GuiRelay guiRelay; +/// +/// // Register callback, for example: +/// guiRelay.OnQEvent([&](QEvent *_event) +/// { +/// if (_event.type() == gz::gui::events::Render::kType) +/// { +/// // Do something +/// } +/// } +/// +class GuiRelay : public QObject { Q_OBJECT /// \brief Constructor - public: inline TestHelper() + public: GuiRelay() { gz::gui::App()->findChild()-> installEventFilter(this); } /// \brief Destructor - public: ~TestHelper() = default; + public: ~GuiRelay() = default; + + /// \brief Wrapper around Qt's event filter + /// \param[in] _cb Function to be called on every received QEvent + public: GuiRelay &OnQEvent(std::function _cb) + { + this->forwardEvent = std::move(_cb); + return *this; + } /// \brief Documentation inherited - public: inline bool eventFilter(QObject *_obj, QEvent *_event) override + public: bool eventFilter(QObject *_obj, QEvent *_event) override { if (this->forwardEvent) this->forwardEvent(_event); @@ -64,5 +80,6 @@ class TestHelper_EXPORTS_API TestHelper : public QObject }; } } +} #endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index f11c03bf7b..1faaa95524 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -213,27 +213,8 @@ if(TARGET INTEGRATION_reset_sensors) endif() if(TARGET INTEGRATION_mouse_drag) - find_package(Qt5Test REQUIRED) - - set (qt_headers - TestHelper.hh - ) - - QT5_WRAP_CPP(headers_MOC ${qt_headers}) - - add_library(${PROJECT_NAME}_test_helpers SHARED - # TestHelper.cc - ${headers_MOC} - ) - target_compile_definitions(${PROJECT_NAME}_test_helpers PRIVATE TestHelper_EXPORTS) - target_link_libraries(${PROJECT_NAME}_test_helpers - PUBLIC - ${PROJECT_LIBRARY_TARGET_NAME} - ) - target_link_libraries(INTEGRATION_mouse_drag ${PROJECT_LIBRARY_TARGET_NAME}-gui - Qt5::Test ${PROJECT_NAME}_test_helpers ) endif() diff --git a/test/integration/mouse_drag.cc b/test/integration/mouse_drag.cc index ac41e275b0..f2eab9310f 100644 --- a/test/integration/mouse_drag.cc +++ b/test/integration/mouse_drag.cc @@ -44,7 +44,8 @@ #include "gz/sim/TestFixture.hh" #include "test_config.hh" #include "../helpers/EnvTestFixture.hh" -#include "TestHelper.hh" +// #include "TestHelper.hh" +#include "../helpers/GuiRelay.hh" #include "gz/sim/gui/Gui.hh" @@ -64,7 +65,7 @@ class MouseDragTestFixture : public InternalFixture<::testing::Test> { std::vector cmd = { common::joinPaths(getInstallPrefix(), "bin", "gz"), - "sim", "-s", "-r", "-v4", /* "--iterations=1000", */_sdfFile}; + "sim", "-s", "-r", "-v4", _sdfFile}; std::vector env = { "LD_LIBRARY_PATH=" + common::joinPaths(getInstallPrefix(), "lib")}; auto serverProc = utils::Subprocess(cmd, env); @@ -188,38 +189,39 @@ TEST_F(MouseDragTestFixture, rendering::ScenePtr scene; rendering::CameraPtr camera; std::optional event; - auto testHelper = std::make_unique(); - testHelper->forwardEvent = [&](QEvent *_event) - { - if (_event->type() == gz::gui::events::Render::kType) + test::GuiRelay guiRelay; + guiRelay.OnQEvent([&](QEvent *_event) { - gzdbg << "Render" << std::endl; - // Get scene and user camera - if (!scene) + if (_event->type() == gz::gui::events::Render::kType) { - scene = rendering::sceneFromFirstRenderEngine(); - EXPECT_NE(nullptr, scene); - for (unsigned int i = 0; i < scene->NodeCount(); ++i) + gzdbg << "Render" << std::endl; + // Get scene and user camera + if (!scene) { - auto cam = std::dynamic_pointer_cast( - scene->NodeByIndex(i)); - if (cam && cam->HasUserData("user-camera") && - std::get(cam->UserData("user-camera"))) + scene = rendering::sceneFromFirstRenderEngine(); + EXPECT_NE(nullptr, scene); + for (unsigned int i = 0; i < scene->NodeCount(); ++i) { - camera = cam; - break; + auto cam = std::dynamic_pointer_cast( + scene->NodeByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + camera = cam; + break; + } } + EXPECT_NE(nullptr, camera); + } + // Send events + if (event) + { + // TODO(anyone): use QTest mouse events instead of gz events? + app->sendEvent(app->findChild(), *event); + event.reset(); } - EXPECT_NE(nullptr, camera); - } - // Send events - if (event) - { - app->sendEvent(app->findChild(), *event); - event.reset(); } - } - }; + }); // Wait for scene unsigned int sleep = 0; diff --git a/test/worlds/mouse_drag.sdf b/test/worlds/mouse_drag.sdf index f0ea301bcb..a1e4b5f2d6 100644 --- a/test/worlds/mouse_drag.sdf +++ b/test/worlds/mouse_drag.sdf @@ -135,4 +135,4 @@ - \ No newline at end of file +