diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index ae727a7fdca48eaa8501e2070679b9f73a3e9ce2..575095e4bb22a4fda58f37524e1cab73583f37b9 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -511,10 +511,19 @@ void RobotNode::showStructure( bool enable, const std::string &visualizationType
if (!ensureVisualization(visualizationType))
return;
- std::string attachName1("RobotNodeStructurePre");
+ std::stringstream ss;
+ ss << getName() << "_RobotNodeStructurePre";
+ std::string attachName1 = ss.str();
std::string attachName2("RobotNodeStructureJoint");
std::string attachName3("RobotNodeStructurePost");
- visualizationModel->detachVisualization(attachName1);
+ SceneObjectPtr par = getParent();
+ RobotNodePtr parRN = dynamic_pointer_cast<RobotNode>(par);
+
+ // need to add "pre" visualization to parent node!
+ if (parRN && parRN->getVisualization())
+ parRN->getVisualization()->detachVisualization(attachName1);
+ else
+ visualizationModel->detachVisualization(attachName1);
visualizationModel->detachVisualization(attachName2);
visualizationModel->detachVisualization(attachName3);
if (enable)
@@ -535,9 +544,19 @@ void RobotNode::showStructure( bool enable, const std::string &visualizationType
if (!preJointTransformation.isIdentity())
{
- VisualizationNodePtr visualizationNode1 = visualizationFactory->createLine(preJointTransformation.inverse(),i);
- if (visualizationNode1)
- visualizationModel->attachVisualization(attachName1,visualizationNode1);
+ VisualizationNodePtr visualizationNode1;
+ if (parRN && parRN->getVisualization())
+ {
+ // add to parent node (pre joint trafo moves with parent!)
+ visualizationNode1 = visualizationFactory->createLine(parRN->postJointTransformation, parRN->postJointTransformation*preJointTransformation);
+ if (visualizationNode1)
+ parRN->getVisualization()->attachVisualization(attachName1,visualizationNode1);
+ } else
+ {
+ visualizationNode1 = visualizationFactory->createLine(preJointTransformation.inverse(),i);
+ if (visualizationNode1)
+ visualizationModel->attachVisualization(attachName1,visualizationNode1);
+ }
}
VisualizationNodePtr visualizationNode2 = visualizationFactory->createSphere(5.0f);
if (visualizationNode2)
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index 821047795ec4f7a90b82edb85de2c8e3c531a3b1..ee76471a775e8ce6928535c038bb8a63718c3455 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -224,14 +224,10 @@ void SceneObject::showPhysicsInformation( bool enableCoM, bool enableInertial, V
{
visualizationModel->detachVisualization("__InertiaMatrix");
}
- if (enableCoM)
+ VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(NULL);
+
+ if (enableCoM && visualizationFactory)
{
- VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(NULL);
- if (!visualizationFactory)
- {
- VR_ERROR << " Could not determine a valid VisualizationFactory!!" << endl;
- return;
- }
// create visu
if (!comModel)
{
@@ -242,6 +238,18 @@ void SceneObject::showPhysicsInformation( bool enableCoM, bool enableInertial, V
v.push_back(comModel2);
comModel = visualizationFactory->createUnitedVisualization(v);
}
+
+ if (comModel)
+ {
+ std::stringstream ss;
+ ss << "COM:" << getName();
+ std::string t = ss.str();
+ VisualizationNodePtr vText = visualizationFactory->createText(t,true,1.0f,VisualizationFactory::Color::Blue(),0,10.0f,0);
+ std::vector<VisualizationNodePtr> v;
+ v.push_back(comModel);
+ v.push_back(vText);
+ comModel = visualizationFactory->createUnitedVisualization(v);
+ }
VisualizationNodePtr comModelClone = comModel->clone();
Eigen::Vector3f l = getCoMLocal() * 0.001f;
cout << "COM:" << l << endl;
@@ -251,7 +259,7 @@ void SceneObject::showPhysicsInformation( bool enableCoM, bool enableInertial, V
visualizationModel->attachVisualization("__CoMLocation",comModelClone);
}
- /*if (enableInertial)
+ if (enableInertial && visualizationFactory)
{
VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(NULL);
if (!visualizationFactory)
@@ -260,41 +268,43 @@ void SceneObject::showPhysicsInformation( bool enableCoM, bool enableInertial, V
return;
}
// create inertia visu
-
+ cout << "INERTIA MATRIX:" << endl << physics.intertiaMatrix << endl;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(physics.intertiaMatrix);
if (eigensolver.info() == Eigen::Success)
{
- cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
+ /*cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
cout << "Here's a matrix whose columns are eigenvectors of A \n"
<< "corresponding to these eigenvalues:\n"
- << eigensolver.eigenvectors() << endl;
- Eigen::Vector3f v1 = eigensolver.eigenvectors().block(0,0,1,3);
- Eigen::Vector3f v2 = eigensolver.eigenvectors().block(0,1,1,3);
- Eigen::Vector3f v3 = eigensolver.eigenvectors().block(0,2,1,3);
+ << eigensolver.eigenvectors() << endl;*/
+ Eigen::Vector3f v1 = eigensolver.eigenvectors().block(0,0,3,1);
+ Eigen::Vector3f v2 = eigensolver.eigenvectors().block(0,1,3,1);
+ Eigen::Vector3f v3 = eigensolver.eigenvectors().block(0,2,3,1);
+ /*cout << "v1:" << v1 << endl;
+ cout << "v2:" << v2 << endl;
+ cout << "v3:" << v3 << endl;*/
+
+ float xl = (float)(eigensolver.eigenvalues().rows()>0?eigensolver.eigenvalues()(0):1e-6);
+ float yl = (float)(eigensolver.eigenvalues().rows()>1?eigensolver.eigenvalues()(1):1e-6);
+ float zl = (float)(eigensolver.eigenvalues().rows()>2?eigensolver.eigenvalues()(2):1e-6);
+ float le = sqrtf(xl*xl + yl*yl + zl*zl);
+ float maxSize = le>1e-8?50.0f/le:5000.0f;
+
+ VisualizationNodePtr inertiaVisu = visualizationFactory->createEllipse(xl*maxSize,yl*maxSize,zl*maxSize,true);
+
+ Eigen::Vector3f l = getCoMLocal() * 0.001f;
+ //cout << "COM:" << l << endl;
+ Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
+ m.block(0,3,3,1) = l;
+ m.block(0,0,3,3) = eigensolver.eigenvectors().block(0,0,3,3); // rotate according to EV
+ visualizationFactory->applyDisplacement(inertiaVisu, m);
+
+ visualizationModel->attachVisualization("__InertiaMatrix",inertiaVisu);
+
} else
{
VR_INFO << " Could not determine eigenvectors of inertia matrix" << endl;
}
-
- VisualizationNodePtr inertiaVisu = visualizationFactory->CreateEllipse
- if (!comModel)
- {
- VisualizationNodePtr comModel1 = visualizationFactory->createSphere(7.05f,1.0f,0.2f,0.2f);
- VisualizationNodePtr comModel2 = visualizationFactory->createBox(10.0f,10.0f,10.0f,0.2f,0.2f,1.0f);
- std::vector<VisualizationNodePtr> v;
- v.push_back(comModel1);
- v.push_back(comModel2);
- comModel = visualizationFactory->createUnitedVisualization(v);
- }
- VisualizationNodePtr comModelClone = comModel->clone();
- Eigen::Vector3f l = getCoMLocal() * 0.001f;
- cout << "COM:" << l << endl;
- Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
- m.block(0,3,3,1) = l;
- visualizationFactory->applyDisplacement(comModelClone, m);
-
- visualizationModel->attachVisualization("__InertiaMatrix",inertiaVisu);
- }*/
+ }
}
bool SceneObject::showCoordinateSystemState()
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index 9e1cef675eca936bd0b1215a9e715bbe6bcafa4c..e324467da0cf32b17cd0ad83c5c711480f01f161 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -499,39 +499,89 @@ namespace VirtualRobot {
return result;
}
- SoSeparator* CoinVisualizationFactory::CreateText(const std::string &s)
+ VisualizationNodePtr CoinVisualizationFactory::createText(const std::string &text, bool billboard, float scaling, Color c, float offsetX, float offsetY, float offsetZ)
+ {
+ SoSeparator* res = new SoSeparator();
+ res->ref();
+ SoUnits *u = new SoUnits();
+ u->units = SoUnits::MILLIMETERS;
+ res->addChild(u);
+ SoScale *sc = new SoScale();
+ sc->scaleFactor.setValue(scaling,scaling,scaling);
+ res->addChild(sc);
+ SoMaterial *m = new SoMaterial();
+ res->addChild(m);
+ m->diffuseColor.setValue(c.r,c.g,c.b);
+ m->transparency.setValue(c.transparency);
+
+ if (billboard)
+ res->addChild(CreateBillboardText(text,offsetX*1000.0f,offsetY*1000.0f,offsetZ*1000.0f));
+ else
+ res->addChild(CreateText(text,offsetX*1000.0f,offsetY*1000.0f,offsetZ*1000.0f));
+
+ VisualizationNodePtr visualizationNode(new CoinVisualizationNode(res));
+ res->unref();
+ return visualizationNode;
+ }
+
+
+ VisualizationNodePtr CoinVisualizationFactory::createEllipse(float x, float y, float z, bool showAxes, float axesHeight, float axesWidth)
+ {
+ SoSeparator* res = new SoSeparator();
+ res->ref();
+ SoUnits *u = new SoUnits();
+ u->units = SoUnits::MILLIMETERS;
+ res->addChild(u);
+ res->addChild(CreateEllipse(x,y,z,NULL,showAxes,axesHeight,axesWidth));
+ VisualizationNodePtr visualizationNode(new CoinVisualizationNode(res));
+ res->unref();
+ return visualizationNode;
+ }
+
+ SoSeparator* CoinVisualizationFactory::CreateText(const std::string &s, float offsetX, float offsetY, float offsetZ)
{
SoSeparator *textSep = new SoSeparator();
+ textSep->ref();
+
SoTranslation *moveT = new SoTranslation();
- moveT->translation.setValue(0.0020f,0.0020f,0.0f);
- textSep->addChild(moveT);
- SoAsciiText *textNode = new SoAsciiText();
+ textSep->addChild(moveT);
+ moveT->translation.setValue(offsetX*0.001f,offsetY*0.001f,offsetZ*0.001f);
+
+ SoAsciiText *textNode = new SoAsciiText();
+ textSep->addChild(textNode);
/*std::string text2(*text);
text2.replace( ' ', "_" );*/
SbString text2(s.c_str());
text2.apply( &IVToolsHelper_ReplaceSpaceWithUnderscore );
textNode->string.set(text2.getString());
- textSep->addChild(textNode);
+
+ textSep->unrefNoDelete();
return textSep;
}
- SoSeparator* CoinVisualizationFactory::CreateBillboardText(const std::string &s)
+ SoSeparator* CoinVisualizationFactory::CreateBillboardText(const std::string &s, float offsetX, float offsetY, float offsetZ)
{
SoSeparator *textSep = new SoSeparator();
+ textSep->ref();
+
+ SoTranslation *moveT = new SoTranslation();
+ textSep->addChild(moveT);
+ moveT->translation.setValue(offsetX*0.001f,offsetY*0.001f,offsetZ*0.001f);
+
SoVRMLBillboard *bb = new SoVRMLBillboard();
textSep->addChild(bb);
- SoTranslation *moveT = new SoTranslation();
- moveT->translation.setValue(0.0020f,0.0020f,0.0f);
- textSep->addChild(moveT);
SoAsciiText *textNode = new SoAsciiText();
+ bb->addChild(textNode);
/*std::string text2(*text);
text2.replace( ' ', "_" );*/
SbString text2(s.c_str());
text2.apply( &IVToolsHelper_ReplaceSpaceWithUnderscore );
textNode->string.set(text2.getString());
- bb->addChild(textNode);
- return textSep;
+
+ textSep->unrefNoDelete();
+ return textSep;
}
+
SoSeparator* CoinVisualizationFactory::CreateVertexVisualization( const Eigen::Vector3f &position, float radius, float transparency, float colorR /*= 0.5f*/, float colorG /*= 0.5f*/, float colorB /*= 0.5f*/ )
{
SoSeparator *res = new SoSeparator;
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index 9ef6e7643e811d1734a376e5669fba0e5d4c1d4f..da95991bdafb318eedf2b893602a4f73354f3b45 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -73,7 +73,18 @@ public:
virtual VisualizationNodePtr createPlane(const Eigen::Vector3f &position, const Eigen::Vector3f &normal, float extend, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f);
virtual VisualizationNodePtr createArrow(const Eigen::Vector3f &n, float length = 50.0f, float width = 2.0f, const Color &color = Color::Gray());
virtual VisualizationNodePtr createTrajectory(TrajectoryPtr t, Color colorNode = Color::Blue(), Color colorLine = Color::Gray(), float nodeSize = 15.0f, float lineSize = 4.0f);
-
+ virtual VisualizationNodePtr createText(const std::string &text, bool billboard = false, float scaling = 1.0f, Color c = Color::Black(), float offsetX = 20.0f, float offsetY = 20.0f, float offsetZ = 0.0f);
+ /*!
+ Creates an coordinate axis aligned ellipse
+ \param x The extend in x direction must be >= 1e-6
+ \param y The extend in y direction must be >= 1e-6
+ \param z The extend in z direction must be >= 1e-6
+ \param showAxes If true, the axes are visualized
+ \param axesHeight The height of the axes (measured from the body surface)
+ \param axesWidth The width of the axes.
+ \return A VisualizationNode containing the visualization.
+ */
+ virtual VisualizationNodePtr createEllipse(float x, float y, float z, bool showAxes = true, float axesHeight = 4.0f, float axesWidth = 8.0f);
/*!
Move local visualization by homogeneous matrix m.
*/
@@ -158,8 +169,14 @@ public:
*/
static SoSeparator* CreateEndEffectorVisualization(EndEffectorPtr eef, SceneObject::VisualizationType = SceneObject::Full);
- static SoSeparator* CreateText(const std::string &s);
- static SoSeparator* CreateBillboardText(const std::string &s);
+ /*!
+ Text visu. Offsets in mm.
+ */
+ static SoSeparator* CreateText(const std::string &s, float offsetX = 20.0f, float offsetY = 20.0f, float offsetZ = 0.0f);
+ /*!
+ Billboard Text visu. Offsets in mm.
+ */
+ static SoSeparator* CreateBillboardText(const std::string &s, float offsetX = 20.0f, float offsetY = 20.0f, float offsetZ = 0.0f);
/*!
Convenient method to retrieve a coin visualization for a robot
diff --git a/VirtualRobot/Visualization/VisualizationFactory.h b/VirtualRobot/Visualization/VisualizationFactory.h
index 90d3302e5bcda2a2fe154805011eb1e71e3e1027..faa612448b1bbf44d2eee4cdc7e684ecd446a2eb 100644
--- a/VirtualRobot/Visualization/VisualizationFactory.h
+++ b/VirtualRobot/Visualization/VisualizationFactory.h
@@ -75,7 +75,18 @@ public:
virtual VisualizationNodePtr createPlane(const MathTools::Plane &plane, float extend, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f){return createPlane(plane.p, plane.n,extend,transparency,colorR,colorG,colorB);}
virtual VisualizationNodePtr createArrow(const Eigen::Vector3f &n, float length = 50.0f, float width = 2.0f, const Color &color = Color::Gray()){return VisualizationNodePtr();}
virtual VisualizationNodePtr createTrajectory(TrajectoryPtr t, Color colorNode = Color::Blue(), Color colorLine = Color::Gray(), float nodeSize = 15.0f, float lineSize = 4.0f){return VisualizationNodePtr();}
-
+ virtual VisualizationNodePtr createText(const std::string &text, bool billboard = false, float scaling = 1.0f, Color c = Color::Black(), float offsetX = 20.0f, float offsetY = 20.0f, float offsetZ = 0.0f){return VisualizationNodePtr();}
+ /*!
+ Creates an coordinate axis aligned ellipse
+ \param x The extend in x direction must be >= 1e-6
+ \param y The extend in y direction must be >= 1e-6
+ \param z The extend in z direction must be >= 1e-6
+ \param showAxes If true, the axes are visualized
+ \param axesHeight The height of the axes (measured from the body surface)
+ \param axesWidth The width of the axes.
+ \return A VisualizationNode containing the visualization.
+ */
+ virtual VisualizationNodePtr createEllipse(float x, float y, float z, bool showAxes = true, float axesHeight = 4.0f, float axesWidth = 8.0f){return VisualizationNodePtr();}
/*!
Move local visualization by homogeneous matrix m.
*/
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index fe0393569f5aac2fb1f70ee2bd75fb027f2120f0..ba36ae2b21294f30e354a21e60362130f528b293 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -900,7 +900,7 @@ void BaseIO::processPhysicsTag(rapidxml::xml_node<char> *physicsXMLNode, const s
}
}
- rapidxml::xml_node<> *inMatXMLNode = physicsXMLNode->first_node("intertiamatrix",0,false);
+ rapidxml::xml_node<> *inMatXMLNode = physicsXMLNode->first_node("inertiamatrix",0,false);
if (inMatXMLNode)
{
physics.intertiaMatrix = process3x3Matrix(inMatXMLNode);
diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
index b9ea4a03cca7cc7c73e5b2d07961f7a3863dd8ba..dbc6f8bca18e0ff2c516c0afd7d0c940a9b2389e 100644
--- a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
+++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
@@ -440,6 +440,12 @@ void showRobotWindow::loadRobot()
cout << " ERROR while creating robot" << endl;
return;
}
+ UI.checkBoxColModel->setChecked(false);
+ UI.checkBoxFullModel->setChecked(true);
+ UI.checkBoxPhysics->setChecked(false);
+ UI.checkBoxRobotCoordSystems->setChecked(false);
+ UI.checkBoxShowCoordSystem->setChecked(false);
+ UI.checkBoxStructure->setChecked(false);
// get nodes
robot->getRobotNodes(allRobotNodes);