From 2a10fd4ba3dbbce03907ea5f67d3276228666dce Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Wed, 12 Jun 2013 11:08:26 +0000
Subject: [PATCH] Fixing trajectory interpolation Renamed coin factory classes
for building transformations
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@385 042f3d55-54a8-47e9-b7fb-15903f145c44
---
.../GraspPlanner/GraspPlannerWindow.cpp | 4 ++--
.../GraspQuality/GraspQualityWindow.cpp | 2 +-
.../examples/GraspRRT/GraspRrtWindow.cpp | 2 +-
.../MTPlanningScenery.cpp | 4 ++--
VirtualRobot/Trajectory.cpp | 4 ++--
.../CoinVisualizationFactory.cpp | 20 +++++++++----------
.../CoinVisualizationFactory.h | 4 ++--
7 files changed, 20 insertions(+), 20 deletions(-)
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
index cf47a5c37..7e6ad3cad 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
@@ -226,7 +226,7 @@ void GraspPlannerWindow::buildVisu()
Eigen::Matrix4f ma;
ma.setIdentity();
ma.block(0,3,3,1) = contacts[i].contactPointFingerGlobal;
- SoMatrixTransform *m = CoinVisualizationFactory::getMatrixTransformMM(ma);
+ SoMatrixTransform *m = CoinVisualizationFactory::getMatrixTransformScaleMM2M(ma);
s->addChild(m);
s->addChild(CoinVisualizationFactory::CreateArrow(contacts[i].approachDirectionGlobal,10.0f,1.0f));
frictionConeSep->addChild(s);
@@ -323,7 +323,7 @@ void GraspPlannerWindow::plan()
{
Eigen::Matrix4f m = grasps->getGrasp(i)->getTcpPoseGlobal(object->getGlobalPose());
SoSeparator *sep1 = new SoSeparator();
- SoMatrixTransform *mt = CoinVisualizationFactory::getMatrixTransformMM(m);
+ SoMatrixTransform *mt = CoinVisualizationFactory::getMatrixTransformScaleMM2M(m);
sep1->addChild(mt);
sep1->addChild(eefVisu);
graspsSep->addChild(sep1);
diff --git a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
index 405ed7080..a4e22744b 100644
--- a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
+++ b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
@@ -387,7 +387,7 @@ void GraspQualityWindow::showGWS()
ows1Sep->removeAllChildren();
ows2Sep->removeAllChildren();
Eigen::Matrix4f m = object->getGlobalPose();
- SoMatrixTransform *mt = CoinVisualizationFactory::getMatrixTransformMM(m);
+ SoMatrixTransform *mt = CoinVisualizationFactory::getMatrixTransformScaleMM2M(m);
#if 0
// test
diff --git a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
index 07d9b1c55..cded146bf 100644
--- a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
+++ b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
@@ -199,7 +199,7 @@ void GraspRrtWindow::buildVisu()
Eigen::Matrix4f m = grasps[i]->getTcpPoseGlobal(targetObject->getGlobalPose());
SoSeparator *sep1 = new SoSeparator();
- SoMatrixTransform *mt = CoinVisualizationFactory::getMatrixTransformMM(m);
+ SoMatrixTransform *mt = CoinVisualizationFactory::getMatrixTransformScaleMM2M(m);
sep1->addChild(mt);
sep1->addChild(eefVisu);
graspsSep->addChild(sep1);
diff --git a/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningScenery.cpp b/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningScenery.cpp
index a26c5b9f8..50e2bbf12 100644
--- a/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningScenery.cpp
+++ b/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningScenery.cpp
@@ -343,10 +343,10 @@ void MTPlanningScenery::buildPlanningThread(bool bMultiCollisionCheckers, int id
return;
pRobot->setJointValues(kinChain,start);
Eigen::Matrix4f gp = rn->getGlobalPose();
- SoMatrixTransform *mt = CoinVisualizationFactory::getMatrixTransformM(gp);//no transformation -> our scene is already in MM units
+ SoMatrixTransform *mt = CoinVisualizationFactory::getMatrixTransform(gp);//no transformation -> our scene is already in MM units
pRobot->setJointValues(kinChain,goal);
gp = rn->getGlobalPose();
- SoMatrixTransform *mt2 =CoinVisualizationFactory::getMatrixTransformM(gp);//no transformation -> our scene is already in MM units
+ SoMatrixTransform *mt2 =CoinVisualizationFactory::getMatrixTransform(gp);//no transformation -> our scene is already in MM units
SoSeparator *sep1 = new SoSeparator();
sep1->addChild(mt);
diff --git a/VirtualRobot/Trajectory.cpp b/VirtualRobot/Trajectory.cpp
index 0460d07a5..c6ea4cd85 100644
--- a/VirtualRobot/Trajectory.cpp
+++ b/VirtualRobot/Trajectory.cpp
@@ -224,7 +224,7 @@ void Trajectory::interpolate( float t, Eigen::VectorXf &storePathPos, int *store
{
storePathPos = getPoint(getNrOfPoints()-1);
if (storeIndex!=NULL)
- *storeIndex = (int)path.size();
+ *storeIndex = (int)path.size()-1;
return;
}
@@ -277,7 +277,7 @@ float Trajectory::getLength() const
float pathLength = 0.0f;
Eigen::VectorXf c1,c2;
float l;
- for (int i=0;i<(int)getNrOfPoints()-2;i++)
+ for (int i=0;i<(int)getNrOfPoints()-1;i++)
{
c1 = path[i];
c2 = path[i+1];
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index e324467da..0f3cb4a2c 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -1062,7 +1062,7 @@ namespace VirtualRobot {
res->addChild(sepGrasp);
// transform
- SoMatrixTransform *mT = getMatrixTransformMM(mat);
+ SoMatrixTransform *mT = getMatrixTransformScaleMM2M(mat);
sepGrasp->addChild(mT);
// eef Visu
@@ -1255,7 +1255,7 @@ namespace VirtualRobot {
{
SoSeparator *res = new SoSeparator;
SoNode *res1 = CoinVisualizationFactory::getCoinVisualization(model,showNormals);
- SoMatrixTransform *mt = getMatrixTransformMM(pose);
+ SoMatrixTransform *mt = getMatrixTransformScaleMM2M(pose);
res->addChild(mt);
res->addChild(res1);
@@ -1263,7 +1263,7 @@ namespace VirtualRobot {
return node;
}
- SoMatrixTransform* CoinVisualizationFactory::getMatrixTransformM( Eigen::Matrix4f &m )
+ SoMatrixTransform* CoinVisualizationFactory::getMatrixTransform( Eigen::Matrix4f &m )
{
SoMatrixTransform *mt = new SoMatrixTransform;
SbMatrix m_(reinterpret_cast<SbMat*>(m.data()));
@@ -1271,7 +1271,7 @@ namespace VirtualRobot {
return mt;
}
- SoMatrixTransform* CoinVisualizationFactory::getMatrixTransformMM( Eigen::Matrix4f &m )
+ SoMatrixTransform* CoinVisualizationFactory::getMatrixTransformScaleMM2M( Eigen::Matrix4f &m )
{
SoMatrixTransform *mt = new SoMatrixTransform;
SbMatrix m_(reinterpret_cast<SbMat*>(m.data()));
@@ -1411,7 +1411,7 @@ namespace VirtualRobot {
if (transformToGlobalPose)
reachSpace->toGlobal(pose);
- SoMatrixTransform *mt = getMatrixTransformMM(pose);
+ SoMatrixTransform *mt = getMatrixTransformScaleMM2M(pose);
sep->addChild(mt);
sep->addChild(arrow);
res->addChild(sep);
@@ -1514,7 +1514,7 @@ namespace VirtualRobot {
col->diffuseColor.setValue(color.r,color.g,color.b);
col->transparency.setValue(color.transparency);
sep->addChild(col);
- SoMatrixTransform *mt = getMatrixTransformMM(m);
+ SoMatrixTransform *mt = getMatrixTransformScaleMM2M(m);
sep->addChild(mt);
sep->addChild(arrow);
res->addChild(sep);
@@ -1811,7 +1811,7 @@ namespace VirtualRobot {
gp(1,3) = yPos;
SoSeparator *sep1 = new SoSeparator();
- SoMatrixTransform* matTr = getMatrixTransformM(gp); // we are already in mm, no conversion to m needed
+ SoMatrixTransform* matTr = getMatrixTransform(gp); // we are already in mm, no conversion to m needed
float intensity = (float)v;
intensity /= maxEntry;
@@ -1931,7 +1931,7 @@ namespace VirtualRobot {
gp(1,3) = yPos;
SoSeparator *sep1 = new SoSeparator();
- SoMatrixTransform* matTr = getMatrixTransformM(gp); // we are in mm unit environment -> no conversion to m needed
+ SoMatrixTransform* matTr = getMatrixTransform(gp); // we are in mm unit environment -> no conversion to m needed
float intensity = (float)v;
intensity /= maxEntry;
@@ -2042,7 +2042,7 @@ namespace VirtualRobot {
gp(1,3) = yPos;
SoSeparator *sep1 = new SoSeparator();
- SoMatrixTransform* matTr = getMatrixTransformM(gp); // we are in mm unit environment -> no conversion to m needed
+ SoMatrixTransform* matTr = getMatrixTransform(gp); // we are in mm unit environment -> no conversion to m needed
float intensity = (float)v;
intensity /= maxEntry;
@@ -2512,7 +2512,7 @@ void CoinVisualizationFactory::applyDisplacement( VisualizationNodePtr o, Eigen:
{
SoSeparator *s = new SoSeparator;
s->ref();
- SoMatrixTransform *ma = getMatrixTransformM(m);
+ SoMatrixTransform *ma = getMatrixTransform(m);
s->addChild(ma);
s->addChild(n->copy(FALSE));
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index da95991bd..3fdec13ec 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -227,14 +227,14 @@ public:
Create a SoMatrixTransform from the given pose
\param m The pose with translation given in meter.
*/
- static SoMatrixTransform* getMatrixTransformM(Eigen::Matrix4f &m);
+ static SoMatrixTransform* getMatrixTransform(Eigen::Matrix4f &m);
/*!
Create a SoMatrixTransform from the given pose
Converts the pose from MM to M (scales by 0.001)
\param m The pose with translation given in millimeter.
*/
- static SoMatrixTransform* getMatrixTransformMM(Eigen::Matrix4f &m);
+ static SoMatrixTransform* getMatrixTransformScaleMM2M(Eigen::Matrix4f &m);
static SoNode* createCoinLine(const Eigen::Matrix4f &from, const Eigen::Matrix4f &to, float width, float colorR, float colorG, float colorB);
/*!
--
GitLab