diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index 20c58c7deb32926360fcb50c1b619e975f83a178..41dc15e2fb1cc9fa40bf4201997c14cc13c7101e 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -459,6 +459,19 @@ void Robot::showStructure( bool enable, const std::string &type )
}
+void Robot::showPhysicsInformation( bool enableCoM, bool enableInertial, VisualizationNodePtr comModel )
+{
+ std::vector<RobotNodePtr> robotNodes = this->getRobotNodes();
+ std::vector<RobotNodePtr>::const_iterator iterator = robotNodes.begin();
+
+ while(robotNodes.end() != iterator)
+ {
+ (*iterator)->showPhysicsInformation(enableCoM,enableInertial,comModel);
+ ++iterator;
+ }
+
+}
+
void Robot::showCoordinateSystems( bool enable, const std::string &type )
{
std::vector<RobotNodePtr> robotNodes = this->getRobotNodes();
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index 06eaa172cd3ed55f7383a549284dc1ce9699fe19..ce935752775b6bcb9fcba9bd60f6296cd239bc67 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -106,6 +106,14 @@ public:
*/
void showCoordinateSystems(bool enable, const std::string &type="");
+ /*!
+ Display some physics debugging information.
+ \p enableCoM If true, the center of mass is shown (if given). If a comModel is given it is used for visualization, otherwise a standrad marker is shown.
+ \p enableInertial If true, a visualization of the inertial matrix is shown (if given).
+ \p comModel If set, this visualization is used to display the CoM location. If not set, a standard marker is used.
+ */
+ void showPhysicsInformation( bool enableCoM, bool enableInertial, VisualizationNodePtr comModel = VisualizationNodePtr());
+
/*!
Setup the full model visualization.
\param showVisualization If false, the visualization is disabled.
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index 1c633f1c3ba7716f1f39a8af9932071968cbdd7e..821047795ec4f7a90b82edb85de2c8e3c531a3b1 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -9,6 +9,7 @@
#include "Robot.h"
#include <cmath>
#include <algorithm>
+#include <Eigen/Dense>
namespace VirtualRobot {
@@ -198,6 +199,104 @@ void SceneObject::showCoordinateSystem( bool enable, float scaling, std::string
}
}
+void SceneObject::showPhysicsInformation( bool enableCoM, bool enableInertial, VisualizationNodePtr comModel)
+{
+ if (!enableCoM && !enableInertial && !visualizationModel)
+ return; // nothing to do
+
+ if (!visualizationModel)
+ return;
+
+ if (physics.massKg<=0)
+ {
+ // no valid physics information
+ // check for valid model
+ TriMeshModelPtr tm = visualizationModel->getTriMeshModel();
+ if (!tm || tm->vertices.size()<2)
+ return;
+ }
+
+ if (visualizationModel->hasAttachedVisualization("__CoMLocation"))
+ {
+ visualizationModel->detachVisualization("__CoMLocation");
+ }
+ if (visualizationModel->hasAttachedVisualization("__InertiaMatrix"))
+ {
+ visualizationModel->detachVisualization("__InertiaMatrix");
+ }
+ if (enableCoM)
+ {
+ VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(NULL);
+ if (!visualizationFactory)
+ {
+ VR_ERROR << " Could not determine a valid VisualizationFactory!!" << endl;
+ return;
+ }
+ // create visu
+ 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("__CoMLocation",comModelClone);
+ }
+ /*if (enableInertial)
+ {
+ VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(NULL);
+ if (!visualizationFactory)
+ {
+ VR_ERROR << " Could not determine a valid VisualizationFactory!!" << endl;
+ return;
+ }
+ // create inertia visu
+
+ Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(physics.intertiaMatrix);
+ if (eigensolver.info() == Eigen::Success)
+ {
+ 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);
+ } 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()
{
if (visualizationModel)
diff --git a/VirtualRobot/SceneObject.h b/VirtualRobot/SceneObject.h
index 38af66e6bafe98aede3732566518523195ae8608..de3623ed20987b345c0fd8737d08ae281c1f6d08 100644
--- a/VirtualRobot/SceneObject.h
+++ b/VirtualRobot/SceneObject.h
@@ -190,6 +190,14 @@ public:
*/
virtual void showCoordinateSystem( bool enable, float scaling = 1.0f, std::string *text = NULL);
+ /*!
+ Display some physics debugging information.
+ \p enableCoM If true, the center of mass is shown (if given). If a comModel is given it is used for visualization, otherwise a standrad marker is shown.
+ \p enableInertial If true, a visualization of the inertial matrix is shown (if given).
+ \p comModel If set, this visualization is used to display the CoM location. If not set, a standard marker is used.
+ */
+ virtual void showPhysicsInformation( bool enableCoM, bool enableInertial, VisualizationNodePtr comModel = VisualizationNodePtr());
+
/*!
Returns true when the coordinate system is currently shown.
*/
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index 0a429001ff958eac26e4470006760b88a060c41c..9e1cef675eca936bd0b1215a9e715bbe6bcafa4c 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -2443,6 +2443,39 @@ SoSeparator* CoinVisualizationFactory::CreateEllipse(float x, float y, float z,
return result;
}
+void CoinVisualizationFactory::applyDisplacement( VisualizationNodePtr o, Eigen::Matrix4f &m )
+{
+ if (!o)
+ return;
+
+
+ if (o->getType() != getName())
+ {
+ VR_ERROR << "Skipping Visualization type " << o->getType() << ", but factory is of type " << getName() << endl;
+ return;
+ }
+ CoinVisualizationNode* cvn = dynamic_cast<CoinVisualizationNode*>(o.get());
+ if (cvn)
+ {
+ SoNode* n = cvn->getCoinVisualization();
+ if (n)
+ {
+ SoSeparator *s = new SoSeparator;
+ s->ref();
+ SoMatrixTransform *ma = getMatrixTransformM(m);
+ s->addChild(ma);
+ s->addChild(n->copy(FALSE));
+
+ cvn->setVisualization(s);
+ //o.reset(new CoinVisualizationNode(s));
+ s->unrefNoDelete();
+ }
+ } else
+ {
+ VR_WARNING << "Invalid type casting to CoinVisualizationNode?!" << endl;
+ }
+}
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index 7878eea8bd939dd066cb3a46ed5b4c7fb1895977..9ef6e7643e811d1734a376e5669fba0e5d4c1d4f 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -74,6 +74,11 @@ public:
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);
+ /*!
+ Move local visualization by homogeneous matrix m.
+ */
+ virtual void applyDisplacement(VisualizationNodePtr o, Eigen::Matrix4f &m);
+
/*!
Create an empty VisualizationNode.
*/
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp
index a7df22db81ed31725f138e712abdbdcbe6279729..3ed576d42f5b1a670f671a31bf264b775642c40b 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp
@@ -44,6 +44,7 @@ CoinVisualizationNode::CoinVisualizationNode(SoNode* visualizationNode) :
visualization->ref();
visualizationAtGlobalPose->addChild(visualization);
+
}
@@ -206,10 +207,14 @@ void CoinVisualizationNode::print()
{
Eigen::Vector3f mi;
Eigen::Vector3f ma;
- triMeshModel->getSize(mi,ma);
- cout << triMeshModel->faces.size() << " triangles" << endl;// Extend: " << ma[0]-mi[0] << ", " << ma[1] - mi[1] << ", " << ma[2] - mi[2] << endl;
- cout << " Min point: (" << mi[0] << "," << mi[1] << "," << mi[2] << ")" << endl;
- cout << " Max point: (" << ma[0] << "," << ma[1] << "," << ma[2] << ")" << endl;
+ if (triMeshModel->faces.size()>0)
+ {
+ cout << triMeshModel->faces.size() << " triangles" << endl;// Extend: " << ma[0]-mi[0] << ", " << ma[1] - mi[1] << ", " << ma[2] - mi[2] << endl;
+ triMeshModel->getSize(mi,ma);
+ cout << " Min point: (" << mi[0] << "," << mi[1] << "," << mi[2] << ")" << endl;
+ cout << " Max point: (" << ma[0] << "," << ma[1] << "," << ma[2] << ")" << endl;
+ } else
+ cout << "No model" << endl;
} else
cout << "No model" << endl;
@@ -293,5 +298,28 @@ void CoinVisualizationNode::setupVisualization( bool showVisualization, bool sho
visualizationAtGlobalPose->removeChild(visualization);
}
+void CoinVisualizationNode::setVisualization( SoNode* newVisu )
+{
+ if (!newVisu)
+ return;
+
+ if (visualizationAtGlobalPose)
+ {
+ int indx = visualizationAtGlobalPose->findChild(visualization);
+
+ if (indx>=0)
+ visualizationAtGlobalPose->removeChild(indx);
+
+ }
+ visualization->unref();
+
+ visualization = newVisu;
+
+ visualization->ref();
+
+ if (visualizationAtGlobalPose)
+ visualizationAtGlobalPose->addChild(visualization);
+}
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h
index 387a5f3653032a4970ea5566b25268915d26ba06..42e851141d499856cbccd483643b657be2aab7b0 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h
@@ -41,6 +41,7 @@ class TriMeshModel;
class VIRTUAL_ROBOT_IMPORT_EXPORT CoinVisualizationNode : virtual public VisualizationNode
{
+ friend CoinVisualizationFactory;
public:
CoinVisualizationNode(SoNode* visualizationNode);
~CoinVisualizationNode();
@@ -80,9 +81,16 @@ public:
virtual std::string getType(){return CoinVisualizationFactory::getName();}
+
protected:
void createTriMeshModel();
+ /*!
+ Replace current visualization of this node.
+ Be careful: any former grabbed trimeshmodels do no longer represent the new datastructure!
+ */
+ void setVisualization(SoNode* newVisu);
+
SoNode* visualization;
SoSeparator* visualizationAtGlobalPose;
SoSeparator* attachedVisualizationsSeparator;
diff --git a/VirtualRobot/Visualization/VisualizationFactory.h b/VirtualRobot/Visualization/VisualizationFactory.h
index ac54604778c7516129a5c5cb07ca78c4d2802d3b..90d3302e5bcda2a2fe154805011eb1e71e3e1027 100644
--- a/VirtualRobot/Visualization/VisualizationFactory.h
+++ b/VirtualRobot/Visualization/VisualizationFactory.h
@@ -76,6 +76,11 @@ public:
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();}
+ /*!
+ Move local visualization by homogeneous matrix m.
+ */
+ virtual void applyDisplacement(VisualizationNodePtr o, Eigen::Matrix4f &m){}
+
/*!
Create an empty VisualizationNode.
*/
diff --git a/VirtualRobot/Visualization/VisualizationNode.cpp b/VirtualRobot/Visualization/VisualizationNode.cpp
index be326a90ddfb585acca2d94e6f3d0408cdfeb1bf..6b339b83bee171606d88e48a7195244e0162d324 100644
--- a/VirtualRobot/Visualization/VisualizationNode.cpp
+++ b/VirtualRobot/Visualization/VisualizationNode.cpp
@@ -20,6 +20,7 @@ VisualizationNode::VisualizationNode()
updateVisualization = true;
showVisualization = true;
showAttachedVisualizations = true;
+ globalPose.setIdentity();
}
VisualizationNode::~VisualizationNode()
diff --git a/VirtualRobot/examples/RobotViewer/RobotViewer.ui b/VirtualRobot/examples/RobotViewer/RobotViewer.ui
index 98b8d5c503d58f05d06b9034c1967e295ab12c28..7ae14c6b1f6e8d1513f960cd422b82b0cc571179 100644
--- a/VirtualRobot/examples/RobotViewer/RobotViewer.ui
+++ b/VirtualRobot/examples/RobotViewer/RobotViewer.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1079</width>
- <height>815</height>
+ <width>1103</width>
+ <height>840</height>
</rect>
</property>
<property name="windowTitle">
@@ -23,6 +23,7 @@
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
+ <zorder>groupBox</zorder>
</widget>
</item>
<item row="0" column="1">
@@ -55,7 +56,7 @@
<x>0</x>
<y>470</y>
<width>201</width>
- <height>171</height>
+ <height>201</height>
</rect>
</property>
<property name="title">
@@ -126,6 +127,19 @@
<string>Full model options</string>
</property>
</widget>
+ <widget class="QCheckBox" name="checkBoxPhysics">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>170</y>
+ <width>131</width>
+ <height>20</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Physics Data</string>
+ </property>
+ </widget>
</widget>
<widget class="QPushButton" name="pushButtonLoad">
<property name="geometry">
@@ -354,9 +368,9 @@
<property name="geometry">
<rect>
<x>0</x>
- <y>649</y>
+ <y>680</y>
<width>201</width>
- <height>101</height>
+ <height>91</height>
</rect>
</property>
<property name="title">
@@ -411,7 +425,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1079</width>
+ <width>1103</width>
<height>21</height>
</rect>
</property>
diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
index 19845afb522368e62bd574f3551b518bb944820b..b9ea4a03cca7cc7c73e5b2d07961f7a3863dd8ba 100644
--- a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
+++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
@@ -114,6 +114,7 @@ void showRobotWindow::setupUI()
connect(UI.pushButtonOpen, SIGNAL(clicked()), this, SLOT(openHand()));
connect(UI.comboBoxEndEffector, SIGNAL(activated(int)), this, SLOT(selectEEF(int)));
+ connect(UI.checkBoxPhysics, SIGNAL(clicked()), this, SLOT(displayPhysics()));
connect(UI.checkBoxColModel, SIGNAL(clicked()), this, SLOT(collisionModel()));
connect(UI.checkBoxStructure, SIGNAL(clicked()), this, SLOT(robotStructure()));
@@ -234,6 +235,18 @@ void showRobotWindow::collisionModel()
}
+void showRobotWindow::displayPhysics()
+{
+ if (!robot)
+ return;
+
+ physicsEnabled = UI.checkBoxPhysics->checkState() == Qt::Checked;
+ robot->showPhysicsInformation(physicsEnabled,physicsEnabled);
+
+ // rebuild visualization
+ collisionModel();
+
+}
void showRobotWindow::showRobot()
{
//m_pGraspScenery->showRobot(m_pShowRobot->state() == QCheckBox::On);
@@ -299,13 +312,13 @@ void showRobotWindow::selectRNS(int nr)
return;
currentRobotNodeSet = robotNodeSets[nr];
currentRobotNodes = currentRobotNodeSet->getAllRobotNodes();
- cout << "HIGHLIGHTING rns " << currentRobotNodeSet->getName() << endl;
+ /*cout << "HIGHLIGHTING rns " << currentRobotNodeSet->getName() << endl;
if (visualization)
{
robot->highlight(visualization,false);
currentRobotNodeSet->highlight(visualization,true);
- }
+ }*/
}
updateJointBox();
@@ -315,11 +328,14 @@ void showRobotWindow::selectRNS(int nr)
void showRobotWindow::selectJoint(int nr)
{
+ if (currentRobotNode)
+ currentRobotNode->showBoundingBox(false);
currentRobotNode.reset();
cout << "Selecting Joint nr " << nr << endl;
if (nr<0 || nr>=(int)currentRobotNodes.size())
return;
currentRobotNode = currentRobotNodes[nr];
+ currentRobotNode->showBoundingBox(true,true);
currentRobotNode->print();
float mi = currentRobotNode->getJointLimitLo();
float ma = currentRobotNode->getJointLimitHi();
@@ -344,13 +360,13 @@ void showRobotWindow::selectJoint(int nr)
else
UI.checkBoxShowCoordSystem->setCheckState(Qt::Unchecked);
- /*cout << "HIGHLIGHTING node " << currentRobotNodes[nr]->getName() << endl;
+ cout << "HIGHLIGHTING node " << currentRobotNodes[nr]->getName() << endl;
if (visualization)
{
robot->highlight(visualization,false);
currentRobotNode->highlight(visualization,true);
- }*/
+ }
displayTriangles();
}
@@ -447,6 +463,7 @@ void showRobotWindow::loadRobot()
// build visualization
collisionModel();
robotStructure();
+ displayPhysics();
viewer->viewAll();
}
diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.h b/VirtualRobot/examples/RobotViewer/showRobotWindow.h
index 1aa3c125927a7597186d121fc9e90cd7381a9075..df41f2bc97bd5af09642177e62288fdfee96c74c 100644
--- a/VirtualRobot/examples/RobotViewer/showRobotWindow.h
+++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.h
@@ -61,6 +61,7 @@ public slots:
void openHand();
void selectEEF(int nr);
void selectRobot();
+ void displayPhysics();
SoQtExaminerViewer* getExaminerViewer(){return viewer;};
@@ -91,6 +92,7 @@ protected:
bool useColModel;
bool structureEnabled;
+ bool physicsEnabled;
boost::shared_ptr<VirtualRobot::CoinVisualization> visualization;
};