From 7b4a83fd830672b0971cd6304d819aac4356eb75 Mon Sep 17 00:00:00 2001
From: jean_patrick_mathes <uomnk@student.kit.edu>
Date: Fri, 23 Jun 2023 11:15:33 +0200
Subject: [PATCH] Fixes that there are no lines in the RobotViewer where the
new joints are used by checking the distance instead of relying on the local
transformation of the nodes
---
VirtualRobot/Nodes/RobotNode.cpp | 24 +++++++++++++++++++++---
VirtualRobot/SceneObject.cpp | 1 -
2 files changed, 21 insertions(+), 4 deletions(-)
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index 077b2d7ab..34e6fe2e1 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -826,7 +826,6 @@ namespace VirtualRobot
{
VisualizationNodePtr visualizationNode1;
-
if (parRN && parRN->getVisualization())
{
// add to parent node (pre joint trafo moves with parent!)
@@ -837,7 +836,7 @@ namespace VirtualRobot
if (visualizationNode1)
{
- parRN->getVisualization()->attachVisualization(attachName1, visualizationNode1);
+ //parRN->getVisualization()->attachVisualization(attachName1, visualizationNode1);
}
}
else
@@ -848,10 +847,29 @@ namespace VirtualRobot
if (visualizationNode1)
{
- visualizationModel->attachVisualization(attachName1, visualizationNode1);
+ //visualizationModel->attachVisualization(attachName1, visualizationNode1);
}
}
}
+ else if (Eigen::Vector3f parPos = parRN
+ ? parRN->getPositionInFrame(getRobot()->getRobotNode(this->getName()))
+ : Eigen::Vector3f::Zero()
+ ; parPos.norm() > 0.01)
+ {
+ // Some nodes (e.g. the Revolute and FourBar) offset their position by values that are not reflected in the local transformation.
+ // Check RobotNodeFourBar::updateTransformationMatrices and RobotNodeRevolute::updateTransformationMatrices.
+ // There the global pose is set to parent * local * tmp, where tmp is important but not part of
+ // This code catches these cases by checking the actual distance between the current node and its parent.
+
+ VisualizationNodePtr visualizationNode1 = visualizationFactory->createLine(Eigen::Vector3f::Zero(), parPos);
+
+ if (visualizationNode1)
+ {
+ visualizationModel->attachVisualization(attachName1, visualizationNode1);
+ }
+
+ std::cout << "VERY IMPORTANT MARKER FROM " << getName() << std::endl;
+ }
VisualizationNodePtr visualizationNode2 = visualizationFactory->createSphere(5.0f);
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index 969a50897..a4ba5970c 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -216,7 +216,6 @@ namespace VirtualRobot
return true;
}
-
VisualizationFactoryPtr visualizationFactory;
if (visualizationType == "")
--
GitLab