diff --git a/VirtualRobot/CollisionDetection/CollisionModel.cpp b/VirtualRobot/CollisionDetection/CollisionModel.cpp
index aff0975520eb74c9e79602efa0ffd0f3d9d4bfa0..d1b7ae4e1b50346ff1a79161568f555cf7d78fd3 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 7547e65075c125b0e1e47ee851453c9b9590d981..b1b6e77fb3d4d8ed477f908c85e5080a5bbcbfcd 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 23efb76b65d999b29a5dc636cd4932cc83ac736b..27dce262f2444dfcfe3a73c526d0c6ddc929bee8 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 4bafec64fca8fb605851f9e78e9fe962dd2ac5f8..6be93aaacdd36f78796a480df531d18416f24a91 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 2107a7b3d1de25b922ca77ce3296f05e1ef3b467..a0ecdac424969910a4b2ef8b7318be28314a23e2 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 c512b5b35cab0470886689b7dba423bd63e542d3..3e62d88805693bc740d5ffb565c1d0b8c94c914b 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 5c8f40d91d39e746aa57c1dc7f64e2113e90e554..9b528ad5b60f987bc3d0c85375e1a97784867159 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