diff --git a/GraspPlanning/CMakeLists.txt b/GraspPlanning/CMakeLists.txt
index cffe959c6e142916a886887e135c6598ad26efb1..bd659c1f56cd31815e5598d07bc7e95420481328 100644
--- a/GraspPlanning/CMakeLists.txt
+++ b/GraspPlanning/CMakeLists.txt
@@ -50,6 +50,7 @@ ConvexHullGenerator.cpp
ContactConeGenerator.cpp
ApproachMovementGenerator.cpp
ApproachMovementSurfaceNormal.cpp
+MeshConverter.cpp
GraspPlanner/GraspPlanner.cpp
GraspPlanner/GenericGraspPlanner.cpp
GraspQuality/GraspQualityMeasure.cpp
@@ -63,6 +64,7 @@ ConvexHullGenerator.h
ContactConeGenerator.h
ApproachMovementGenerator.h
ApproachMovementSurfaceNormal.h
+MeshConverter.h
GraspPlanner/GraspPlanner.h
GraspPlanner/GenericGraspPlanner.h
GraspQuality/GraspQualityMeasure.h
@@ -85,7 +87,6 @@ IF(BUILD_powercrust)
GraspPlanner/MATPlanner/LocalNeighborhood.cpp
GraspPlanner/MATPlanner/MatGraspPlanner.cpp
GraspPlanner/MATPlanner/MedialSphere.cpp
- GraspPlanner/MATPlanner/MeshConverter.cpp
GraspPlanner/MATPlanner/SphereHelpers.cpp
GraspPlanner/MATPlanner/StrOutHelpers.cpp
GraspPlanner/MATPlanner/TestCases.cpp
@@ -103,7 +104,6 @@ IF(BUILD_powercrust)
GraspPlanner/MATPlanner/LocalNeighborhood.h
GraspPlanner/MATPlanner/MatGraspPlanner.h
GraspPlanner/MATPlanner/MedialSphere.h
- GraspPlanner/MATPlanner/MeshConverter.h
GraspPlanner/MATPlanner/SphereHelpers.h
GraspPlanner/MATPlanner/StrOutHelpers.h
GraspPlanner/MATPlanner/TestCases.h
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.cpp b/GraspPlanning/MeshConverter.cpp
similarity index 78%
rename from GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.cpp
rename to GraspPlanning/MeshConverter.cpp
index 3a1f8b84de6d8aac7ad550c00b5353fabdf40273..cf2ca68157532d35481823eb5aeb66705179af80 100644
--- a/GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.cpp
+++ b/GraspPlanning/MeshConverter.cpp
@@ -22,6 +22,7 @@
*/
#include "MeshConverter.h"
#include <VirtualRobot/Visualization/VisualizationFactory.h>
+#include <GraspPlanning/ConvexHullGenerator.h>
using namespace std;
using namespace Eigen;
@@ -29,8 +30,74 @@ using namespace VirtualRobot;
namespace GraspStudio
{
+ VirtualRobot::ManipulationObjectPtr MeshConverter::CreateManipulationObject(const std::string &name, VirtualRobot::MathTools::ConvexHull3DPtr hull)
+ {
+ VirtualRobot::ManipulationObjectPtr res;
+
+ if (!hull)
+ {
+ return res;
+ }
+
+ TriMeshModelPtr tm = MeshConverter::CreateTriMeshModel(hull);
+
+ if (!tm)
+ {
+ return res;
+ }
+
+ VisualizationFactoryPtr cv = VisualizationFactory::first(NULL);
+ if (!cv)
+ {
+ return res;
+ }
+
+ Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
+ VisualizationNodePtr visu = cv->createTriMeshModelVisualization(tm, false, gp, false);
+ CollisionModelPtr cm(new CollisionModel(visu));
+ res.reset(new ManipulationObject(name, visu, cm));
+
+ return res;
+ }
+ VirtualRobot::TriMeshModelPtr MeshConverter::CreateTriMeshModel(VirtualRobot::MathTools::ConvexHull3DPtr hull)
+ {
+ VirtualRobot::TriMeshModelPtr res(new TriMeshModel());
+
+ if (!hull || hull->vertices.size() == 0 || hull->faces.size() == 0)
+ {
+ return res;
+ }
+
+ int nFaces = (int)hull->faces.size();
+ int nVertices = nFaces * 3;
+
+ int nVertexCount = 0;
+
+ for (int i = 0; i < nFaces; i++)
+ {
+ Eigen::Vector3f &v1 = hull->vertices.at(hull->faces[i].id1);
+ Eigen::Vector3f &v2 = hull->vertices.at(hull->faces[i].id2);
+ Eigen::Vector3f &v3 = hull->vertices.at(hull->faces[i].id3);
+
+ Eigen::Vector3f &n = hull->faces[i].normal;
+
+ bool bNeedFlip = ConvexHullGenerator::checkVerticeOrientation(v1, v2, v3, n);
+
+ if (bNeedFlip)
+ {
+ res->addTriangleWithFace(v3, v2, v1);
+ }
+ else
+ {
+ res->addTriangleWithFace(v1, v2, v3);
+ }
+ nVertexCount+=3;
+ }
+ return res;
+ }
+
int MeshConverter::hasVertex(std::vector< Eigen::Vector3f>& vectList, Eigen::Vector3f& obj)
{
for (size_t j = 0; j < vectList.size(); j++)
@@ -44,7 +111,7 @@ namespace GraspStudio
return -1;
}
- VirtualRobot::ObstaclePtr MeshConverter::refineObjectSurface(VirtualRobot::ObstaclePtr object, float maxDist)
+ VirtualRobot::ObstaclePtr MeshConverter::RefineObjectSurface(VirtualRobot::ObstaclePtr object, float maxDist)
{
VirtualRobot::ObstaclePtr res;
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.h b/GraspPlanning/MeshConverter.h
similarity index 76%
rename from GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.h
rename to GraspPlanning/MeshConverter.h
index bbc29f66962b9181868febbb615ea4544937752d..c60a45070e06a559011a990f63db0d75db75be5a 100644
--- a/GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.h
+++ b/GraspPlanning/MeshConverter.h
@@ -23,7 +23,7 @@
#ifndef MESHCONVERTER_H
#define MESHCONVERTER_H
-#include "../../GraspStudio.h"
+#include "GraspStudio.h"
#include <vector>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
@@ -36,7 +36,12 @@ namespace GraspStudio
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- static VirtualRobot::ObstaclePtr refineObjectSurface(VirtualRobot::ObstaclePtr object, float maxDist);
+ /*!
+ Create an object. The visualization and collision model is created from the convex hull.
+ */
+ static VirtualRobot::ManipulationObjectPtr CreateManipulationObject(const std::string &name, VirtualRobot::MathTools::ConvexHull3DPtr hull);
+ static VirtualRobot::TriMeshModelPtr CreateTriMeshModel(VirtualRobot::MathTools::ConvexHull3DPtr hull);
+ static VirtualRobot::ObstaclePtr RefineObjectSurface(VirtualRobot::ObstaclePtr object, float maxDist);
//! Returns -1 if obj is not part of vectList, otherwise the index of vectList is returned.
static int hasVertex(std::vector< Eigen::Vector3f>& vectList, Eigen::Vector3f& obj);
diff --git a/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.cpp b/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.cpp
index 7625a026ac6c007cdd3d39b4665c8693ff73e93e..8f54f032618ee1df32795fa062d71dd3ad1294e2 100644
--- a/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.cpp
+++ b/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.cpp
@@ -111,11 +111,6 @@ namespace GraspStudio
int nFaces = (int)convHull->faces.size();
int nVertices = nFaces * 3;
- //Face3D f;
- //Vec3D v1,v2,v3;
-
- //SoNormal *pNormals = new SoNormal;
- //SbVec3f* normalsArray = new SbVec3f[nFaces];
// compute points and normals
SbVec3f* pVertexArray = new SbVec3f[nVertices];
@@ -155,23 +150,6 @@ namespace GraspStudio
}
nVertexCount++;
-
- /*if (bNeedFlip)
- {
- normalsArray[i][0] = n(0);
- normalsArray[i][1] = n(1);
- normalsArray[i][2] = n(2);
- } else
- {
- normalsArray[i][0] = -n(0);
- normalsArray[i][1] = -n(1);
- normalsArray[i][2] = -n(2);
- }*/
- //VR_INFO << "Face " << i << ": v1: " << v1(0) << "," << v1(1) << "," << v1(2) << endl;
- //VR_INFO << " " << i << ": v2: " << v2(0) << "," << v2(1) << "," << v2(2) << endl;
- //VR_INFO << " " << i << ": v3: " << v3(0) << "," << v3(1) << "," << v3(2) << endl;
-
-
}
// set normals
@@ -205,28 +183,6 @@ namespace GraspStudio
return result;
}
- /*
- void addVertex(Vec3D &v1,Vec3D &v2,Vec3D &v3,Vec3D &normal,SbVec3f *pVertexArray, int& nVertexCount)
- {
- bool bNeedFlip = GraspStudioHelpers::checkVerticeOrientation(v1,v2,v3,normal);
-
- // COUNTER CLOCKWISE
- if (bNeedFlip)
- pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z);
- else
- pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z);
- nVertexCount++;
-
- pVertexArray[nVertexCount].setValue((float)v2.x,(float)v2.y,(float)v2.z);
- nVertexCount++;
-
- if (bNeedFlip)
- pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z);
- else
- pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z);
- nVertexCount++;
- }*/
-
SoSeparator* CoinConvexHullVisualization::createConvexHullVisualization(VirtualRobot::MathTools::ConvexHull6DPtr& convHull, bool buseFirst3Coords)
{
@@ -293,117 +249,7 @@ namespace GraspStudio
result->addChild(hullV);
result->unrefNoDelete();
return result;
- /*
- // creates 3d-projection of all 6d facets
- int nVertices = nFaces*12;
- SoCoordinate3* pCoords = new SoCoordinate3();
- SoFaceSet* pFaceSet = new SoFaceSet();
- Face6d f;
- Vec3d v1,v2,v3,v4,v5,v6;
- Vec3d normal;
-
- SbVec3f *pVertexArray = new SbVec3f[nVertices];
-
- int nVertexCount = 0;
- bool bNeedFlip = false;
-
- for (int i=0;i<nFaces;i++)
- {
- f = convHull.faces.at(i);
- if (buseFirst3Coords)
- {
- v1.x = convHull.vertices.at(f.id[0]).x;
- v1.y = convHull.vertices.at(f.id[0]).y;
- v1.z = convHull.vertices.at(f.id[0]).z;
- v2.x = convHull.vertices.at(f.id[1]).x;
- v2.y = convHull.vertices.at(f.id[1]).y;
- v2.z = convHull.vertices.at(f.id[1]).z;
- v3.x = convHull.vertices.at(f.id[2]).x;
- v3.y = convHull.vertices.at(f.id[2]).y;
- v3.z = convHull.vertices.at(f.id[2]).z;
- v4.x = convHull.vertices.at(f.id[3]).x;
- v4.y = convHull.vertices.at(f.id[3]).y;
- v4.z = convHull.vertices.at(f.id[3]).z;
- v5.x = convHull.vertices.at(f.id[4]).x;
- v5.y = convHull.vertices.at(f.id[4]).y;
- v5.z = convHull.vertices.at(f.id[4]).z;
- v6.x = convHull.vertices.at(f.id[5]).x;
- v6.y = convHull.vertices.at(f.id[5]).y;
- v6.z = convHull.vertices.at(f.id[5]).z;
- normal.x = f.normal.x;
- normal.y = f.normal.y;
- normal.z = f.normal.z;
- } else
- {
- v1.x = convHull.vertices.at(f.id[0]).nx;
- v1.y = convHull.vertices.at(f.id[0]).ny;
- v1.z = convHull.vertices.at(f.id[0]).nz;
- v2.x = convHull.vertices.at(f.id[1]).nx;
- v2.y = convHull.vertices.at(f.id[1]).ny;
- v2.z = convHull.vertices.at(f.id[1]).nz;
- v3.x = convHull.vertices.at(f.id[2]).nx;
- v3.y = convHull.vertices.at(f.id[2]).ny;
- v3.z = convHull.vertices.at(f.id[2]).nz;
- v4.x = convHull.vertices.at(f.id[3]).nx;
- v4.y = convHull.vertices.at(f.id[3]).ny;
- v4.z = convHull.vertices.at(f.id[3]).nz;
- v5.x = convHull.vertices.at(f.id[4]).nx;
- v5.y = convHull.vertices.at(f.id[4]).ny;
- v5.z = convHull.vertices.at(f.id[4]).nz;
- v6.x = convHull.vertices.at(f.id[5]).nx;
- v6.y = convHull.vertices.at(f.id[5]).ny;
- v6.z = convHull.vertices.at(f.id[5]).nz;
- normal.x = f.normal.nx;
- normal.y = f.normal.ny;
- normal.z = f.normal.nz;
- }
- bool bNeedFlip = GraspStudioHelpers::checkVerticeOrientation(v1,v2,v3,normal);
- if (bNeedFlip)
- {
- pVertexArray[nVertexCount].setValue((float)v6.x,(float)v6.y,(float)v6.z);
- nVertexCount++;
- pVertexArray[nVertexCount].setValue((float)v5.x,(float)v5.y,(float)v5.z);
- nVertexCount++;
- pVertexArray[nVertexCount].setValue((float)v4.x,(float)v4.y,(float)v4.z);
- nVertexCount++;
- pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z);
- nVertexCount++;
- pVertexArray[nVertexCount].setValue((float)v2.x,(float)v2.y,(float)v2.z);
- nVertexCount++;
- pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z);
- nVertexCount++;
- } else
- {
- pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z);
- nVertexCount++;
- pVertexArray[nVertexCount].setValue((float)v2.x,(float)v2.y,(float)v2.z);
- nVertexCount++;
- pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z);
- nVertexCount++;
- pVertexArray[nVertexCount].setValue((float)v4.x,(float)v4.y,(float)v4.z);
- nVertexCount++;
- pVertexArray[nVertexCount].setValue((float)v5.x,(float)v5.y,(float)v5.z);
- nVertexCount++;
- pVertexArray[nVertexCount].setValue((float)v6.x,(float)v6.y,(float)v6.z);
- nVertexCount++;
- }
- }
- pCoords->point.setValues(0,nVertices,pVertexArray);
- long *nNumVertices = new long[nFaces];
- for (int i=0;i<nFaces;i++)
- nNumVertices[i] = 6;
- pFaceSet->numVertices.setValues(0,nFaces,(const int32_t*)nNumVertices);
-
- pStoreResult->addChild(pCoords);
- pStoreResult->addChild(pFaceSet);
- delete []pVertexArray;
- delete []nNumVertices;
-
- qhull_mutex.unlock();
-
- return true;
- */
}
-} // namespace Saba
+}
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
index a0c813d19aac84dd2d01e37a69af39a4b2e5beb0..fed82a13c64d18df6fbb2c63d7e4c9f1dea02d58 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
@@ -2,6 +2,7 @@
#include "GraspPlannerWindow.h"
#include "GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.h"
#include "GraspPlanning/ContactConeGenerator.h"
+#include "GraspPlanning/MeshConverter.h"
#include "VirtualRobot/EndEffector/EndEffector.h"
#include "VirtualRobot/Workspace/Reachability.h"
#include "VirtualRobot/ManipulationObject.h"
@@ -302,6 +303,12 @@ void GraspPlannerWindow::loadObject()
object = Obstacle::createBox(50.0f, 50.0f, 10.0f);
}
+#if 0
+ TriMeshModelPtr tm = object->getVisualization()->getTriMeshModel();
+ MathTools::ConvexHull3DPtr cv = ConvexHullGenerator::CreateConvexHull(tm->vertices);
+ object = GraspStudio::MeshConverter::CreateManipulationObject(object->getName(),cv);
+#endif
+
//Eigen::Vector3f minS,maxS;
//object->getCollisionModel()->getTriMeshModel()->getSize(minS,maxS);
//cout << "minS: \n" << minS << "\nMaxS:\n" << maxS << endl;
diff --git a/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.cpp b/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.cpp
index 3829c5ed3b1a70b6017e9623d724512bbed9383c..b5dda9f18207a2510ab3a76add4afd3c470103f1 100644
--- a/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.cpp
+++ b/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.cpp
@@ -444,7 +444,7 @@ void MatGraspPlannerWindow::loadObject()
object = Obstacle::createBox(100.0f, 100.0f, 40.0f);
}
- object = MeshConverter::refineObjectSurface(object, 4.0f);
+ object = MeshConverter::RefineObjectSurface(object, 4.0f);
qualityMeasure.reset(new GraspStudio::GraspQualityMeasureWrenchSpace(object));
@@ -460,7 +460,7 @@ void MatGraspPlannerWindow::loadObjectFromFile(string objectFilename)
if (UI.checkBoxRefineMesh->isChecked())
{
cout << "Refining surface mesh..." << endl;
- object = MeshConverter::refineObjectSurface(object, 4.0f);
+ object = MeshConverter::RefineObjectSurface(object, 4.0f);
cout << "Refining surface mesh: Done." << endl;
}
diff --git a/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.h b/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.h
index 046ed29a7298d2dab2e86f09ce448b538bb69970..b20e44019aec09b053a86fed2b4f7a7f4ffec3a3 100644
--- a/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.h
+++ b/GraspPlanning/examples/MATGraspPlanner/MatGraspPlannerWindow.h
@@ -53,7 +53,7 @@
#include "GraspPlanning/GraspStudio.h"
#include "GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h"
#include "GraspPlanning/GraspPlanner/MATPlanner/MatGraspPlanner.h"
-#include "GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.h"
+#include "GraspPlanning/MeshConverter.h"
#include "GraspPlanning/GraspPlanner/MATPlanner/GraspPlannerConfiguration.h"
class MatGraspPlannerWindow : public QMainWindow