From a0f24ce5d4b2c162066dd703f48c453fe28ae078 Mon Sep 17 00:00:00 2001
From: Fabian Paus <fabian.paus@kit.edu>
Date: Mon, 10 Jan 2022 14:51:38 +0100
Subject: [PATCH] ArViz: Allow translation only along specified axes (first
working version)
---
.../RobotAPI/components/ArViz/Client/Client.h | 34 +---
.../components/ArViz/Coin/Visualizer.cpp | 177 ++++++++++++------
.../components/ArViz/Coin/Visualizer.h | 3 +-
.../components/ArViz/Example/ArVizExample.cpp | 30 ++-
source/RobotAPI/interface/ArViz/Elements.ice | 21 +--
5 files changed, 153 insertions(+), 112 deletions(-)
diff --git a/source/RobotAPI/components/ArViz/Client/Client.h b/source/RobotAPI/components/ArViz/Client/Client.h
index 5500e7747..12fc06825 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.h
+++ b/source/RobotAPI/components/ArViz/Client/Client.h
@@ -146,31 +146,6 @@ namespace viz
}
}
- bool isTranslation() const
- {
- return data_.type & data::InteractionFeedbackType::TRANSLATION_FLAG;
- }
-
- bool isRotation() const
- {
- return data_.type & data::InteractionFeedbackType::ROTATION_FLAG;
- }
-
- bool isAxisX() const
- {
- return data_.type & data::InteractionFeedbackType::AXIS_X_FLAG;
- }
-
- bool isAxisY() const
- {
- return data_.type & data::InteractionFeedbackType::AXIS_Y_FLAG;
- }
-
- bool isAxisZ() const
- {
- return data_.type & data::InteractionFeedbackType::AXIS_Z_FLAG;
- }
-
bool isTransformBegin() const
{
return data_.type & data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG;
@@ -206,14 +181,9 @@ namespace viz
return data_.chosenContextMenuEntry;
}
- Eigen::Matrix4f originalPose() const
- {
- return toEigen(data_.originalPose);
- }
-
- Eigen::Matrix4f chosenPose() const
+ Eigen::Matrix4f transformation() const
{
- return toEigen(data_.chosenPose);
+ return toEigen(data_.transformation);
}
data::InteractionFeedback data_;
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index a2fdeadfa..3af96a367 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -4,11 +4,12 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/GetTypeString.h>
-#include <Inventor/nodes/SoMaterial.h>
-#include <Inventor/nodes/SoUnits.h>
#include <Inventor/SoPath.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
namespace armarx::viz
{
namespace coin
@@ -53,19 +54,19 @@ namespace coin
static void startManipulationCallback(void* data, SoDragger* dragger)
{
CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
- this_->onManipulation(data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG);
+ this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG);
}
static void duringManipulationCallback(void* data, SoDragger* dragger)
{
CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
- this_->onManipulation(data::InteractionFeedbackType::TRANSFORM_DURING_FLAG);
+ this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_DURING_FLAG);
}
static void finishManipulationCallback(void* data, SoDragger* dragger)
{
CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
- this_->onManipulation(data::InteractionFeedbackType::TRANSFORM_END_FLAG);
+ this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_END_FLAG);
}
static const char* toString(CoinVisualizerState state)
@@ -336,6 +337,7 @@ namespace coin
foundInteraction->element = elementID;
foundInteraction->interaction = interactionDesc;
+ foundInteraction->visu = visu;
// Add user data to Coin node
visu->separator->setUserData(foundInteraction);
@@ -575,27 +577,10 @@ namespace coin
return;
}
- ElementInteractionData const* id = elementInteractions[index].get();
-
- CoinLayer* layer = layers.findLayer(id->layer);
- if (layer == nullptr)
- {
- ARMARX_WARNING << "Selected an element whose layer does not exist: \n"
- << "Layer: " << id->layer.first << "/" << id->layer.second
- << ", element: " << id->element;
- return;
- }
- CoinLayerElement* element = layer->findElement(id->element);
- if (element == nullptr)
- {
- ARMARX_WARNING << "Selected an element which does not exist: \n"
- << "Layer: " << id->layer.first << "/" << id->layer.second
- << ", element: " << id->element;
- return;
- }
+ ElementInteractionData* id = elementInteractions[index].get();
selection->deselectAll();
- selection->select(element->visu->separator);
+ selection->select(id->visu->separator);
}
static ElementInteractionData* findInteractionDataOnPath(SoPath* path)
@@ -654,24 +639,6 @@ namespace coin
// Does the element support transform interactions?
if (enableFlags & ANY_TRANSFORM)
{
- // TODO: Look up the element and add it to the manipulator group
- CoinLayer* layer = layers.findLayer(id->layer);
- if (layer == nullptr)
- {
- ARMARX_WARNING << "Selected an element whose layer does not exist: \n"
- << "Layer: " << id->layer.first << "/" << id->layer.second
- << ", element: " << id->element;
- return;
- }
- CoinLayerElement* element = layer->findElement(id->element);
- if (element == nullptr)
- {
- ARMARX_WARNING << "Selected an element which does not exist: \n"
- << "Layer: " << id->layer.first << "/" << id->layer.second
- << ", element: " << id->element;
- return;
- }
-
manipulatorGroup->removeAllChildren();
// We need to create the manipulator everytime to achieve the desired effect
// If we reuse an instance, the manipulator gets stuck to old objects...
@@ -684,7 +651,7 @@ namespace coin
manipulatorGroup->addChild(manipulator);
// We add the same visualization node again
- manipulatorGroup->addChild(element->visu->separator);
+ manipulatorGroup->addChild(id->visu->separator);
manipulator->unsquishKnobs();
}
@@ -703,7 +670,29 @@ namespace coin
feedback.revision = pulledUpdates.revision;
}
- void CoinVisualizer::onManipulation(int eventType)
+ Eigen::Matrix4f toEigen(SbMat const& mat)
+ {
+ Eigen::Matrix4f result;
+ for (int y = 0; y < 4; ++y)
+ {
+ for (int x = 0; x < 4; ++x)
+ {
+ result(x, y) = mat[y][x];
+ }
+ }
+
+ return result;
+ }
+
+ Eigen::Affine3f toEigen(SbRotation const& rot, SbVec3f const& transl)
+ {
+ Eigen::Affine3f result;
+ result.linear() = Eigen::Quaternion(rot[3], rot[0], rot[1], rot[2]).toRotationMatrix();
+ result.translation() = Eigen::Vector3f(transl[0], transl[1], transl[2]);
+ return result;
+ }
+
+ void CoinVisualizer::onManipulation(SoDragger* dragger, int eventType)
{
if (state != CoinVisualizerState::RUNNING)
{
@@ -715,30 +704,94 @@ namespace coin
return;
}
- std::string elementID = selectedElement->layer.first + "/"
- + selectedElement->layer.second + "/"
- + selectedElement->element;
-
- SbVec3f translation = manipulator->translation.getValue();
- SbRotation rotation = manipulator->rotation.getValue();
- if (eventType == data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG)
+ // If there is an entry in the feedback buffer already, update it
+ // This way, we prevent multiple events being sent to the client
+ viz::data::InteractionFeedback* newFeedback = nullptr;
+ for (viz::data::InteractionFeedback& feedback : interactionFeedbackBuffer)
{
- ARMARX_INFO << elementID << ": Begin manipulation \n"
- << "Translation: " << translation.toString().getString()
- << "\nRotation: " << rotation.toString().getString();
+ if ((feedback.type & 0x7) == data::InteractionFeedbackType::TRANSFORM
+ && feedback.component == selectedElement->layer.first
+ && feedback.layer == selectedElement->layer.second
+ && feedback.element == selectedElement->element)
+ {
+ // This is a transform interaction concerning the same element
+ newFeedback = &feedback;
+ }
}
- else if (eventType == data::InteractionFeedbackType::TRANSFORM_DURING_FLAG)
+ if (newFeedback == nullptr)
{
- ARMARX_INFO << elementID << ": During manipulation \n"
- << "Translation: " << translation.toString().getString()
- << "\nRotation: " << rotation.toString().getString();
+ // Create a new interaction
+ newFeedback = &interactionFeedbackBuffer.emplace_back();
+ newFeedback->component = selectedElement->layer.first;
+ newFeedback->layer = selectedElement->layer.second;
+ newFeedback->element = selectedElement->element;
}
- else if (eventType == data::InteractionFeedbackType::TRANSFORM_END_FLAG)
+
+ // TODO: Add more flags about translation/rotation, axes
+ // Can we get this information from the dragger?
+ newFeedback->type = data::InteractionFeedbackType::TRANSFORM | eventType;
+ newFeedback->revision = pulledUpdates.revision;
+
+
+ SbVec3f t_m = manipulator->translation.getValue();
+ SbRotation r_m = manipulator->rotation.getValue();
+ SbVec3f s_m = manipulator->scaleFactor.getValue();
+
+ // TODO: Compute the same math but using Coin math types, so we can get rid of the Eigen/ include!
+
+ Eigen::Affine3f T_G_X = toEigen(r_m, t_m);
+ Eigen::Affine3f T_G_O;
+
+ // Lookup the element
+ // Now, we can get the transformation of the element
+ SbVec3f t_O = selectedElement->visu->transform->translation.getValue();
+ SbRotation R_O = selectedElement->visu->transform->rotation.getValue();
+
+ T_G_O = toEigen(R_O, t_O);
+
+ // Let us calculate perceived translation and see whether it meets our expectation
+ Eigen::Matrix3f R_G_X = T_G_X.linear();
+ Eigen::Vector3f t_G_O = 0.001f * T_G_O.translation();
+ Eigen::Vector3f t_G_X = T_G_X.translation();
+
+ Eigen::DiagonalMatrix<float, 3> scaleMatrix(s_m[0], s_m[1], s_m[2]);
+
+ // This value stays constant during a pure rotation!
+ // It is zero when the manipulation first starts
+ // So we can reduce the perceived motion here (hopefully)
+ Eigen::Vector3f deltaT = (R_G_X * scaleMatrix - Eigen::Matrix3f::Identity()) * t_G_O + t_G_X;
+ ARMARX_INFO << "deltaT: " << deltaT.transpose();
+
+ int enableFlags = selectedElement->interaction.enableFlags;
{
- ARMARX_INFO << elementID << ": End manipulation \n"
- << "Translation: " << translation.toString().getString()
- << "\nRotation: " << rotation.toString().getString();
+ // Prevent translation along disabled axes
+ if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_X) == 0)
+ {
+ deltaT.x() = 0.0f;
+ }
+ if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_Y) == 0)
+ {
+ deltaT.y() = 0.0f;
+ }
+ if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_Z) == 0)
+ {
+ deltaT.z() = 0.0f;
+ }
+
+ Eigen::Vector3f t_G_X_projected = deltaT - (R_G_X * scaleMatrix - Eigen::Matrix3f::Identity()) * t_G_O;
+ manipulator->translation.setValue(t_G_X_projected.x(), t_G_X_projected.y(), t_G_X_projected.z());
+ //manipulator->rotation.setValue(newRotation.x(), newRotation.y(), newRotation.z(), newRotation.w());
}
+ // TODO: Prevent rotation along disabled axes
+
+ data::GlobalPose& transformation = newFeedback->transformation;
+ transformation.x = t_m[0];
+ transformation.y = t_m[1];
+ transformation.z = t_m[2];
+ transformation.qw = r_m[3];
+ transformation.qx = r_m[0];
+ transformation.qy = r_m[1];
+ transformation.qz = r_m[2];
}
void CoinVisualizer::exportToVRML(const std::string& exportFilePath)
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index 37404e6ca..573e86708 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -192,6 +192,7 @@ namespace armarx::viz
CoinLayerID layer;
std::string element;
viz::data::InteractionDescription interaction;
+ coin::ElementVisualization* visu = nullptr;
};
class CoinVisualizer
@@ -242,7 +243,7 @@ namespace armarx::viz
void selectElement(int index);
void onSelectEvent(SoPath* path, int eventType);
- void onManipulation(int eventType);
+ void onManipulation(SoDragger* dragger, int eventType);
// These are selectable element IDs and need to be persistent in memory.
// We store a raw pointer to these into the SoSeperator objects of Coin.
// Later, we can retrieve the element ID from this pointer, if it has been selected.
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index 7ac979f03..fd84d32cb 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -512,7 +512,8 @@ namespace armarx
box.enable(viz::interaction()
.selection()
.contextMenu({"First Option", "Second Option", "Third Option"})
- .fullTransform());
+ .rotation()
+ .translation(true, false, false));
layer.add(box);
@@ -526,7 +527,8 @@ namespace armarx
cyl.enable(viz::interaction()
.selection()
.contextMenu({"Cyl Option 1", "Cyl Option 2"})
- .fullTransform());
+ .rotation()
+ .translation(false, true, false));
layer.add(cyl);
}
@@ -634,6 +636,30 @@ namespace armarx
<< "] Chosen context menu: "
<< interaction.chosenContextMenuEntry();
}
+ else if (interaction.type() == viz::InteractionFeedbackType::Transform)
+ {
+ std::string transformState;
+ if (interaction.isTransformBegin())
+ {
+ transformState = "Begin";
+ }
+ else if (interaction.isTransformDuring())
+ {
+ transformState = "During";
+ }
+ else if (interaction.isTransformEnd())
+ {
+ transformState = "End";
+ }
+ else
+ {
+ transformState = "<Unknwon>";
+ }
+ ARMARX_INFO << "[" << interaction.layer()
+ << "/" << interaction.element()
+ << "] Transformation " << transformState
+ << ": \n" << interaction.transformation();
+ }
else
{
ARMARX_INFO << "[" << interaction.layer()
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index ca27a0e10..7558d379c 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -69,19 +69,10 @@ module data
const int TRANSFORM = 4;
- // Flag to indicate the kind of transformation
- const int TRANSLATION_FLAG = 16;
- const int ROTATION_FLAG = 32;
-
- // Flag to indicate the axis used for transformation
- const int AXIS_X_FLAG = 64;
- const int AXIS_Y_FLAG = 128;
- const int AXIS_Z_FLAG = 256;
-
// Flag to indicate state of the transformation
- const int TRANSFORM_BEGIN_FLAG = 512;
- const int TRANSFORM_DURING_FLAG = 1024;
- const int TRANSFORM_END_FLAG = 2048;
+ const int TRANSFORM_BEGIN_FLAG = 16;
+ const int TRANSFORM_DURING_FLAG = 32;
+ const int TRANSFORM_END_FLAG = 64;
};
struct InteractionFeedback
@@ -100,9 +91,9 @@ module data
// Chosen context menu entry is only relevant for type == CONTEXT_MENU_CHOSEN
int chosenContextMenuEntry = 0;
- // Original and chosen poase are only relevant for type == TRANSFORM
- GlobalPose originalPose;
- GlobalPose chosenPose;
+ // Applied transformation is only relevant for type == TRANSFORM
+ GlobalPose transformation;
+ Vector3f scale;
};
sequence<InteractionFeedback> InteractionFeedbackSeq;
--
GitLab