From 8b5e05dfe7af1cb11c9fde4cedc1ee27dd13b93b Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 1 Feb 2023 20:14:13 +0100
Subject: [PATCH] fix: collision nodes: previously ignored local pose; fixed
also in visualization
---
.../CollisionDetection/CollisionModel.cpp | 2 +-
.../CollisionDetection/CollisionModel.h | 11 +++++++++-
VirtualRobot/Import/URDF/SimoxURDFFactory.cpp | 12 ++++++-----
VirtualRobot/Import/URDF/SimoxURDFFactory.h | 3 +--
VirtualRobot/Nodes/RobotNode.cpp | 2 +-
VirtualRobot/SceneObject.cpp | 6 +++---
.../CoinVisualizationFactory.cpp | 21 ++++++++++++++++++-
7 files changed, 43 insertions(+), 14 deletions(-)
diff --git a/VirtualRobot/CollisionDetection/CollisionModel.cpp b/VirtualRobot/CollisionDetection/CollisionModel.cpp
index aff097552..d1b7ae4e1 100644
--- a/VirtualRobot/CollisionDetection/CollisionModel.cpp
+++ b/VirtualRobot/CollisionDetection/CollisionModel.cpp
@@ -20,6 +20,7 @@ namespace VirtualRobot
this->name = name;
this->margin = margin;
this->colChecker = colChecker;
+ this->localPose = visu->getLocalPose();
if (!this->colChecker)
{
@@ -450,4 +451,3 @@ namespace VirtualRobot
} // namespace VirtualRobot
-
diff --git a/VirtualRobot/CollisionDetection/CollisionModel.h b/VirtualRobot/CollisionDetection/CollisionModel.h
index 7547e6507..b1b6e77fb 100644
--- a/VirtualRobot/CollisionDetection/CollisionModel.h
+++ b/VirtualRobot/CollisionDetection/CollisionModel.h
@@ -94,6 +94,15 @@ namespace VirtualRobot
return getBoundingBox();
}
+ void setGlobalParentPose(const Eigen::Matrix4f& globalParentPose)
+ {
+ setGlobalPose(globalParentPose * localPose);
+ }
+
+ void setLocalPose(const Eigen::Matrix4f& localPose)
+ {
+ this->localPose = localPose;
+ }
/*!
The global pose defines the position of the joint in the world. This value is used for visualization.
@@ -224,6 +233,7 @@ namespace VirtualRobot
Eigen::Matrix4f globalPose; //< The transformation that is used for visualization and for updating the col model
+ Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
#if defined(VR_COLLISION_DETECTION_PQP)
std::shared_ptr< CollisionModelPQP > collisionModelImplementation;
@@ -233,4 +243,3 @@ namespace VirtualRobot
};
} // namespace VirtualRobot
-
diff --git a/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp b/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp
index 23efb76b6..27dce262f 100644
--- a/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp
+++ b/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp
@@ -4,6 +4,8 @@
#include <iostream>
#include <map>
+#include <Eigen/Geometry>
+
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
@@ -144,7 +146,7 @@ namespace VirtualRobot
return robo;
}
- Eigen::Matrix4f SimoxURDFFactory::convertPose(urdf::Pose& p)
+ Eigen::Matrix4f SimoxURDFFactory::convertPose(const urdf::Pose& p)
{
const float scale = 1000.0f; // mm
Eigen::Matrix4f res;
@@ -272,20 +274,20 @@ namespace VirtualRobot
return res;
}
- VisualizationNodePtr SimoxURDFFactory::convertVisuArray(std::vector<std::shared_ptr<urdf::Collision> > visu_array, const string& basePath)
+ VisualizationNodePtr SimoxURDFFactory::convertVisuArray(std::vector<std::shared_ptr<urdf::Collision> > col_array, const string& basePath)
{
VirtualRobot::VisualizationNodePtr res;
std::shared_ptr<VisualizationFactory> factory = CoinVisualizationFactory::createInstance(NULL);
- if (visu_array.size() == 0)
+ if (col_array.empty())
{
return res;
}
std::vector< VisualizationNodePtr > visus;
- for (size_t i = 0; i < visu_array.size(); i++)
+ for (auto & visu : col_array)
{
- VirtualRobot::VisualizationNodePtr v = convertVisu(visu_array[i]->geometry, visu_array[i]->origin, basePath);
+ VirtualRobot::VisualizationNodePtr v = convertVisu(visu->geometry, visu->origin, basePath);
if (v)
{
visus.push_back(v);
diff --git a/VirtualRobot/Import/URDF/SimoxURDFFactory.h b/VirtualRobot/Import/URDF/SimoxURDFFactory.h
index 4bafec64f..6be93aaac 100644
--- a/VirtualRobot/Import/URDF/SimoxURDFFactory.h
+++ b/VirtualRobot/Import/URDF/SimoxURDFFactory.h
@@ -77,7 +77,7 @@ namespace VirtualRobot
protected:
RobotNodePtr createBodyNode(const std::string& name, RobotPtr robot, std::shared_ptr<urdf::Link> urdfBody, const std::string& basePath, bool useColModelsIfNoVisuModel = true);
RobotNodePtr createJointNode(RobotPtr robot, std::shared_ptr<urdf::Joint> urdfJoint);
- Eigen::Matrix4f convertPose(urdf::Pose& p);
+ Eigen::Matrix4f convertPose(const urdf::Pose& p);
VirtualRobot::VisualizationNodePtr convertVisu(std::shared_ptr<urdf::Geometry> g, urdf::Pose& pose, const std::string& basePath);
VirtualRobot::VisualizationNodePtr convertVisuArray(std::vector<std::shared_ptr<urdf::Visual> > visu_array, const std::string& basePath);
VirtualRobot::VisualizationNodePtr convertVisuArray(std::vector<std::shared_ptr<urdf::Collision> > visu_array, const std::string& basePath);
@@ -87,4 +87,3 @@ namespace VirtualRobot
};
} // namespace VirtualRobot
-
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index 2107a7b3d..a0ecdac42 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -319,7 +319,7 @@ namespace VirtualRobot
}
if (collisionModel && updateCollisionModel)
{
- collisionModel->setGlobalPose(globalPose);
+ collisionModel->setGlobalParentPose(globalPose);
}
}
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index c512b5b35..3e62d8880 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -133,7 +133,7 @@ namespace VirtualRobot
if (collisionModel && updateCollisionModel)
{
- collisionModel->setGlobalPose(globalPose);
+ collisionModel->setGlobalParentPose(globalPose);
}
if (updateChildren)
@@ -157,7 +157,7 @@ namespace VirtualRobot
}
if (collisionModel && updateCollisionModel)
{
- collisionModel->setGlobalPose(globalPose);
+ collisionModel->setGlobalParentPose(globalPose);
}
}
@@ -545,7 +545,7 @@ namespace VirtualRobot
if (collisionModel)
{
collisionModel->setUpdateVisualization(updateCollisionModel);
- collisionModel->setGlobalPose(globalPose);
+ collisionModel->setGlobalParentPose(globalPose);
}
}
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index 5c8f40d91..9b528ad5b 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -26,9 +26,11 @@
#include "../../Workspace/WorkspaceGrid.h"
#include "../../XML/BaseIO.h"
#include "../../Import/MeshImport/AssimpReader.h"
+#include "SimoxUtility/math/convert/mat3f_to_quat.h"
#include <Inventor/SoDB.h>
#include <Inventor/nodes/SoFile.h>
#include <Inventor/nodes/SoNode.h>
+#include <Inventor/nodes/SoTransformation.h>
#include <Inventor/nodes/SoUnits.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoCube.h>
@@ -4087,7 +4089,24 @@ namespace VirtualRobot
if (n)
{
- s->addChild(n->copy(FALSE));
+ SoSeparator* ss = new SoSeparator();
+
+ SoUnits* u = new SoUnits();
+ u->units = SoUnits::MILLIMETERS;
+
+ const Eigen::Isometry3f localPose(visualizations[i]->getLocalPose());
+ const Eigen::Vector3f& translation = localPose.translation();
+
+ SoTransform* t = new SoTransform;
+ t->translation.setValue(translation.x(), translation.y(), translation.z());
+ const Eigen::Quaternionf q = simox::math::mat3f_to_quat(localPose.linear());
+ t->rotation.setValue(q.x(), q.y(), q.z(), q.w());
+
+ ss->addChild(u);
+ ss->addChild(t);
+ ss->addChild(n->copy(FALSE));
+
+ s->addChild(ss);
}
}
else
--
GitLab