From f0a325f20b200a92a4f2e4b4cb5eb75598cf047c Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Thu, 28 Nov 2013 12:56:53 +0000
Subject: [PATCH] planner
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@443 042f3d55-54a8-47e9-b7fb-15903f145c44
---
.../MATPlanner/CandidateGrasp.cpp | 86 +++
.../GraspPlanner/MATPlanner/CandidateGrasp.h | 70 ++
.../MATPlanner/CandidateGraspGenerator.cpp | 246 ++++++
.../MATPlanner/CandidateGraspGenerator.h | 59 ++
.../MATPlanner/CandidateGraspTester.cpp | 156 ++++
.../MATPlanner/CandidateGraspTester.h | 68 ++
.../CoinVisualization/DrawHelpers.cpp | 366 +++++++++
.../CoinVisualization/DrawHelpers.h | 68 ++
.../GraspPlanner/MATPlanner/Converter.cpp | 82 ++
.../GraspPlanner/MATPlanner/Converter.h | 50 ++
.../MATPlanner/GraspPlannerConfiguration.cpp | 141 ++++
.../MATPlanner/GraspPlannerConfiguration.h | 88 +++
.../MATPlanner/GridOfMedialSpheres.cpp | 434 +++++++++++
.../MATPlanner/GridOfMedialSpheres.h | 91 +++
.../MATPlanner/GridParameters.cpp | 47 ++
.../GraspPlanner/MATPlanner/GridParameters.h | 47 ++
.../MATPlanner/LocalNeighborhood.cpp | 187 +++++
.../MATPlanner/LocalNeighborhood.h | 74 ++
.../MATPlanner/MatGraspPlanner.cpp | 419 ++++++++++
.../GraspPlanner/MATPlanner/MatGraspPlanner.h | 151 ++++
.../GraspPlanner/MATPlanner/MedialSphere.cpp | 115 +++
.../GraspPlanner/MATPlanner/MedialSphere.h | 67 ++
.../GraspPlanner/MATPlanner/MeshConverter.cpp | 227 ++++++
.../GraspPlanner/MATPlanner/MeshConverter.h | 49 ++
.../GraspPlanner/MATPlanner/SphereHelpers.cpp | 280 +++++++
.../GraspPlanner/MATPlanner/SphereHelpers.h | 87 +++
.../GraspPlanner/MATPlanner/StrOutHelpers.cpp | 87 +++
.../GraspPlanner/MATPlanner/StrOutHelpers.h | 50 ++
.../GraspPlanner/MATPlanner/TestCases.cpp | 726 ++++++++++++++++++
.../GraspPlanner/MATPlanner/TestCases.h | 32 +
30 files changed, 4650 insertions(+)
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/CandidateGrasp.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/CandidateGrasp.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspGenerator.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspGenerator.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspTester.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspTester.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/CoinVisualization/DrawHelpers.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/CoinVisualization/DrawHelpers.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/Converter.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/Converter.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/GraspPlannerConfiguration.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/GraspPlannerConfiguration.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/GridOfMedialSpheres.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/GridOfMedialSpheres.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/GridParameters.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/GridParameters.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/LocalNeighborhood.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/LocalNeighborhood.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/MatGraspPlanner.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/MatGraspPlanner.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/MedialSphere.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/MedialSphere.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/SphereHelpers.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/SphereHelpers.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/StrOutHelpers.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/StrOutHelpers.h
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/TestCases.cpp
create mode 100644 GraspPlanning/GraspPlanner/MATPlanner/TestCases.h
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/CandidateGrasp.cpp b/GraspPlanning/GraspPlanner/MATPlanner/CandidateGrasp.cpp
new file mode 100644
index 000000000..9e8773d6a
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/CandidateGrasp.cpp
@@ -0,0 +1,86 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include "CandidateGrasp.h"
+#include "StrOutHelpers.h"
+
+using namespace std;
+using namespace Eigen;
+
+namespace GraspStudio
+{
+
+
+ CandidateGrasp::CandidateGrasp()
+ {
+ graspTargetPoint.setZero();
+ handApproachDirection.setZero();
+ handOrientation.setZero();
+ handPreshape.setZero();
+ candidateGraspType = "None";
+
+ tested = false;
+ finalHandPose.setZero();
+ quality = -1.0;
+
+ }
+
+ void CandidateGrasp::printDebug()
+ {
+ cout << "=== CandidateGrasp: ===" << std::endl;
+ cout << "graspTargetPoint: " << StrOutHelpers::toString(graspTargetPoint) << endl;
+ cout << "handApproachDirection: " << StrOutHelpers::toString(handApproachDirection) << endl;
+ cout << "handOrientation: " << StrOutHelpers::toString(handOrientation) << endl;
+ //cout << "handPreshape: " << StrOutHelpers::toString(handPreshape) << endl;
+ cout << "candidateGraspType: " << candidateGraspType.c_str() << endl;
+ cout << "tested: " << tested << endl;
+
+ if (tested)
+ {
+ // cout << "finalHandPose: " << finalHandPose << endl;
+ //cout << "finalJointAngles: " << StrOutHelpers::toString(finalJointAngles) << endl;
+ // cout << "qualityVolume: " << qualityVolume << endl;
+ cout << "quality: " << quality << endl;
+ }
+
+ }
+
+ Matrix4f CandidateGrasp::toMatrix4f(float positionScaleFactor)
+ {
+ //approach dir now has to point TOWARDS the object (not away from it)
+ Eigen::Vector3f tempHandApproachDir = -1 * handApproachDirection;
+
+ //hand orientation: new y axis
+ //approach direction: new z axis
+ //new x axis: cross(y,z)
+
+ Matrix4f cgAsMatrix;
+ cgAsMatrix.setZero();
+ cgAsMatrix.block<3, 1>(0, 1) = handOrientation;
+ cgAsMatrix.block<3, 1>(0, 2) = tempHandApproachDir;
+ cgAsMatrix.block<3, 1>(0, 0) = handOrientation.cross(tempHandApproachDir);
+ cgAsMatrix.block<3, 1>(0, 3) = positionScaleFactor * graspTargetPoint;
+ cgAsMatrix(3, 3) = 1.0f;
+
+ return cgAsMatrix;
+ }
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/CandidateGrasp.h b/GraspPlanning/GraspPlanner/MATPlanner/CandidateGrasp.h
new file mode 100644
index 000000000..5740237e7
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/CandidateGrasp.h
@@ -0,0 +1,70 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#ifndef CANDIDATEGRASP_H
+#define CANDIDATEGRASP_H
+
+#include <VirtualRobot\EndEffector\EndEffector.h>
+#include "../../GraspStudio.h"
+#include <Eigen/Geometry>
+#include "MedialSphere.h"
+#include "LocalNeighborhood.h"
+
+namespace GraspStudio
+{
+
+ class CandidateGrasp;
+ typedef boost::shared_ptr<CandidateGrasp> CandidateGraspPtr;
+
+ class GRASPSTUDIO_IMPORT_EXPORT CandidateGrasp
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+
+ CandidateGrasp();
+ bool tested;
+ bool forceClosure;
+ float quality;
+ VirtualRobot::EndEffector::ContactInfoVector contacts;
+ Eigen::Vector3f graspTargetPoint;
+ Eigen::Vector3f handApproachDirection;
+ Eigen::Vector3f handOrientation;
+ Eigen::VectorXf handPreshape;
+
+ std::string candidateGraspType;
+ MedialSpherePtr medialSphere;
+ LocalNeighborhoodPtr localNeighborhood;
+
+
+
+ Eigen::Matrix4f finalHandPose;
+ VirtualRobot::RobotConfigPtr finalJointAngles;
+
+
+ void printDebug();
+
+ Eigen::Matrix4f toMatrix4f(float positionScaleFactor = 1.0);
+ };
+
+}
+#endif // CANDIDATEGRASP_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspGenerator.cpp b/GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspGenerator.cpp
new file mode 100644
index 000000000..53faf7474
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspGenerator.cpp
@@ -0,0 +1,246 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include "CandidateGraspGenerator.h"
+
+using namespace std;
+namespace GraspStudio
+{
+
+
+ CandidateGraspGenerator::CandidateGraspGenerator()
+ {
+ verbose = false;
+ }
+
+ CandidateGraspGenerator::CandidateGraspGenerator(bool verbose)
+ {
+ this->verbose = verbose;
+ }
+
+
+ std::vector<CandidateGraspPtr> CandidateGraspGenerator::generateCandidateGrasps(LocalNeighborhoodPtr nbhd, GraspPlannerConfigurationPtr gpConfig)
+ {
+ //TODO: (maybe) don't generate candidate grasps for very small spheres
+
+ std::vector<CandidateGraspPtr> candidates;
+
+ if ((2.0 * nbhd->centerSphere->radius > gpConfig->maxGraspDiameterOfCurrentRobotHand)
+ && (gpConfig->maxGraspDiameterOfCurrentRobotHand > 0.0))
+ //sphere too big to grasp
+ {
+ if (verbose)
+ VR_INFO << "CandidateGraspGenerator::generateCandidateGrasps(): sphere radius "
+ << nbhd->centerSphere->radius
+ << " too big to grasp with current robot hand, maxGraspDiameterOfCurrentRobotHand: "
+ << gpConfig->maxGraspDiameterOfCurrentRobotHand << endl;
+ return candidates;
+ }
+
+ if (nbhd->isEmpty)
+ return candidates;
+
+ if (nbhd->ratioOfEigenvalues <= gpConfig->thresholdSymmetryAxis)
+ {
+ candidates = generateSymmetryAxisOrthogonalCandidateGrasps(nbhd, gpConfig);
+ }
+ else
+ {
+ if (nbhd->ratioOfEigenvalues <= gpConfig->thresholdSymmetryPlane)
+ {
+ candidates = generateSymmetryPlaneParallelCandidateGrasps(nbhd, gpConfig);
+ }
+ else
+ {
+ if (verbose)
+ VR_INFO << "CandidateGraspGenerator::generateCandidateGrasps(): ratioOfEigenvalues: "
+ << nbhd->ratioOfEigenvalues
+ << " -> target sphere located inside plane, skip generating approach directions."
+ << endl;
+ }
+ }
+
+ return candidates;
+ }
+
+ std::vector<CandidateGraspPtr> CandidateGraspGenerator::generateSymmetryAxisOrthogonalCandidateGrasps(LocalNeighborhoodPtr nbhd, GraspPlannerConfigurationPtr gpConfig)
+ {
+ std::vector<CandidateGraspPtr> candidates;
+
+ Eigen::Vector3f symAxisDirection = nbhd->eigenvector1;
+ int numApproachDirections = gpConfig->numberOfApproachDirectionsForLocalSymmetryAxis;
+
+ float radius = 1;
+ std::vector<Eigen::Vector3f> pointsOnCircle = generatePointsOnCircle(nbhd->centerSphere->center,
+ symAxisDirection, numApproachDirections, radius);
+
+ for (size_t i = 0; i < pointsOnCircle.size(); i++)
+ {
+ CandidateGraspPtr cg = CandidateGraspPtr(new CandidateGrasp);
+ cg->graspTargetPoint = nbhd->centerSphere->center;
+ cg->handApproachDirection = pointsOnCircle.at(i) - nbhd->centerSphere->center;
+ cg->handOrientation = symAxisDirection;
+ cg->candidateGraspType = "symmetryAxisOrthogonal";
+
+ cg->medialSphere = nbhd->centerSphere;
+ cg->localNeighborhood = nbhd;
+ candidates.push_back(cg);
+ }
+
+ if (gpConfig->generateFlippedCandidates)
+ {
+ for (size_t i = 0; i < pointsOnCircle.size(); i++)
+ {
+ CandidateGraspPtr cg = CandidateGraspPtr(new CandidateGrasp);
+ cg->graspTargetPoint = nbhd->centerSphere->center;
+ cg->handApproachDirection = pointsOnCircle.at(i) - nbhd->centerSphere->center;
+ cg->handOrientation = -1 * symAxisDirection;
+ cg->candidateGraspType = "symmetryAxisOrthogonal";
+
+ cg->medialSphere = nbhd->centerSphere;
+ cg->localNeighborhood = nbhd;
+ candidates.push_back(cg);
+ }
+ }
+
+ return candidates;
+ }
+
+ std::vector<CandidateGraspPtr> CandidateGraspGenerator::generateSymmetryPlaneParallelCandidateGrasps(LocalNeighborhoodPtr nbhd, GraspPlannerConfigurationPtr gpConfig)
+ {
+ std::vector<CandidateGraspPtr> candidates;
+
+ Eigen::Vector3f planeDirection0 = nbhd->eigenvector1;
+ Eigen::Vector3f planeDirection1 = nbhd->eigenvector2;
+
+ Eigen::Vector3f temporaryApproachDirection = planeDirection1;
+ Eigen::Vector3f finalApproachDirection;
+
+ Eigen::Vector3f directionToCoG = nbhd->centerOfGravity - nbhd->center;
+ float angleDeg = SphereHelpers::calcAngleBetweenTwoVectorsDeg(temporaryApproachDirection,
+ directionToCoG);
+ if (angleDeg <= 90)
+ finalApproachDirection = -1 * temporaryApproachDirection;
+ else
+ finalApproachDirection = temporaryApproachDirection;
+
+ CandidateGraspPtr cg = CandidateGraspPtr(new CandidateGrasp);
+ cg->graspTargetPoint = nbhd->centerSphere->center;
+ cg->handApproachDirection = finalApproachDirection;
+ cg->handOrientation = planeDirection0;
+ cg->candidateGraspType = "symmetryPlaneParallel";
+
+ cg->medialSphere = nbhd->centerSphere;
+ cg->localNeighborhood = nbhd;
+ candidates.push_back(cg);
+
+ if (gpConfig->generateFlippedCandidates)
+ {
+ CandidateGraspPtr cgFlipped = CandidateGraspPtr(new CandidateGrasp);
+ cgFlipped->graspTargetPoint = nbhd->centerSphere->center;
+ cgFlipped->handApproachDirection = finalApproachDirection;
+ cgFlipped->handOrientation = -1 * planeDirection0;
+ cgFlipped->candidateGraspType = "symmetryPlaneParallel";
+
+ cgFlipped->medialSphere = nbhd->centerSphere;
+ cgFlipped->localNeighborhood = nbhd;
+ candidates.push_back(cgFlipped);
+ }
+
+ return candidates;
+ }
+
+
+ std::vector<Eigen::Vector3f> CandidateGraspGenerator::generatePointsOnCircle(Eigen::Vector3f circleCenter, Eigen::Vector3f rotationAxis, int numberOfPoints, float radius)
+ {
+ std::vector<Eigen::Vector3f> pointsOnCircle;
+
+ Eigen::Vector3f xAxis(1, 0, 0);
+ Eigen::Vector3f yAxis(0, 1, 0);
+ Eigen::Vector3f zAxis(0, 0, 1);
+
+ Eigen::Vector3f xAxisNew;
+ Eigen::Vector3f yAxisNew;
+ Eigen::Vector3f zAxisNew;
+
+ //calculate a new coordinate system where rotationAxis is the new z axis.
+ if ((zAxis - rotationAxis).norm() > 1e-12)
+ {
+ xAxisNew = zAxis.cross(rotationAxis);
+ yAxisNew = xAxisNew.cross(rotationAxis);
+ zAxisNew = rotationAxis;
+
+ float xAxisNewNorm = xAxisNew.norm();
+ float yAxisNewNorm = yAxisNew.norm();
+ float zAxisNewNorm = zAxisNew.norm();
+
+ //dangerous (Norm 0)
+ if ((xAxisNewNorm < 1e-12) || (yAxisNewNorm < 1e-12) || (zAxisNewNorm < 1e-12) || verbose)
+ {
+ VR_WARNING << "-----> WARNING: generatePointsOnCircle(): norms of new coordinate frame axes are close to zero; skipping..." << endl;
+ }
+ else
+ {
+ xAxisNew = xAxisNew / xAxisNewNorm;
+ yAxisNew = yAxisNew / yAxisNewNorm;
+ zAxisNew = zAxisNew / zAxisNewNorm;
+ }
+ if (verbose)
+ VR_INFO << "generatePointsOnCircle(): NEW coord system" << endl;
+
+ }
+ else
+ {
+ xAxisNew = xAxis;
+ yAxisNew = yAxis;
+ zAxisNew = zAxis;
+
+ if (verbose)
+ VR_INFO << "generatePointsOnCircle(): did not change coord system." << endl;
+ }
+
+ //calculate points on a circle around rotationAxis
+ std::vector<float> polarAngles;
+ float angleStepRad = (float)(2.0f * M_PI / (float)numberOfPoints);
+
+ for (int k = 0; k < numberOfPoints; k++)
+ polarAngles.push_back(k * angleStepRad);
+
+ for (int i = 0; i < numberOfPoints; i++)
+ {
+ float phi = polarAngles.at(i);
+ Eigen::Vector3f pointCoordsCartesian = circleCenter + radius * xAxisNew * cos(phi)
+ + radius * yAxisNew * sin(phi);
+ pointsOnCircle.push_back(pointCoordsCartesian);
+ }
+
+ if (verbose)
+ {
+ VR_INFO << "generatePointsOnCircle(): points on circle:" << endl;
+ VR_INFO << StrOutHelpers::toString(pointsOnCircle) << endl;
+ }
+
+ return pointsOnCircle;
+ }
+
+}
+
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspGenerator.h b/GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspGenerator.h
new file mode 100644
index 000000000..11207a7a9
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspGenerator.h
@@ -0,0 +1,59 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#ifndef CANDIDATEGRASPGENERATOR_H
+#define CANDIDATEGRASPGENERATOR_H
+
+
+#include <vector>
+#include "../../GraspStudio.h"
+#include "CandidateGrasp.h"
+#include "LocalNeighborhood.h"
+#include "GraspPlannerConfiguration.h"
+
+namespace GraspStudio
+{
+
+ class CandidateGraspGenerator;
+ typedef boost::shared_ptr<CandidateGraspGenerator> CandidateGraspGeneratorPtr;
+
+ class GRASPSTUDIO_IMPORT_EXPORT CandidateGraspGenerator
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ CandidateGraspGenerator();
+ CandidateGraspGenerator(bool verbose);
+
+ std::vector<CandidateGraspPtr> generateCandidateGrasps(LocalNeighborhoodPtr nbhd, GraspPlannerConfigurationPtr gpConfig);
+ std::vector<CandidateGraspPtr> generateSymmetryAxisOrthogonalCandidateGrasps(LocalNeighborhoodPtr nbhd, GraspPlannerConfigurationPtr gpConfig);
+ std::vector<CandidateGraspPtr> generateSymmetryPlaneParallelCandidateGrasps(LocalNeighborhoodPtr nbhd, GraspPlannerConfigurationPtr gpConfig);
+
+ private:
+ std::vector<Eigen::Vector3f> generatePointsOnCircle(Eigen::Vector3f circleCenter, Eigen::Vector3f rotationAxis, int numberOfPoints, float radius);
+
+ bool verbose;
+
+ };
+
+}
+#endif // CANDIDATEGRASPGENERATOR_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspTester.cpp b/GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspTester.cpp
new file mode 100644
index 000000000..3d3c696ef
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspTester.cpp
@@ -0,0 +1,156 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include "CandidateGraspTester.h"
+#include "../GraspQuality/GraspQualityMeasure.h"
+#include "../GraspQuality/GraspQualityMeasureWrenchSpace.h"
+#include <VirtualRobot\Robot.h>
+#include <VirtualRobot\EndEffector\EndEffector.h>
+
+using namespace std;
+using namespace Eigen;
+
+namespace GraspStudio
+{
+
+ CandidateGraspTester::CandidateGraspTester(VirtualRobot::RobotPtr robot, VirtualRobot::SceneObjectPtr object,
+ std::string eefName, GraspQualityMeasureWrenchSpacePtr qualityMeasure, bool verbose)
+ :robot(robot), object(object), eefName(eefName), qualityMeasure(qualityMeasure), verbose(verbose)
+ {
+ if (!robot)
+ {
+ VR_ERROR << "no robot" << endl;
+ return;
+ }
+
+ eef = robot->getEndEffector(eefName);
+
+ if (!eef)
+ {
+ VR_ERROR << "FAILED to get eef with name " << eefName << endl;
+ return;
+ }
+
+ eefRobot = eef->createEefRobot(eef->getName(), eef->getName());
+ eefCloned = eefRobot->getEndEffector(eef->getName());
+ }
+
+ void CandidateGraspTester::setEEF(Eigen::Matrix4f &poseGCP)
+ {
+ if (!eefRobot || !eefCloned)
+ return;
+ eefRobot->setGlobalPoseForRobotNode(eefCloned->getGCP(), poseGCP);
+ }
+
+ void CandidateGraspTester::testCandidate(CandidateGraspPtr candidate)
+ {
+ //set hand (eef) to start pose:
+ float positionScaleFactor = 1000.0; //ACHTUNG: als Argument...!
+ Eigen::Matrix4f poseGCP = candidate->toMatrix4f(positionScaleFactor);
+ setEEF(poseGCP);
+
+ openEEF();
+ closeEEFAndTest(candidate, positionScaleFactor);
+ }
+
+ void CandidateGraspTester::moveEEFAway(const Eigen::Vector3f &approachDir, float step, int maxLoops)
+ {
+ VirtualRobot::SceneObjectSetPtr sos = eefCloned->createSceneObjectSet();
+ if (!sos)
+ return;
+ int loop = 0;
+ Eigen::Vector3f delta = approachDir * step;
+ while (loop < maxLoops && eefCloned->getCollisionChecker()->checkCollision(object->getCollisionModel(), sos))
+ {
+ updateEEFPose(delta);
+ loop++;
+ }
+ }
+
+ bool CandidateGraspTester::updateEEFPose(const Eigen::Vector3f &deltaPosition)
+ {
+ Eigen::Matrix4f deltaPose;
+ deltaPose.setIdentity();
+ deltaPose.block(0, 3, 3, 1) = deltaPosition;
+ return updateEEFPose(deltaPose);
+ }
+
+ bool CandidateGraspTester::updateEEFPose(const Eigen::Matrix4f &deltaPose)
+ {
+ Eigen::Matrix4f pose = eefCloned->getGCP()->getGlobalPose();
+ pose = deltaPose * pose;
+ eefRobot->setGlobalPoseForRobotNode(eefCloned->getGCP(), pose);
+ return true;
+ }
+
+ void CandidateGraspTester::closeEEFAndTest(CandidateGraspPtr candidate, float positionScaleFactor)
+ {
+ if (!eefCloned || !qualityMeasure)
+ return;
+
+ //cout << "todo: moveaway..." << endl;
+ moveEEFAway(candidate->handApproachDirection,3.0f,5); //MP 2013-10-01
+
+ candidate->contacts.clear();
+
+ if (eefCloned && eefRobot)
+ {
+ candidate->contacts = eefCloned->closeActors(object);
+ qualityMeasure->setContactPoints(candidate->contacts);
+
+ candidate->quality = qualityMeasure->getGraspQuality();
+ candidate->forceClosure = qualityMeasure->isGraspForceClosure();
+ if (verbose)
+ {
+ std::stringstream ss;
+ ss << std::setprecision(3);
+ ss << "Quality (wrench space): "
+ << candidate->quality << "\nForce closure: ";
+ if (candidate->forceClosure)
+ ss << "yes";
+ else
+ ss << "no";
+ cout << ss.str();
+ }
+
+ }
+
+ candidate->tested = true;
+
+ candidate->finalHandPose = eefCloned->getGCP()->getGlobalPose();
+ candidate->finalJointAngles = eefCloned->getConfiguration();
+ }
+
+
+
+ void CandidateGraspTester::openEEF()
+ {
+ if (!eefCloned)
+ return;
+ if (eefCloned && eefRobot)
+ {
+ eefCloned->openActors();
+ }
+ }
+
+
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspTester.h b/GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspTester.h
new file mode 100644
index 000000000..55826b423
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/CandidateGraspTester.h
@@ -0,0 +1,68 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#ifndef CANDIDATEGRASPTESTER_H
+#define CANDIDATEGRASPTESTER_H
+
+#include "../../GraspStudio.h"
+#include "CandidateGrasp.h"
+#include <vector>
+#include "MedialSphere.h"
+
+#include "powercrust.h"
+
+namespace GraspStudio
+{
+ class CandidateGraspTester;
+ typedef boost::shared_ptr<CandidateGraspTester> CandidateGraspTesterPtr;
+
+
+ class GRASPSTUDIO_IMPORT_EXPORT CandidateGraspTester
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ CandidateGraspTester(VirtualRobot::RobotPtr robot, VirtualRobot::SceneObjectPtr object,
+ std::string eefName, GraspQualityMeasureWrenchSpacePtr qualityMeasure, bool verbose);
+
+ void testCandidate(CandidateGraspPtr candidate);
+
+ protected:
+ void closeEEFAndTest(CandidateGraspPtr candidate, float positionScaleFactor);
+ void openEEF();
+ void setEEF(Eigen::Matrix4f &poseGCP);
+ void moveEEFAway(const Eigen::Vector3f &approachDir, float step, int maxLoops);
+ bool updateEEFPose(const Eigen::Vector3f &deltaPosition);
+ bool updateEEFPose(const Eigen::Matrix4f &deltaPose);
+
+ VirtualRobot::RobotPtr robot;
+ VirtualRobot::SceneObjectPtr object;
+ VirtualRobot::EndEffectorPtr eef;
+ VirtualRobot::RobotPtr eefRobot;
+ VirtualRobot::EndEffectorPtr eefCloned;
+ std::string eefName;
+ GraspQualityMeasureWrenchSpacePtr qualityMeasure;
+ bool verbose;
+ };
+}
+
+#endif // CANDIDATEGRASPTESTER_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/CoinVisualization/DrawHelpers.cpp b/GraspPlanning/GraspPlanner/MATPlanner/CoinVisualization/DrawHelpers.cpp
new file mode 100644
index 000000000..71371ad3f
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/CoinVisualization/DrawHelpers.cpp
@@ -0,0 +1,366 @@
+#include <iostream>
+
+#include "DrawHelpers.h"
+
+using namespace std;
+using namespace Eigen;
+
+namespace GraspStudio
+{
+
+
+ DrawHelpers::DrawHelpers()
+ {
+ }
+
+ SoSeparator* DrawHelpers::DrawSingleSphere(Eigen::Vector3f& position, float radius, float colorR, float colorG, float colorB, float transparency)
+ {
+ SoSeparator *s = new SoSeparator();
+ s->ref();
+
+ SoTranslation *tr = new SoTranslation();
+ tr->translation.setValue(position(0), position(1), position(2));
+ s->addChild(tr);
+
+ SoUnits *u = new SoUnits();
+ //u->units = SoUnits::MILLIMETERS;
+ u->units = SoUnits::METERS;
+ s->addChild(u);
+
+ SoMaterial *m = new SoMaterial();
+ s->addChild(m);
+ //m->ambientColor.setValue(colorR,colorG,colorB);
+ m->ambientColor.setValue(0, 0, 0);
+ m->diffuseColor.setValue(colorR, colorG, colorB);
+ m->transparency.setValue(transparency);
+
+ SoSphere *c = new SoSphere();
+ s->addChild(c);
+ c->radius = radius;
+
+ return s;
+ }
+
+ SoSeparator* DrawHelpers::DrawSingleSphere(MedialSpherePtr ms, float colorR, float colorG, float colorB, float transparency)
+ {
+ SoSeparator* sep = DrawSingleSphere(ms->center, ms->radius, colorR, colorG, colorB, transparency);
+ return sep;
+ }
+
+ SoSeparator* DrawHelpers::DrawSinglePoint(MedialSpherePtr ms, float colorR, float colorG, float colorB, float transparency, float pointSize)
+ {
+ SoSeparator* sep = DrawSingleSphere(ms->center, pointSize, colorR, colorG, colorB, transparency);
+ return sep;
+ }
+
+ SoSeparator* DrawHelpers::DrawSinglePoint(Eigen::Vector3f& position, float colorR, float colorG, float colorB, float transparency, float pointSize)
+ {
+ SoSeparator* sep = DrawSingleSphere(position, pointSize, colorR, colorG, colorB, transparency);
+ return sep;
+ }
+
+ SoSeparator* DrawHelpers::DrawPointCloud(std::vector<MedialSpherePtr>& spheres, float colorR, float colorG, float colorB, float transparency, float pointSize)
+ {
+ SoSeparator *sphereVectorSep = new SoSeparator();
+ sphereVectorSep->ref();
+
+ for (size_t i = 0; i < spheres.size(); i++)
+ {
+ SoSeparator* sphereSep = DrawSinglePoint(spheres.at(i), colorR, colorG, colorB, transparency, pointSize);
+ sphereVectorSep->addChild(sphereSep);
+ }
+
+ return sphereVectorSep;
+ }
+
+ SoSeparator* DrawHelpers::DrawPointCloud(std::vector<Eigen::Vector3f>& points, float colorR, float colorG, float colorB, float transparency, float pointSize)
+ {
+ SoSeparator *sphereVectorSep = new SoSeparator();
+ sphereVectorSep->ref();
+
+ for (size_t i = 0; i < points.size(); i++)
+ {
+ SoSeparator* sphereSep = DrawSinglePoint(points.at(i), colorR, colorG, colorB, transparency, pointSize);
+ sphereVectorSep->addChild(sphereSep);
+ }
+
+ return sphereVectorSep;
+ }
+
+ SoSeparator* DrawHelpers::DrawVectorOfSpheres(std::vector<MedialSpherePtr>& spheres, float maxRadius, float transparency)
+ {
+ float kFactorForColor = 1.1f;
+ float maxValue = maxRadius; //biggest sphere of the MAT
+
+ SoSeparator *sphereVectorSep = new SoSeparator();
+ sphereVectorSep->ref();
+
+ for (size_t i = 0; i < spheres.size(); i++)
+ {
+ MedialSpherePtr currentSphere = spheres.at(i);
+ Vector3f currentColor = DrawHelpers::getColorFromTable(currentSphere->radius, kFactorForColor, maxValue);
+
+ SoSeparator* sphereSep = DrawSingleSphere(currentSphere, currentColor(0), currentColor(1), currentColor(2), transparency);
+
+ sphereVectorSep->addChild(sphereSep);
+ }
+
+ return sphereVectorSep;
+ }
+
+ SoSeparator* DrawHelpers::DrawSingleLine(Eigen::Vector3f from, Eigen::Vector3f to, float width, float colorR, float colorG, float colorB)
+ {
+ SoSeparator *s = new SoSeparator();
+ s->ref();
+ SoUnits *u = new SoUnits();
+ //u->units = SoUnits::MILLIMETERS;
+ u->units = SoUnits::METERS; //MP 2013-09-13 Test...
+ s->addChild(u);
+
+ SoMaterial *m = new SoMaterial();
+ s->addChild(m);
+ m->ambientColor.setValue(colorR, colorG, colorB);
+ m->diffuseColor.setValue(colorR, colorG, colorB);
+
+ // create line
+ float x = from(0);
+ float y = from(1);
+ float z = from(2);
+ float x2 = to(0);
+ float y2 = to(1);
+ float z2 = to(2);
+
+ SbVec3f points[2];
+ points[0].setValue(x2, y2, z2);
+ points[1].setValue(x, y, z);
+
+ SoDrawStyle *lineSolutionStyle = new SoDrawStyle();
+ lineSolutionStyle->lineWidth.setValue(width);
+ s->addChild(lineSolutionStyle);
+
+ SoCoordinate3* coordinate3 = new SoCoordinate3;
+ coordinate3->point.set1Value(0, points[0]);
+ coordinate3->point.set1Value(1, points[1]);
+ s->addChild(coordinate3);
+
+ SoLineSet* lineSet = new SoLineSet;
+ lineSet->numVertices.setValue(2);
+ lineSet->startIndex.setValue(0);
+ s->addChild(lineSet);
+ s->unrefNoDelete();
+
+ return s;
+ }
+
+ SoSeparator* DrawHelpers::DrawLocalNeighborhood(LocalNeighborhoodPtr neighborhood, bool drawEigenvectors, bool drawCenterOfGravity, bool drawSearchRadius, float pointSize, float scaleValue)
+ {
+ SoSeparator* neighborhoodSep = new SoSeparator();
+ neighborhoodSep->ref();
+
+ //1. Draw the center of the query sphere:
+ float kFactorForColor = 1.1f;
+ float maxValue = 1.0; //maximum possible value of ratioOfEigenvalues
+ float pointTransparency = 0.0;
+ Vector3f centerColor = DrawHelpers::getColorFromTable(neighborhood->ratioOfEigenvalues,
+ kFactorForColor, maxValue);
+ SoSeparator* centerSep = DrawSinglePoint(neighborhood->center,
+ centerColor[0], centerColor[1], centerColor[2],
+ pointTransparency, pointSize);
+ neighborhoodSep->addChild(centerSep);
+
+ //2. Draw eigenvectors
+ if (drawEigenvectors)
+ {
+ Eigen::Vector3f endPoint1, endPoint2;
+ float lineWidth = 2.0;
+ endPoint1 = neighborhood->center + scaleValue * neighborhood->eigenvector1;
+ endPoint2 = neighborhood->center + scaleValue * neighborhood->eigenvector2;
+
+ SoSeparator* sepLine1 = DrawHelpers::DrawSingleLine(neighborhood->center,
+ endPoint1, lineWidth, 1.0, 0.0, 0.0);
+ neighborhoodSep->addChild(sepLine1);
+ SoSeparator* sepLine2 = DrawHelpers::DrawSingleLine(neighborhood->center,
+ endPoint2, lineWidth, 0.0, 1.0, 0.0);
+ neighborhoodSep->addChild(sepLine2);
+ }
+
+ //3. Draw center of gravity (cog)
+ if (drawCenterOfGravity)
+ {
+ //center of gravity
+ SoSeparator* cogSep = DrawSinglePoint(neighborhood->centerOfGravity,
+ 0.0f, 0.0f, 1.0f,
+ pointTransparency, 0.5f*pointSize);
+ neighborhoodSep->addChild(cogSep);
+
+ //line from sphere center to cog
+ float cogLineWidth = 1.0;
+ SoSeparator* sepCogLine = DrawHelpers::DrawSingleLine(
+ neighborhood->centerOfGravity, neighborhood->center,
+ cogLineWidth, 0.0, 0.0, 1.0);
+ neighborhoodSep->addChild(sepCogLine);
+ }
+
+ //4. Draw search radius (as a transparent sphere)
+ if (drawSearchRadius)
+ {
+ float neighborhoodTransparency = 0.975f;
+ SoSeparator* searchRadiusSep = DrawSingleSphere(neighborhood->center,
+ neighborhood->radius, 0.0f, 0.2f, 0.4f, neighborhoodTransparency);
+ neighborhoodSep->addChild(searchRadiusSep);
+ }
+
+ return neighborhoodSep;
+ }
+
+ SoSeparator* DrawHelpers::DrawCandidateGrasp(CandidateGraspPtr cg, float scaleValue)
+ {
+ Eigen::Vector3f startPoint, orientationPoint;
+ float lineWidth = 1.0;
+ startPoint = cg->graspTargetPoint + scaleValue * cg->handApproachDirection;
+ orientationPoint = startPoint + 0.15f * scaleValue * cg->handOrientation;
+ Eigen::Vector3f approachColor;
+ Eigen::Vector3f orientationColor;
+
+ if (cg->candidateGraspType.compare("symmetryAxisOrthogonal") == 0)
+ {
+ approachColor << 1.0f, 0.35f, 0.15f; //orange
+ }
+ else
+ {
+ if (cg->candidateGraspType.compare("symmetryPlaneParallel") == 0)
+ {
+ approachColor << 0.0f, 0.7f, 0.3f; //green
+ }
+ else
+ {
+ approachColor << 0.0f, 0.0f, 0.0f; //black
+ }
+ }
+
+ orientationColor << 1.0f, 0.0f, 0.5f; //magenta
+
+ SoSeparator* cgSep = new SoSeparator();
+ cgSep->ref();
+
+ //approach direction
+ SoSeparator* sepLine1 = DrawHelpers::DrawSingleLine(cg->graspTargetPoint,
+ startPoint, lineWidth, approachColor[0],
+ approachColor[1], approachColor[2]);
+ cgSep->addChild(sepLine1);
+
+ //hand orientation
+ SoSeparator* sepLine2 = DrawHelpers::DrawSingleLine(startPoint,
+ orientationPoint, lineWidth,
+ orientationColor[0], orientationColor[1], orientationColor[2]);
+ cgSep->addChild(sepLine2);
+
+ return cgSep;
+ }
+
+ SoSeparator* DrawHelpers::DrawLocalNeighborhoods(std::vector<LocalNeighborhoodPtr>& neighborhoods, bool drawEigenvectors, bool drawCenterOfGravity, bool drawSearchRadius, float pointSize, float scaleValue)
+ {
+ SoSeparator* sepNeighborhoods = new SoSeparator;
+ sepNeighborhoods->ref();
+
+ for (int i = 0; i < (int)neighborhoods.size(); i++)
+ {
+ SoSeparator* sepCurrentNbhd =
+ DrawHelpers::DrawLocalNeighborhood(neighborhoods.at(i),
+ drawEigenvectors, drawCenterOfGravity, drawSearchRadius,
+ pointSize, scaleValue);
+ sepNeighborhoods->addChild(sepCurrentNbhd);
+ }
+
+ return sepNeighborhoods;
+ }
+
+
+ SoSeparator* DrawHelpers::DrawSearchRadius(LocalNeighborhoodPtr neighborhood)
+ {
+ float neighborhoodTransparency = 0.99f;
+ SoSeparator* searchRadiusSep = DrawSingleSphere(neighborhood->center,
+ neighborhood->radius, 0.0f, 0.2f, 0.4f,
+ neighborhoodTransparency);
+ return searchRadiusSep;
+ }
+
+ SoSeparator* DrawHelpers::DrawSearchRadii(std::vector<LocalNeighborhoodPtr> neighborhoods)
+ {
+ SoSeparator* sepRadii = new SoSeparator;
+ sepRadii->ref();
+
+ for (size_t i = 0; i < neighborhoods.size(); i++)
+ {
+ SoSeparator* sepCurrentNbhd =
+ DrawHelpers::DrawSearchRadius(neighborhoods.at(i));
+ sepRadii->addChild(sepCurrentNbhd);
+ }
+ return sepRadii;
+ }
+
+ SoSeparator* DrawHelpers::DrawCandidateGrasps(std::vector<CandidateGraspPtr>& cg, float scaleValue)
+ {
+ SoSeparator* sepCandidates = new SoSeparator;
+ sepCandidates->ref();
+
+ for (size_t i = 0; i < cg.size(); i++)
+ {
+ SoSeparator* sepCurrentCandidate =
+ DrawHelpers::DrawCandidateGrasp(cg.at(i),
+ scaleValue);
+ sepCandidates->addChild(sepCurrentCandidate);
+ }
+
+ return sepCandidates;
+ }
+
+ Eigen::Vector3f DrawHelpers::getColorFromTable(float currentValue, float kFactor, float maxInputValue)
+ {
+ //kFactor: choose values of maybe 1.1 or 1.2 to avoid spheres of max radius to be drawn white...
+
+ //Maximum value to be handled by this color table
+ float maxColorValue = kFactor * maxInputValue;
+
+ float R;
+ float G;
+ float B;
+
+ if ((currentValue >= 0) && (currentValue <= 0.25 * maxColorValue))
+ {
+ R = 4 * (currentValue / maxColorValue);
+ G = 0;
+ B = 0;
+ }
+ else if ((currentValue > 0.25 * maxColorValue) && (currentValue <= 0.5 * maxColorValue))
+ {
+ R = -4 * (currentValue / maxColorValue) + 2;
+ G = 4 * (currentValue / maxColorValue) - 1;
+ B = 0;
+ }
+ else if ((currentValue > 0.5 * maxColorValue) && (currentValue <= 0.75 * maxColorValue))
+ {
+ R = 0;
+ G = -4 * (currentValue / maxColorValue) + 3;
+ B = 4 * (currentValue / maxColorValue) - 2;
+ }
+ else if ((currentValue > 0.75 * maxColorValue) && (currentValue <= maxColorValue))
+ {
+ R = 0;
+ G = 0;
+ B = -4 * (currentValue / maxColorValue) + 4;
+ }
+ else
+ {
+ VR_ERROR << "getColorFromTable(): OUTSIDE expected range of values!: "
+ << currentValue << endl;
+ R = 0.7f;
+ G = 0;
+ B = 0.7f;
+ }
+
+ Eigen::Vector3f color(R, G, B);
+ return color;
+ }
+
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/CoinVisualization/DrawHelpers.h b/GraspPlanning/GraspPlanner/MATPlanner/CoinVisualization/DrawHelpers.h
new file mode 100644
index 000000000..cad2ccea9
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/CoinVisualization/DrawHelpers.h
@@ -0,0 +1,68 @@
+#ifndef DRAWHELPERS_H
+#define DRAWHELPERS_H
+
+
+#include <Eigen/Geometry>
+
+#include <Inventor/nodes/SoTranslation.h>
+#include <Inventor/nodes/SoUnits.h>
+#include <Inventor/nodes/SoSphere.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/nodes/SoLineSet.h>
+#include <Inventor/nodes/SoDrawStyle.h>
+#include <Inventor/nodes/SoCoordinate3.h>
+
+#include <vector>
+#include "../../../GraspStudio.h"
+#include "../MedialSphere.h"
+#include "../LocalNeighborhood.h"
+#include "../StrOutHelpers.h"
+#include "../CandidateGrasp.h"
+
+namespace GraspStudio
+{
+ class GRASPSTUDIO_IMPORT_EXPORT DrawHelpers
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ DrawHelpers();
+
+ static SoSeparator* DrawSingleSphere(Eigen::Vector3f& position, float radius, float colorR, float colorG, float colorB, float transparency);
+
+ static SoSeparator* DrawSingleSphere(MedialSpherePtr ms, float colorR, float colorG, float colorB, float transparency);
+
+ static SoSeparator* DrawSinglePoint(MedialSpherePtr ms, float colorR, float colorG, float colorB, float transparency, float pointSize);
+
+ static SoSeparator* DrawSinglePoint(Eigen::Vector3f& position, float colorR, float colorG, float colorB, float transparency, float pointSize);
+
+ static SoSeparator* DrawVectorOfSpheres(std::vector<MedialSpherePtr>& spheres, float maxRadius, float transparency);
+
+ static SoSeparator* DrawPointCloud(std::vector<MedialSpherePtr>& spheres, float colorR, float colorG, float colorB, float transparency, float pointSize);
+
+ static SoSeparator* DrawPointCloud(std::vector<Eigen::Vector3f>& points, float colorR, float colorG, float colorB, float transparency, float pointSize);
+
+ static SoSeparator* DrawSingleLine(Eigen::Vector3f from, Eigen::Vector3f to, float width, float colorR, float colorG, float colorB);
+
+ static SoSeparator* DrawLocalNeighborhood(LocalNeighborhoodPtr neighborhood,
+ bool drawEigenvectors, bool drawCenterOfGravity, bool drawSearchRadius,
+ float pointSize, float scaleValue);
+
+ static SoSeparator* DrawLocalNeighborhoods(std::vector<LocalNeighborhoodPtr>& neighborhoods,
+ bool drawEigenvectors, bool drawCenterOfGravity, bool drawSearchRadius,
+ float pointSize, float scaleValue);
+
+ static SoSeparator* DrawSearchRadius(LocalNeighborhoodPtr neighborhood);
+ static SoSeparator* DrawSearchRadii(std::vector<LocalNeighborhoodPtr> neighborhoods);
+
+ static SoSeparator* DrawCandidateGrasp(CandidateGraspPtr cg, float scaleValue);
+
+ static SoSeparator* DrawCandidateGrasps(std::vector<CandidateGraspPtr>& cg, float scaleValue);
+
+ static Eigen::Vector3f getColorFromTable(float currentValue, float kFactor, float maxInputValue);
+ };
+
+}
+
+#endif // DRAWHELPERS_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/Converter.cpp b/GraspPlanning/GraspPlanner/MATPlanner/Converter.cpp
new file mode 100644
index 000000000..b5e2531e1
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/Converter.cpp
@@ -0,0 +1,82 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include "Converter.h"
+
+using namespace std;
+using namespace Eigen;
+
+namespace GraspStudio
+{
+
+ Converter::Converter()
+ {
+ }
+
+
+ MedialSpherePtr Converter::convertPowerCrustPolarBallToMedialSphere(GraspStudio::PowerCrust::PolarBall& pB)
+ {
+ Vector3f center;
+ float radius;
+ Vector3f tempSurfacePoint;
+ vector<Vector3f> surfacePoints;
+
+ center << (float)pB.poleCenter[0],
+ (float)pB.poleCenter[1],
+ (float)pB.poleCenter[2];
+ radius = (float)pB.poleRadius;
+
+ tempSurfacePoint << (float)pB.surfacePoint1[0],
+ (float)pB.surfacePoint1[1],
+ (float)pB.surfacePoint1[2];
+ surfacePoints.push_back(tempSurfacePoint);
+ tempSurfacePoint << (float)pB.surfacePoint2[0],
+ (float)pB.surfacePoint2[1],
+ (float)pB.surfacePoint2[2];
+ surfacePoints.push_back(tempSurfacePoint);
+ tempSurfacePoint << (float)pB.surfacePoint3[0],
+ (float)pB.surfacePoint3[1],
+ (float)pB.surfacePoint3[2];
+ surfacePoints.push_back(tempSurfacePoint);
+ tempSurfacePoint << (float)pB.surfacePoint4[0],
+ (float)pB.surfacePoint4[1],
+ (float)pB.surfacePoint4[2];
+ surfacePoints.push_back(tempSurfacePoint);
+
+ MedialSpherePtr ms = MedialSpherePtr(new MedialSphere(center, radius, surfacePoints));
+
+ return ms;
+ }
+
+ std::vector<MedialSpherePtr> Converter::convertPowerCrustPolarBallsToMedialSpheres(std::vector<GraspStudio::PowerCrust::PolarBall>& polarBalls)
+ {
+ std::vector<MedialSpherePtr> allSpheres;
+
+ for (size_t i = 0; i < polarBalls.size(); i++)
+ {
+ MedialSpherePtr ms = convertPowerCrustPolarBallToMedialSphere(polarBalls.at(i));
+ allSpheres.push_back(ms);
+ }
+
+ return allSpheres;
+ }
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/Converter.h b/GraspPlanning/GraspPlanner/MATPlanner/Converter.h
new file mode 100644
index 000000000..a5970dae1
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/Converter.h
@@ -0,0 +1,50 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#ifndef CONVERTER_H
+#define CONVERTER_H
+
+#include "../../GraspStudio.h"
+#include <vector>
+#include "MedialSphere.h"
+
+#include "powercrust.h"
+
+namespace GraspStudio
+{
+
+ class GRASPSTUDIO_IMPORT_EXPORT Converter
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ Converter();
+
+ static std::vector<MedialSpherePtr> convertPowerCrustPolarBallsToMedialSpheres(std::vector<GraspStudio::PowerCrust::PolarBall>& polarBalls);
+
+ static MedialSpherePtr convertPowerCrustPolarBallToMedialSphere(GraspStudio::PowerCrust::PolarBall& polarBall);
+
+
+ };
+}
+
+#endif // CONVERTER_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/GraspPlannerConfiguration.cpp b/GraspPlanning/GraspPlanner/MATPlanner/GraspPlannerConfiguration.cpp
new file mode 100644
index 000000000..382c572c7
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/GraspPlannerConfiguration.cpp
@@ -0,0 +1,141 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include "GraspPlannerConfiguration.h"
+#include <iostream>
+using namespace std;
+
+namespace GraspStudio
+{
+
+ GraspPlannerConfiguration::GraspPlannerConfiguration()
+ {
+ //constant for setting up the grid of medial spheres
+ gridConstant = 4;
+
+ //candidate grasp generation constants
+ thresholdSymmetryAxis = 0.01f; // do not touch!
+ thresholdSymmetryPlane = 0.4f; // do not touch!
+ minimumObjectAngle = 120.0f; // do not touch!
+
+ //candidate grasp generation options
+ minimumSphereRadiusRelative = 0.0; //If >0.0, discard spheres that are small relative
+ //to the biggest sphere in the object.
+ //Useful to cancel noise and/or to discard
+ //surface details.
+
+ neighborhoodSearchRadius = 0.01f; //Radius of the local neighborhood around a
+ //medial sphere that should be analyzed during
+ //candidate grasp generation.
+
+ maxGraspDiameterOfCurrentRobotHand = -1.0; //If !=-1, specifies the diameter of the
+ //biggest sphere the hand can grasp.
+ //In that case, no candidate grasps will be
+ //generated for spheres bigger than that.
+
+ generateFlippedCandidates = true; //if true: for each candidate, generate a second
+ //candidate with inverted hand orientation
+
+ numberOfApproachDirectionsForLocalSymmetryAxis = 4; //generate this number of approach
+ //directions in equal angle
+ //intervals around a local
+ //symmetry axis
+
+ fractionOfSpheresToAnalyze = 1.0; //should all (filtered) medial spheres be used for
+ //candidate grasp generation, or only a fraction of
+ //them?
+
+ stopAfterAnalyzingThisNumberOfSpheres = -1; //analyze this number of spheres, then stop
+
+ //visualization options
+ //basic stuff
+ drawSurfacePointCloud = false;
+ drawMedialSpheresBeforeFiltering = false;
+ drawMedialAxisPointCloudBeforeFiltering = false;
+ drawMedialSpheresAfterFiltering = true;
+ drawMedialAxisPointCloudAfterFiltering = true;
+
+ //results of analysis
+ drawNeighborhoods = true;
+ drawNeighborhoodCenterOfGravity = true;
+ drawNeighborhoodEigenvectors = true;
+ drawNeighborhoodSearchRadius = false;
+ drawScale = 0.003f; //draw stuff this big
+ drawPointSize = 0.001f;
+ drawCandidateGrasps = true;
+
+ //debug output
+ printMedialSpheres = false;
+ printNeighborhoods = false;
+ printCandidateGrasps = false;
+
+ }
+
+ void GraspPlannerConfiguration::printDebug()
+ {
+ cout << "=== GraspPlannerConfiguration: ===" << endl;
+
+ cout << "thresholdSymmetryAxis: " << thresholdSymmetryAxis << endl;
+ cout << "thresholdSymmetryPlane: " << thresholdSymmetryPlane << endl;
+
+ cout << "minimumObjectAngle: " << minimumObjectAngle << endl;
+ cout << "minimumSphereRadiusRelative: " << minimumSphereRadiusRelative << endl;
+
+ cout << "neighborhoodSearchRadius: " << neighborhoodSearchRadius << endl;
+ cout << "maxGraspDiameterOfCurrentRobotHand: "
+ << maxGraspDiameterOfCurrentRobotHand << endl;
+
+ cout << "generateFlippedCandidates: " << generateFlippedCandidates << endl;
+ cout << "numberOfApproachDirectionsForLocalSymmetryAxis: "
+ << numberOfApproachDirectionsForLocalSymmetryAxis << endl;
+
+ cout << "fractionOfSpheresToAnalyze: " << fractionOfSpheresToAnalyze << endl;
+ cout << "stopAfterAnalyzingThisNumberOfSpheres: "
+ << stopAfterAnalyzingThisNumberOfSpheres << endl;
+
+ cout << "drawSurfacePointCloud:" << drawSurfacePointCloud << endl;
+ cout << "drawMedialSpheresBeforeFiltering: "
+ << drawMedialSpheresBeforeFiltering << endl;
+ cout << "drawMedialAxisPointCloudBeforeFiltering: "
+ << drawMedialAxisPointCloudBeforeFiltering << endl;
+ cout << "drawMedialSpheresAfterFiltering: "
+ << drawMedialSpheresAfterFiltering << endl;
+ cout << "drawMedialAxisPointCloudAfterFiltering: "
+ << drawMedialAxisPointCloudAfterFiltering << endl;
+
+ cout << "drawNeighborhoods: " << drawNeighborhoods << endl;
+ cout << "drawNeighborhoodCenterOfGravity: "
+ << drawNeighborhoodCenterOfGravity << endl;
+ cout << "drawNeighborhoodEigenvectors: "
+ << drawNeighborhoodEigenvectors << endl;
+ cout << "drawNeighborhoodSearchRadius: "
+ << drawNeighborhoodSearchRadius << endl;
+ cout << "drawScale: " << drawScale << endl;
+ cout << "drawPointSize: " << drawPointSize << endl;
+ cout << "drawCandidateGrasps: " << drawCandidateGrasps << endl;
+
+ cout << "printMedialSpheres: " << printMedialSpheres << endl;
+ cout << "printNeighborhoods: " << printNeighborhoods << endl;
+ cout << "printCandidateGrasps: " << printCandidateGrasps << endl;
+
+ }
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/GraspPlannerConfiguration.h b/GraspPlanning/GraspPlanner/MATPlanner/GraspPlannerConfiguration.h
new file mode 100644
index 000000000..e254f7171
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/GraspPlannerConfiguration.h
@@ -0,0 +1,88 @@
+#ifndef GRASPPLANNERCONFIGURATION_H
+#define GRASPPLANNERCONFIGURATION_H
+
+#include <iostream>
+#include <boost/shared_ptr.hpp>
+
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+
+#include "../../GraspStudio.h"
+
+namespace GraspStudio
+{
+ class GraspPlannerConfiguration;
+ typedef boost::shared_ptr<GraspPlannerConfiguration> GraspPlannerConfigurationPtr;
+
+ class GRASPSTUDIO_IMPORT_EXPORT GraspPlannerConfiguration
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ GraspPlannerConfiguration();
+
+ void printDebug();
+
+ int gridConstant;
+
+ //some constants for candidate grasp generation
+ float thresholdSymmetryAxis;
+ float thresholdSymmetryPlane;
+ float minimumObjectAngle;
+ float minimumSphereRadiusRelative;
+ float neighborhoodSearchRadius;
+ float maxGraspDiameterOfCurrentRobotHand;
+
+ bool generateFlippedCandidates;
+ int numberOfApproachDirectionsForLocalSymmetryAxis;
+
+ float fractionOfSpheresToAnalyze;
+ int stopAfterAnalyzingThisNumberOfSpheres;
+
+
+ //visualization options
+ //basic stuff
+ bool drawSurfacePointCloud;
+ bool drawMedialSpheresBeforeFiltering;
+ bool drawMedialAxisPointCloudBeforeFiltering;
+ bool drawMedialSpheresAfterFiltering;
+ bool drawMedialAxisPointCloudAfterFiltering;
+
+ //results of analysis
+ bool drawNeighborhoods;
+ bool drawNeighborhoodCenterOfGravity;
+ bool drawNeighborhoodEigenvectors;
+ bool drawNeighborhoodSearchRadius;
+ float drawScale;
+ float drawPointSize;
+ bool drawCandidateGrasps;
+
+ //debug output
+ bool printMedialSpheres;
+ bool printNeighborhoods;
+ bool printCandidateGrasps;
+
+ };
+
+}
+#endif // GRASPPLANNERCONFIGURATION_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/GridOfMedialSpheres.cpp b/GraspPlanning/GraspPlanner/MATPlanner/GridOfMedialSpheres.cpp
new file mode 100644
index 000000000..9325857a6
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/GridOfMedialSpheres.cpp
@@ -0,0 +1,434 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include <cmath>
+#include <iostream>
+
+#include "GridOfMedialSpheres.h"
+
+using namespace std;
+using namespace Eigen;
+
+namespace GraspStudio
+{
+
+ GridOfMedialSpheres::GridOfMedialSpheres(bool verbose)
+ {
+ this->verbose = verbose;
+ }
+
+ GridOfMedialSpheres::~GridOfMedialSpheres()
+ {
+ }
+
+ void GridOfMedialSpheres::reset()
+ {
+ numberOfSpheres = 0;
+ minRadius = -1;
+ maxRadius = -1;
+
+ grid.clear();
+ }
+
+
+ void GridOfMedialSpheres::setup(vector< Vector3f > surfacePoints, vector<MedialSpherePtr> spheres, int gridConstant)
+ {
+ if (verbose)
+ VR_INFO << "GridOfMedialSpheres::setup() called." << endl;
+
+ GridParameters gridParams = determineGridParameters(surfacePoints, spheres, gridConstant);
+ this->gridParameters = gridParams;
+
+ //build the grid (i.e. insert all the spheres)
+ buildGrid(spheres, gridParams);
+
+ if (verbose)
+ VR_INFO << "GridOfMedialSpheres::setup() finished." << endl;
+
+ }
+
+ GridParameters GridOfMedialSpheres::determineGridParameters(vector<Vector3f> surfacePoints, vector<MedialSpherePtr> spheres, int gridConstant = 4)
+ {
+ if (verbose)
+ {
+ VR_INFO << "determineGridParameters() called" << endl;
+ VR_INFO << "gridConstant: " << gridConstant << endl;
+ }
+
+ //determine the corners of axis-aligned bounding box of the surface points
+ Vector3f minPoint;
+ Vector3f maxPoint;
+ findMinMax(surfacePoints, minPoint, maxPoint);
+ if (verbose)
+ {
+ VR_INFO << "minPoint: " << StrOutHelpers::toString(minPoint) << endl;
+ VR_INFO << "maxPoint: " << StrOutHelpers::toString(maxPoint) << endl;
+ }
+
+ //how many cells (for fast access)
+ Vector3i numCells;
+ float cellWidth;
+ determineCellNumbers(surfacePoints, minPoint, maxPoint, gridConstant, numCells, cellWidth);
+ GridParameters gridParams = GridParameters(numCells, minPoint, maxPoint, cellWidth);
+
+ return gridParams;
+ }
+
+ void GridOfMedialSpheres::findMinMax(vector<Vector3f> points, Vector3f &minPoint, Vector3f &maxPoint)
+ {
+ //init
+ minPoint = points.at(0);
+ maxPoint = points.at(0);
+
+
+ for (size_t i = 0; i < points.size(); i++)
+ {
+ //x
+ if (points.at(i)(0) < minPoint(0))
+ minPoint(0) = points.at(i)(0);
+ else
+ if (points.at(i)(0) > maxPoint(0))
+ maxPoint(0) = points.at(i)(0);
+
+ //y
+ if (points.at(i)(1) < minPoint(1))
+ minPoint(1) = points.at(i)(1);
+ else
+ if (points.at(i)(1) > maxPoint(1))
+ maxPoint(1) = points.at(i)(1);
+
+ //z
+ if (points.at(i)(2) < minPoint(2))
+ minPoint(2) = points.at(i)(2);
+ else
+ if (points.at(i)(2) > maxPoint(2))
+ maxPoint(2) = points.at(i)(2);
+ }
+ }
+
+ void GridOfMedialSpheres::determineCellNumbers(vector<Vector3f> points, Vector3f minPoint, Vector3f maxPoint, int gridConstant, Vector3i& numCells, float& cellWidth)
+ {
+ if (verbose)
+ {
+ VR_INFO << "determineGridParameters() called" << endl;
+ }
+
+ Vector3f diffVect = maxPoint - minPoint;
+ float maxDist = diffVect.maxCoeff();
+ float minDist = diffVect.minCoeff();
+
+ float ratio = maxDist / minDist;
+ int numPoints = points.size();
+
+ //ACHTUNG: alter Algorithmus! TODO: Änderungen einpflegen!
+ char mainDirection;
+ //x is main dimension
+ if (diffVect(0) == maxDist) //ACHTUNG: fuzzy floating point arithmetic... ?
+ {
+ mainDirection = 'x';
+ //numCells(0) = ceil(ceil(ratio * (numPoints ** (1.0 / 3))) / gridConstant);
+ numCells(0) = (int)ceil(ceil(ratio *pow(numPoints, (1.0f / 3.0f))) / gridConstant);
+ cellWidth = maxDist / numCells(0);
+ numCells(1) = (int)ceil(diffVect(1) / cellWidth);
+ numCells(2) = (int)ceil(diffVect(2) / cellWidth);
+ } //y is main dimension
+ else if (diffVect(1) == maxDist) //ACHTUNG: fuzzy floating point arithmetic... ?
+ {
+ mainDirection = 'y';
+ //numCells(1) = ceil(ceil(ratio * (num_points ** (1.0 / 3))) / k_factor);
+ numCells(1) = (int)ceil(ceil(ratio *pow(numPoints, (1.0f / 3.0f))) / gridConstant);
+ cellWidth = maxDist / numCells(1);
+ numCells(0) = (int)ceil(diffVect(0) / cellWidth);
+ numCells(2) = (int)ceil(diffVect(2) / cellWidth);
+
+ } //z is main dimension
+ else if (diffVect(2) == maxDist)
+ {
+ mainDirection = 'z';
+ //numCells(2) = ceil(ceil(ratio * (numPoints ** (1.0 / 3))) / gridConstant);
+ numCells(2) = (int)ceil(ceil(ratio *pow(numPoints, (1.0f / 3.0f))) / gridConstant);
+ cellWidth = maxDist / numCells(2);
+ numCells(0) = (int)ceil(diffVect(0) / cellWidth);
+ numCells(1) = (int)ceil(diffVect(1) / cellWidth);
+ }
+ else // Should never occur.
+ VR_ERROR << "GridOfMedialSpheres::determineCellNumbers(): Unexpected error!" << endl;
+
+ int numCellsTotal = (numCells(0) + 1) * (numCells(1) + 1) * (numCells(2) + 1);
+
+ if (verbose)
+ {
+ VR_INFO << "main direction is: " << mainDirection << endl;
+ VR_INFO << "numCells: " << numCells(0) << " " << numCells(1) << " " << numCells(2) << endl;
+ VR_INFO << "numCellsTotal: " << numCellsTotal << " cellWidth: " << cellWidth << endl;
+ }
+
+ }
+
+ void GridOfMedialSpheres::buildGrid(std::vector<MedialSpherePtr> spheres, GridParameters gridParams)
+ {
+ if (verbose)
+ {
+ VR_INFO << "buildGrid() called." << endl;
+ }
+
+ allocateGrid(gridParams.numCells);
+
+ for (size_t i = 0; i < spheres.size(); i++)
+ insertSphere(spheres.at(i));
+
+ if (verbose)
+ {
+ VR_INFO << "buildGrid() finished." << endl;
+ }
+ }
+
+ void GridOfMedialSpheres::allocateGrid(Eigen::Vector3i numCells)
+ {
+ if (verbose)
+ {
+ VR_INFO << "allocateGrid() called." << endl;
+ }
+
+ //numCells[i] + 1 ??
+ int xdim = numCells(0);
+ int ydim = numCells(1);
+ int zdim = numCells(2);
+
+ vector<MedialSpherePtr> v1D;
+ vector< vector<MedialSpherePtr> > v2D;
+ vector< vector< vector<MedialSpherePtr> > > v3D;
+
+ v2D.reserve(zdim);
+ for (size_t j = 0; j < v2D.capacity(); j++)
+ v2D.push_back(v1D);
+
+ v3D.reserve(ydim);
+ for (size_t k = 0; k < v3D.capacity(); k++)
+ v3D.push_back(v2D);
+
+ grid.reserve(xdim);
+ for (size_t l = 0; l < grid.capacity(); l++)
+ grid.push_back(v3D);
+
+ if (verbose)
+ {
+ VR_INFO << "allocateGrid() finished." << endl;
+ }
+ }
+
+ void GridOfMedialSpheres::printDebugGridCells()
+ {
+ for (size_t x = 0; x < grid.size(); x++)
+ {
+ for (size_t y = 0; y < grid.at(x).size(); y++)
+ {
+ for (size_t z = 0; z < grid.at(x).at(y).size(); z++)
+ {
+ VR_INFO << "Number of spheres in cell ["
+ << x << " " << y << " " << z << " " << "]: "
+ << grid.at(x).at(y).at(z).size() << endl;
+ }
+ VR_INFO << endl;
+ }
+ VR_INFO << endl;
+ }
+
+ }
+
+ void GridOfMedialSpheres::insertSphere(MedialSpherePtr s)
+ {
+ Vector3i gridIndex = calcGridIndex(s->center);
+ if (isGridIndexValid(gridIndex))
+ {
+ //update minRadius and maxRadius
+ if (numberOfSpheres == 0)
+ {
+ minRadius = s->radius;
+ maxRadius = s->radius;
+ }
+ else
+ {
+ if (s->radius > maxRadius)
+ maxRadius = s->radius;
+ else if (s->radius < minRadius)
+ minRadius = s->radius;
+ }
+ //put sphere into the grid
+ grid.at(gridIndex(0)).at(gridIndex(1)).at(gridIndex(2)).push_back(s);
+ }
+ else
+ {
+ if (verbose)
+ {
+ VR_WARNING << "INVALID grid index: " << gridIndex(0) << " " << gridIndex(1) << " " << gridIndex(2) << " --> Discard sphere (outside of surface points bounding box). " << endl;
+ }
+ }
+
+ }
+
+ Vector3i GridOfMedialSpheres::calcGridIndex(Vector3f sphereCenter)
+ {
+ float x = sphereCenter(0);
+ float y = sphereCenter(1);
+ float z = sphereCenter(2);
+
+ float x_min = gridParameters.minPoint(0);
+ float y_min = gridParameters.minPoint(1);
+ float z_min = gridParameters.minPoint(2);
+ float x_max = gridParameters.maxPoint(0);
+ float y_max = gridParameters.maxPoint(1);
+ float z_max = gridParameters.maxPoint(2);
+
+ Vector3i gridIndex;
+ gridIndex(0) = (int)floor(gridParameters.numCells(0) * (x - x_min) / (x_max - x_min));
+ gridIndex(1) = (int)floor(gridParameters.numCells(1) * (y - y_min) / (y_max - y_min));
+ gridIndex(2) = (int)floor(gridParameters.numCells(2) * (z - z_min) / (z_max - z_min));
+
+ return gridIndex;
+ }
+
+ bool GridOfMedialSpheres::isGridIndexValid(Vector3i gridIndex)
+ {
+ //Check bounds
+ if (gridIndex(0) > (gridParameters.numCells(0) - 1))
+ return false;
+ else if (gridIndex(1) > (gridParameters.numCells(1) - 1))
+ return false;
+ else if (gridIndex(2) > (gridParameters.numCells(2) - 1))
+ return false;
+ else if (gridIndex(0) < 0)
+ return false;
+ else if (gridIndex(1) < 0)
+ return false;
+ else if (gridIndex(2) < 0)
+ return false;
+ else
+ return true;
+ }
+
+ vector<MedialSpherePtr> GridOfMedialSpheres::getSpheresInNeighborhood(MedialSpherePtr s, float searchRadius)
+ {
+ if (verbose)
+ VR_INFO << "getSpheresInNeighborhood() called." << endl;
+
+ //Get all cube cells inside searchRadius
+ Vector3i startGridIndex = calcGridIndex(s->center);
+ int cubeRadiusToSearch = int(ceil(searchRadius / this->gridParameters.cellWidth));
+
+ if (verbose)
+ {
+ VR_INFO << "startGridIndex: " << StrOutHelpers::toString(startGridIndex) << endl;
+ VR_INFO << "searchRadius: " << searchRadius << endl;
+ VR_INFO << "cellWidth: " << gridParameters.cellWidth << endl;
+ VR_INFO << "cubeRadiusToSearch: " << cubeRadiusToSearch << endl;
+ }
+
+ //calc grid cell indices
+ vector<Vector3i> cubeIndicesToSearch = computeCubeIndices(startGridIndex,
+ this->gridParameters.maxGridIndex,
+ cubeRadiusToSearch);
+
+ //get spheres from the grid cells
+ vector<MedialSpherePtr> spheresInCuboidNeighborhood = getSpheresFromGrid(cubeIndicesToSearch);
+
+
+ //get all spheres inside searchRadius
+ vector<MedialSpherePtr> spheresInSphericalNeighborhood =
+ SphereHelpers::getSpheresInsideSearchRadius(spheresInCuboidNeighborhood,
+ s, searchRadius);
+
+
+ return spheresInSphericalNeighborhood;
+ }
+
+ vector<Vector3i> GridOfMedialSpheres::computeCubeIndices(Vector3i seedIndex, Vector3i maxCoords, int cubeRadiusToSearch)
+ {
+ int maxIndexX = seedIndex(0) + cubeRadiusToSearch;
+ int minIndexX = seedIndex(0) - cubeRadiusToSearch;
+ if (maxIndexX > maxCoords(0)) //ACHTUNG: > oder >= ?
+ maxIndexX = maxCoords(0); //ACHTUNG: evtl. maxCoords(0)-1 zuweisen?
+ if (minIndexX < 0)
+ minIndexX = 0;
+
+ int maxIndexY = seedIndex(1) + cubeRadiusToSearch;
+ int minIndexY = seedIndex(1) - cubeRadiusToSearch;
+ if (maxIndexY > maxCoords(1)) //ACHTUNG: > oder >= ?
+ maxIndexY = maxCoords(1); //ACHTUNG: evtl. maxCoords(1)-1 zuweisen?
+ if (minIndexY < 0)
+ minIndexY = 0;
+
+ int maxIndexZ = seedIndex(2) + cubeRadiusToSearch;
+ int minIndexZ = seedIndex(2) - cubeRadiusToSearch;
+ if (maxIndexZ > maxCoords(2)) //ACHTUNG: > oder >= ?
+ maxIndexZ = maxCoords(2); //ACHTUNG: evtl. maxCoords(2)-1 zuweisen?
+ if (minIndexZ < 0)
+ minIndexZ = 0;
+
+ vector<Vector3i> cubeIndices;
+ Vector3i currentIndex;
+
+ for (int i = minIndexX; i <= maxIndexX; i++)
+ for (int j = minIndexY; j <= maxIndexY; j++)
+ for (int k = minIndexZ; k <= maxIndexZ; k++)
+ {
+ currentIndex << i, j, k;
+ cubeIndices.push_back(currentIndex);
+ }
+
+ return cubeIndices;
+ }
+
+ vector<MedialSpherePtr> GridOfMedialSpheres::getSpheresFromGrid(vector<Vector3i> gridIndices)
+ {
+ // if (verbose)
+ // {
+ // VR_INFO << "get spheres from the following grid cells: " << endl;
+ // VR_INFO << StrOutHelpers::toString(gridIndices) << endl;
+ // }
+
+ vector<MedialSpherePtr> vectorOfSpheres;
+ for (size_t i = 0; i < gridIndices.size(); i++)
+ {
+ Vector3i currentIndex = gridIndices.at(i);
+ vector<MedialSpherePtr>::iterator itStart = (grid.at(currentIndex(0)).at(currentIndex(1)).at(currentIndex(2))).begin();
+ vector<MedialSpherePtr>::iterator itEnd = (grid.at(currentIndex(0)).at(currentIndex(1)).at(currentIndex(2))).end();
+ vectorOfSpheres.insert(vectorOfSpheres.end(), itStart, itEnd);
+
+ // if (verbose)
+ // {
+ // VR_INFO << "currentIndex: "
+ // << StrOutHelpers::toString(currentIndex) << endl;
+ // VR_INFO << "current spheres total: "
+ // << vectorOfSpheres.size() << endl;
+ // }
+ }
+
+ // VR_INFO << "getSpheresFromGrid() fetched " << vectorOfSpheres.size()
+ // << " spheres, leaving..." << endl;
+
+ return vectorOfSpheres;
+
+ }
+
+
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/GridOfMedialSpheres.h b/GraspPlanning/GraspPlanner/MATPlanner/GridOfMedialSpheres.h
new file mode 100644
index 000000000..39ee40afb
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/GridOfMedialSpheres.h
@@ -0,0 +1,91 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#ifndef GRIDOFMEDIALSPHERES_H
+#define GRIDOFMEDIALSPHERES_H
+
+#include <vector>
+#include <Eigen/Geometry>
+#include <boost/shared_ptr.hpp>
+#include "../../GraspStudio.h"
+#include "MedialSphere.h"
+#include "GridParameters.h"
+#include "SphereHelpers.h"
+
+namespace GraspStudio
+{
+
+ class GridOfMedialSpheres;
+ typedef boost::shared_ptr<GridOfMedialSpheres> GridOfMedialSpheresPtr;
+
+ class GRASPSTUDIO_IMPORT_EXPORT GridOfMedialSpheres
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ GridOfMedialSpheres(bool verbose = false);
+ ~GridOfMedialSpheres();
+
+ void setup(std::vector< Eigen::Vector3f > surfacePoints, std::vector<MedialSpherePtr> spheres, int gridConstant = 4);
+
+ void reset();
+
+ void allocateGrid(Eigen::Vector3i numCells);
+ std::vector<Eigen::Vector3i> computeCubeIndices(Eigen::Vector3i seedIndex, Eigen::Vector3i maxCoords, int cubeRadiusToSearch);
+
+
+ //methods for access
+ void insertSphere(MedialSpherePtr s);
+
+ std::vector<MedialSpherePtr> getSpheresFromGrid(std::vector<Eigen::Vector3i> gridIndices);
+ std::vector<MedialSpherePtr> getSpheresInNeighborhood(MedialSpherePtr s, float searchRadius);
+
+ Eigen::Vector3i calcGridIndex(Eigen::Vector3f sphereCenter);
+ bool isGridIndexValid(Eigen::Vector3i gridIndex);
+
+ //for debugging
+ void printDebugGridCells();
+
+ protected:
+
+ //methods for setting up the grid
+ void computeGrid();
+
+ GridParameters determineGridParameters(std::vector<Eigen::Vector3f> surfacePoints, std::vector<MedialSpherePtr> spheres, int gridConstant);
+ void findMinMax(std::vector<Eigen::Vector3f> surfacePoints, Eigen::Vector3f& minPoint, Eigen::Vector3f& maxPoint);
+ void determineCellNumbers(std::vector<Eigen::Vector3f> surfacePoints, Eigen::Vector3f minPoint, Eigen::Vector3f maxPoint, int gridConstant, Eigen::Vector3i& numCells, float& cellWidth);
+ void buildGrid(std::vector<MedialSpherePtr> spheres, GridParameters gridParams);
+
+ bool verbose;
+
+ GridParameters gridParameters;
+
+ //the actual grid of spheres
+ std::vector< std::vector< std::vector< std::vector< MedialSpherePtr > > > > grid;
+
+ int numberOfSpheres;
+ float minRadius;
+ float maxRadius;
+
+ };
+}
+#endif // GRIDOFMEDIALSPHERES_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/GridParameters.cpp b/GraspPlanning/GraspPlanner/MATPlanner/GridParameters.cpp
new file mode 100644
index 000000000..0d9ab240a
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/GridParameters.cpp
@@ -0,0 +1,47 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include "GridParameters.h"
+
+using namespace std;
+using namespace Eigen;
+
+namespace GraspStudio
+{
+
+ GridParameters::GridParameters()
+ {
+ }
+
+ GridParameters::GridParameters(Vector3i numCells, Vector3f minPoint, Vector3f maxPoint, float cellWidth)
+ {
+ this->numCells = numCells;
+ this->minPoint = minPoint;
+ this->maxPoint = maxPoint;
+ this->cellWidth = cellWidth;
+
+ maxGridIndex(0) = numCells(0) - 1;
+ maxGridIndex(1) = numCells(1) - 1;
+ maxGridIndex(2) = numCells(2) - 1;
+ }
+
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/GridParameters.h b/GraspPlanning/GraspPlanner/MATPlanner/GridParameters.h
new file mode 100644
index 000000000..ac25b954b
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/GridParameters.h
@@ -0,0 +1,47 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#ifndef GRIDPARAMETERS_H
+#define GRIDPARAMETERS_H
+
+#include <vector>
+#include <Eigen/Geometry>
+namespace GraspStudio
+{
+
+ class GridParameters
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ GridParameters();
+ GridParameters(Eigen::Vector3i numCells, Eigen::Vector3f minPoint, Eigen::Vector3f maxPoint, float cellWidth);
+
+ Eigen::Vector3i numCells;
+ Eigen::Vector3i maxGridIndex;
+ Eigen::Vector3f minPoint;
+ Eigen::Vector3f maxPoint;
+ float cellWidth;
+
+ };
+}
+#endif // GRIDPARAMETERS_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/LocalNeighborhood.cpp b/GraspPlanning/GraspPlanner/MATPlanner/LocalNeighborhood.cpp
new file mode 100644
index 000000000..cfd4fc028
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/LocalNeighborhood.cpp
@@ -0,0 +1,187 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include "LocalNeighborhood.h"
+
+namespace GraspStudio
+{
+
+ LocalNeighborhood::LocalNeighborhood()
+ {
+ }
+
+ LocalNeighborhood::LocalNeighborhood(MedialSpherePtr seedSphere, std::vector<MedialSpherePtr>& spheresInNeighborhood, float neighborhoodRadius)
+ {
+ centerSphere = seedSphere;
+ center = seedSphere->center;
+ spheres = spheresInNeighborhood;
+ radius = neighborhoodRadius;
+ numberOfSpheres = spheres.size();
+
+ if (numberOfSpheres > 1)
+ isEmpty = false;
+ else
+ isEmpty = true;
+
+ hasBeenAnalyzed = false;
+ }
+
+ void LocalNeighborhood::analyze()
+ {
+ determineEigenvectorsAndEigenvalues_viaSVD();
+ computeCenterOfGravity();
+
+ hasBeenAnalyzed = true;
+ }
+
+ void LocalNeighborhood::determineEigenvectorsAndEigenvalues_viaSVD()
+ {
+ std::vector<Eigen::Vector3f> sphereCenters = SphereHelpers::getSphereCenters(spheres);
+ Eigen::MatrixXf sphereCentersAsMatrix = SphereHelpers::toMatrix_3xN(sphereCenters);
+
+ //normalize the data
+ Eigen::Vector3f meanVect = sphereCentersAsMatrix.rowwise().mean();
+ Eigen::MatrixXf sphereCentersAsMatrixNormalized = sphereCentersAsMatrix.colwise() - meanVect;
+
+ Eigen::JacobiSVD<Eigen::MatrixXf> svd(sphereCentersAsMatrixNormalized,
+ Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+ // std::cout << "Its singular values are: " << std::endl << svd.singularValues() << std::endl;
+ // std::cout << "Its left singular vectors are the columns of the thin U matrix: "
+ // << std::endl << svd.matrixU() << std::endl;
+
+ eigenvalue1 = svd.singularValues()[0] * svd.singularValues()[0];
+ eigenvalue2 = svd.singularValues()[1] * svd.singularValues()[1];
+
+ eigenvector1 = svd.matrixU().block<3, 1>(0, 0);
+ eigenvector2 = svd.matrixU().block<3, 1>(0, 1);
+
+ ratioOfEigenvalues = eigenvalue2 / eigenvalue1;
+ }
+
+ //void LocalNeighborhood::determineEigenvectorsAndEigenvalues_viaCovMatrix()
+ //{
+ // std::vector<Eigen::Vector3f> sphereCenters = SphereHelpers::getSphereCenters(spheres);
+ // Eigen::MatrixXf sphereCentersAsMatrix = SphereHelpers::toMatrix_3xN(sphereCenters);
+
+ // //normalize the data
+ // Eigen::Vector3f meanVect = sphereCentersAsMatrix.rowwise().mean();
+ // Eigen::MatrixXf sphereCentersAsMatrixNormalized = sphereCentersAsMatrix.colwise() - meanVect;
+
+ // Eigen::MatrixXf cov = sphereCentersAsMatrixNormalized * sphereCentersAsMatrixNormalized.transpose();
+
+ // Eigen::EigenSolver<Eigen::MatrixXf> es(cov,true);
+
+ //// std::cout << "eigenvalues: " << es.eigenvalues() << std::endl;
+ //// std::cout << "eigenvectors: " << es.eigenvectors() << std::endl;
+
+ // eigenvalue1 = std::real(es.eigenvalues()[0]);
+ // eigenvalue2 = std::real(es.eigenvalues()[1]);
+
+ // if (eigenvalue1 > eigenvalue2)
+ // {
+ // //everything OK
+ // eigenvector1 << std::real(es.eigenvectors().col(0)[0]),
+ // std::real(es.eigenvectors().col(0)[1]),
+ // std::real(es.eigenvectors().col(0)[2]);
+ // eigenvector2 << std::real(es.eigenvectors().col(1)[0]),
+ // std::real(es.eigenvectors().col(1)[1]),
+ // std::real(es.eigenvectors().col(1)[2]);
+ // }
+ // else
+ // {
+ // //swap stuff
+ // eigenvalue1 = std::real(es.eigenvalues()[1]);
+ // eigenvalue2 = std::real(es.eigenvalues()[0]);
+ // eigenvector1 << std::real(es.eigenvectors().col(1)[0]),
+ // std::real(es.eigenvectors().col(1)[1]),
+ // std::real(es.eigenvectors().col(1)[2]);
+ // eigenvector2 << std::real(es.eigenvectors().col(0)[0]),
+ // std::real(es.eigenvectors().col(0)[1]),
+ // std::real(es.eigenvectors().col(0)[2]);
+ // }
+
+ //// std::cout << "eigenvalue1: " << eigenvalue1 << " eigenvalue2: " << eigenvalue2 << std::endl;
+ //// std::cout << "eigenvector1: " << eigenvector1 << std::endl;
+ //// std::cout << "eigenvector2: " << eigenvector2 << std::endl;
+
+ // // std::cout << "eigenvectors: " << es.eigenvectors() << std::endl;
+
+ // ratioOfEigenvalues = eigenvalue2 / eigenvalue1;
+
+ //}
+
+ void LocalNeighborhood::computeCenterOfGravity()
+ {
+ std::vector<Eigen::Vector3f> sphereCenters = SphereHelpers::getSphereCenters(spheres);
+ Eigen::MatrixXf sphereCentersAsMatrix = SphereHelpers::toMatrix_3xN(sphereCenters);
+
+ centerOfGravity = sphereCentersAsMatrix.rowwise().mean();
+ }
+
+ void LocalNeighborhood::printDebug()
+ {
+ std::cout << "=== LocalNeighborhood ===" << std::endl;
+ std::cout << "center: " << StrOutHelpers::toString(center) << std::endl;
+ std::cout << "radius: " << radius << std::endl;
+ std::cout << "numberOfSpheres: " << numberOfSpheres << std::endl;
+ std::cout << "isEmpty: " << isEmpty << std::endl;
+ std::cout << "hasBeenAnalyzed: " << hasBeenAnalyzed << std::endl;
+
+ if (hasBeenAnalyzed)
+ {
+ std::cout << "eigenvector1: " << StrOutHelpers::toString(eigenvector1) << std::endl;
+ std::cout << "eigenvector2: " << StrOutHelpers::toString(eigenvector2) << std::endl;
+ std::cout << "eigenvalue1: " << eigenvalue1 << std::endl;
+ std::cout << "eigenvalue2: " << eigenvalue2 << std::endl;
+ std::cout << "ratioOfEigenvalues: " << ratioOfEigenvalues << std::endl;
+ std::cout << "centerOfGravity: " << StrOutHelpers::toString(centerOfGravity) << std::endl;
+ }
+ }
+
+ bool LocalNeighborhood::isValid()
+ {
+ if (ratioOfEigenvalues < 0.0)
+ {
+ VR_WARNING << "WARNING: LocalNeighborhood::isValid(): ratioOfEigenvalues negative! Discarding this local neighborhood!" << std::endl;
+ printDebug();
+ return false;
+ }
+
+ if (eigenvalue1 <= 0.0)
+ {
+ VR_WARNING << "WARNING: LocalNeighborhood::isValid(): eigenvalue1 zero/negative! Discarding this local neighborhood!" << std::endl;
+ printDebug();
+ return false;
+ }
+
+ if (eigenvalue2 < 0.0)
+ {
+ VR_WARNING << "WARNING: LocalNeighborhood::isValid(): eigenvalue2 negative! Discarding this local neighborhood!" << std::endl;
+ printDebug();
+ return false;
+ }
+
+ return true;
+ }
+
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/LocalNeighborhood.h b/GraspPlanning/GraspPlanner/MATPlanner/LocalNeighborhood.h
new file mode 100644
index 000000000..0d3e4719e
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/LocalNeighborhood.h
@@ -0,0 +1,74 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#ifndef LOCALNEIGHBORHOOD_H
+#define LOCALNEIGHBORHOOD_H
+
+#include "../../GraspStudio.h"
+#include <vector>
+#include <Eigen/Geometry>
+#include <Eigen/Eigenvalues>
+
+#include "SphereHelpers.h"
+#include "StrOutHelpers.h"
+
+namespace GraspStudio
+{
+
+ class LocalNeighborhood;
+ typedef boost::shared_ptr<LocalNeighborhood> LocalNeighborhoodPtr;
+
+ class GRASPSTUDIO_IMPORT_EXPORT LocalNeighborhood
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ LocalNeighborhood();
+ LocalNeighborhood(MedialSpherePtr seedSphere, std::vector<MedialSpherePtr>& spheresInNeighborhood, float neighborhoodRadius);
+
+ MedialSpherePtr centerSphere;
+ Eigen::Vector3f center;
+ std::vector<MedialSpherePtr> spheres;
+ float radius;
+ size_t numberOfSpheres;
+ bool isEmpty;
+ bool hasBeenAnalyzed;
+
+ //evaluation of local symmetry properties
+ Eigen::Vector3f eigenvector1;
+ Eigen::Vector3f eigenvector2;
+ float eigenvalue1;
+ float eigenvalue2;
+ float ratioOfEigenvalues;
+
+ Eigen::Vector3f centerOfGravity;
+
+ void determineEigenvectorsAndEigenvalues_viaSVD();
+ //void determineEigenvectorsAndEigenvalues_viaCovMatrix();
+ void computeCenterOfGravity();
+ void analyze();
+ void printDebug();
+ bool isValid();
+
+ };
+}
+#endif // LOCALNEIGHBORHOOD_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/MatGraspPlanner.cpp b/GraspPlanning/GraspPlanner/MATPlanner/MatGraspPlanner.cpp
new file mode 100644
index 000000000..cb08c893d
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/MatGraspPlanner.cpp
@@ -0,0 +1,419 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include "MatGraspPlanner.h"
+#include <VirtualRobot/Grasping/Grasp.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotConfig.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Visualization/TriMeshModel.h>
+#include <iostream>
+#include <sstream>
+#include <GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h>
+#include <GraspPlanning/GraspQuality/GraspQualityMeasure.h>
+#include <GraspPlanning/ApproachMovementGenerator.h>
+
+using namespace std;
+using namespace VirtualRobot;
+
+namespace GraspStudio
+{
+
+
+MatGraspPlanner::MatGraspPlanner( VirtualRobot::GraspSetPtr graspSet,
+ RobotPtr robot,
+ EndEffectorPtr eef,
+ GraspQualityMeasureWrenchSpacePtr graspQuality,
+ GraspPlannerConfigurationPtr gpConfig,
+ float minQuality,
+ bool forceClosure,
+ bool verbose)
+:GraspPlanner(graspSet), eef(eef), graspQuality(graspQuality), gpConfig(gpConfig), minQuality(minQuality), forceClosure(forceClosure)
+{
+ THROW_VR_EXCEPTION_IF(!graspQuality, "NULL grasp quality...");
+ THROW_VR_EXCEPTION_IF(!graspQuality->getObject(), "no object...");
+ object = graspQuality->getObject();
+ THROW_VR_EXCEPTION_IF(!eef, "NULL eef...");
+ THROW_VR_EXCEPTION_IF(!graspSet, "NULL graspSet...");
+
+ this->robot = robot;
+
+ eefName = eef->getName();
+
+
+ if (!gpConfig)
+ {
+ // setup with standard parameters
+ gpConfig.reset(new GraspPlannerConfiguration());
+ }
+ this->verbose = verbose;
+ initialized = false;
+}
+
+MatGraspPlanner::~MatGraspPlanner()
+{
+}
+
+bool MatGraspPlanner::init()
+{
+ // init powercrust data structures
+ executePowercrust();
+
+ if (pc->resultingPolarBalls.size()==0)
+ {
+ VR_ERROR << "No polar balls from powercrust?!" << endl;
+ return false;
+ }
+
+ cgGenerator = CandidateGraspGeneratorPtr(new CandidateGraspGenerator(verbose));
+
+ cgTester = CandidateGraspTesterPtr(new CandidateGraspTester(robot, object,
+ eefName, graspQuality, verbose));
+
+ medialSpheres.clear();
+ medialSpheresFiltered.clear();
+ localNeighborhoods.clear();
+ candidateGrasps.clear();
+
+ //1. get medial spheres from powercrust
+ medialSpheres = Converter::convertPowerCrustPolarBallsToMedialSpheres(
+ pc->resultingPolarBalls);
+
+ float maxSphereRadiusForColorComputation =
+ SphereHelpers::findMaxSphereRadius(medialSpheres);
+ if (maxSphereRadiusForColorComputation<=0)
+ {
+ VR_ERROR << "Max radius invalid?!" << endl;
+ return false;
+ }
+
+ //drawMedialAxisPointCloud(medialSpheres, 0.5*gpConfig->drawPointSize);
+ //drawMedialSpheres(medialSpheres, maxSphereRadiusForColorComputation);
+
+ //2. filter spheres
+ for (size_t i=0; i<medialSpheres.size(); i++)
+ medialSpheresFiltered.push_back(medialSpheres.at(i)->clone());
+
+ SphereHelpers::filterSpheres(medialSpheresFiltered, gpConfig->minimumObjectAngle,
+ gpConfig->minimumSphereRadiusRelative, verbose);
+
+ //drawMedialAxisPointCloudFiltered(medialSpheresFiltered, 0.5*gpConfig->drawPointSize);
+ //drawMedialSpheresFiltered(medialSpheresFiltered, maxSphereRadiusForColorComputation);
+
+ //3. setup the grid
+ goms.reset(new GridOfMedialSpheres(verbose));
+ goms->setup(surfacePoints, medialSpheresFiltered, gpConfig->gridConstant);
+
+
+ // compute candidates (todo: maybe we do not need to compute all candidates in advance, e.g. plan 10 grasps)
+ computeAllCandidates();
+
+ initialized = true;
+ return true;
+}
+
+
+std::vector<CandidateGraspPtr> MatGraspPlanner::computeCandidates(MedialSpherePtr ms)
+{
+ vector<CandidateGraspPtr> candidatesTemp;
+ vector<MedialSpherePtr> neighborSpheres;
+ neighborSpheres = goms->getSpheresInNeighborhood(
+ ms, gpConfig->neighborhoodSearchRadius);
+ // cout << "got " << neighborSpheres.size() << " spheres in neighborhood" << endl;
+
+ LocalNeighborhoodPtr nbhd = LocalNeighborhoodPtr(new LocalNeighborhood(ms,
+ neighborSpheres,
+ gpConfig->neighborhoodSearchRadius));
+ if (!nbhd->isEmpty)
+ {
+ nbhd->analyze();
+
+ if (gpConfig->printNeighborhoods)
+ nbhd->printDebug();
+
+ if (nbhd->isValid())
+ {
+ localNeighborhoods.push_back(nbhd);
+
+ candidatesTemp = cgGenerator->generateCandidateGrasps(nbhd, gpConfig);
+ }
+ else
+ {
+ //VR_INFO << "Invalid neighborhood for sphere ID: " << i << endl;
+ }
+ }
+ else
+ {
+ if (verbose)
+ VR_INFO << "===> Local neighborhood empty, skip this sphere! If this occurs too often: maybe you should use a bigger search radius value?)" << endl;
+ }
+
+
+ return candidatesTemp;
+}
+
+bool MatGraspPlanner::computeAllCandidates()
+{
+ //4. analyze neighborhoods and generate candidate grasps
+ candidateGrasps.clear();
+
+ int sphereCounter = 0;
+ int everyNthSphere = 1;
+ if (gpConfig->fractionOfSpheresToAnalyze>0)
+ everyNthSphere = (int)ceil(1.0 / (double)gpConfig->fractionOfSpheresToAnalyze);
+
+ for (size_t i=0; i<medialSpheresFiltered.size(); i++)
+ {
+ if ((gpConfig->fractionOfSpheresToAnalyze < 1.0) && (i % everyNthSphere != 0))
+ {
+ // cout << "Testing only every " << everyNthSphere
+ // << "th sphere. Skipping... (fractionOfSpheresToAnalyze: "
+ // << gpConfig->fractionOfSpheresToAnalyze << ") "
+ // << endl;
+ continue;
+ }
+
+ sphereCounter++;
+ if (verbose)
+ {
+ if (i % 100 == 0)
+ {
+ cout << "--- --- ---> sphere ID: " << i
+ << "; analyzing sphere no. "<< sphereCounter
+ << " of " << medialSpheresFiltered.size()
+ << ", will stop after analyzing "
+ << gpConfig->stopAfterAnalyzingThisNumberOfSpheres << " spheres."
+ << endl;
+ }
+ }
+
+ MedialSpherePtr ms = medialSpheresFiltered.at(i);
+ //ms.printDebug();
+ vector<CandidateGraspPtr> candidatesTemp = computeCandidates(ms);
+ if (candidatesTemp.size()>0)
+ {
+ vector<CandidateGraspPtr>::iterator itStart = candidatesTemp.begin();
+ vector<CandidateGraspPtr>::iterator itEnd = candidatesTemp.end();
+ candidateGrasps.insert(candidateGrasps.end(), itStart, itEnd);
+ if (gpConfig->printCandidateGrasps && verbose)
+ {
+ for (size_t k=0; k<candidatesTemp.size(); k++)
+ candidatesTemp.at(k)->printDebug();
+ }
+ }
+
+ if ((gpConfig->stopAfterAnalyzingThisNumberOfSpheres > 0) &&
+ (sphereCounter > gpConfig->stopAfterAnalyzingThisNumberOfSpheres))
+ {
+ if (verbose)
+ {
+
+ VR_INFO << "Stopping after analyzing " << sphereCounter
+ << " medial spheres. " << std::endl;
+ }
+ break;
+ }
+ }
+ return (candidateGrasps.size()>0);
+}
+
+int MatGraspPlanner::plan(int nrGrasps, int timeOutMS)
+{
+ if (!initialized)
+ init();
+ startTime = clock();
+ this->timeOutMS = timeOutMS;
+
+ int nLoop = 0;
+ int nGraspsCreated = 0;
+
+ if (verbose)
+ {
+ GRASPSTUDIO_INFO << ": Searching " << nrGrasps << " grasps for EEF:" << eef->getName() << " and object:" << graspQuality->getObject()->getName() << ".\n";
+ GRASPSTUDIO_INFO << ": Grasps are evaluated with " << graspQuality->getName() << endl;
+ }
+
+ while (!timeout() && nGraspsCreated < nrGrasps)
+ {
+ VirtualRobot::GraspPtr g = planGrasp();
+ if (g)
+ {
+ if (graspSet)
+ graspSet->addGrasp(g);
+ plannedGrasps.push_back(g);
+ nGraspsCreated++;
+ }
+ nLoop++;
+ }
+ if (verbose)
+ GRASPSTUDIO_INFO << ": created " << nGraspsCreated << " valid grasps in " << nLoop << " loops" << endl;
+ return nGraspsCreated;
+}
+
+VirtualRobot::GraspPtr MatGraspPlanner::planGrasp()
+{
+ /*
+ std::string sGraspPlanner("Simox - GraspStudio - ");
+ sGraspPlanner += graspQuality->getName();
+ std::string sGraspNameBase = "Grasp ";
+
+ VirtualRobot::RobotPtr robot = approach->getEEFOriginal()->getRobot();
+ VirtualRobot::RobotNodePtr tcp = eef->getTcp();
+
+ VR_ASSERT(robot);
+ VR_ASSERT(tcp);
+
+
+ bool bRes = approach->setEEFToRandomApproachPose();
+ if (!bRes)
+ return VirtualRobot::GraspPtr();
+
+ VirtualRobot::EndEffector::ContactInfoVector contacts;
+ contacts = eef->closeActors(object);
+
+ if (contacts.size()<2)
+ {
+ if (verbose)
+ GRASPSTUDIO_INFO << ": ignoring grasp hypothesis, low number of contacts" << endl;
+ return VirtualRobot::GraspPtr();
+ }
+
+ graspQuality->setContactPoints(contacts);
+ float score = graspQuality->getGraspQuality();
+ if (score<minQuality)
+ return VirtualRobot::GraspPtr();
+ if (forceClosure && !graspQuality->isGraspForceClosure())
+ return VirtualRobot::GraspPtr();
+
+ // found valid grasp
+ if (verbose)
+ {
+ GRASPSTUDIO_INFO << ": Found grasp with " << contacts.size() << " contacts, score: " << score << endl;
+ }
+ std::stringstream ss;
+ ss << sGraspNameBase << (graspSet->getSize()+1);
+ std::string sGraspName = ss.str();
+ Eigen::Matrix4f objP = object->getGlobalPose();
+ Eigen::Matrix4f pLocal = tcp->toLocalCoordinateSystem(objP);
+ VirtualRobot::GraspPtr g(new VirtualRobot::Grasp(sGraspName,robot->getType(),eef->getName(),pLocal,sGraspPlanner,score));
+ // set joint config
+ VirtualRobot::RobotConfigPtr config = eef->getConfiguration();
+ std::map< std::string, float > configValues = config->getRobotNodeJointValueMap();
+ g->setConfiguration(configValues);
+ return g;
+ */
+ VirtualRobot::GraspPtr gp;
+ return gp;
+}
+
+bool MatGraspPlanner::timeout()
+{
+ if (timeOutMS<=0)
+ return false;
+ clock_t endTime = clock();
+ int timeMS = (int)(((float)(endTime - startTime) / (float)CLOCKS_PER_SEC) * 1000.0);
+ return (timeMS > timeOutMS);
+}
+
+
+
+void MatGraspPlanner::executePowercrust()
+{
+ surfacePoints.clear();
+ surfacePoints = getSurfacePoints(0.001f); // convert from MM to M
+
+ if (verbose)
+ VR_INFO << "Nr of points: " << surfacePoints.size() << endl;
+
+ pc.reset(new PowerCrust::PowerCrust());
+ pc->setVerbose(verbose);
+ pc->init(surfacePoints);
+ pc->computeMedialAxis();
+ size_t numPolarBalls = pc->resultingPolarBalls.size();
+
+ if (verbose)
+ VR_INFO << "MAT computation complete. Number of resulting polar balls: " << numPolarBalls << endl;
+}
+
+std::vector<Eigen::Vector3f> MatGraspPlanner::getSurfacePoints(float scaling, bool checkForDoubledEntries)
+{
+ std::vector<Eigen::Vector3f> res;
+ if (!object || !object->getCollisionModel() || !object->getCollisionModel()->getTriMeshModel())
+ {
+ VR_ERROR << "No collision model..." << endl;
+ return res;
+ }
+
+ TriMeshModelPtr tm = object->getCollisionModel()->getTriMeshModel();
+ for (size_t i=0; i<tm->vertices.size(); i++)
+ {
+ bool entryFound=false;
+ if (checkForDoubledEntries)
+ for (size_t j=0;j<res.size();j++)
+ {
+ if (res[j] == tm->vertices[i])
+ {
+ entryFound = true;
+ break;
+ }
+ }
+ if (!entryFound)
+ res.push_back(tm->vertices[i]*scaling);
+ }
+ return res;
+}
+
+
+std::vector<MedialSpherePtr> MatGraspPlanner::getMedialSpheres()
+{
+ return medialSpheres;
+}
+
+std::vector<MedialSpherePtr> MatGraspPlanner::getMedialSpheresFiltered()
+{
+ return medialSpheresFiltered;
+}
+
+std::vector<LocalNeighborhoodPtr> MatGraspPlanner::getLocalNeighborhoods()
+{
+ return localNeighborhoods;
+}
+
+std::vector<CandidateGraspPtr> MatGraspPlanner::getCandidateGrasps()
+{
+ return candidateGrasps;
+}
+
+std::vector<Eigen::Vector3f> MatGraspPlanner::getSurfacePoints()
+{
+ return surfacePoints;
+}
+
+void MatGraspPlanner::testCandidate(CandidateGraspPtr candidate)
+{
+ if (!candidate || !cgTester)
+ return;
+
+ cgTester->testCandidate(candidate);
+}
+
+} // namespace
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/MatGraspPlanner.h b/GraspPlanning/GraspPlanner/MATPlanner/MatGraspPlanner.h
new file mode 100644
index 000000000..cec71024d
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/MatGraspPlanner.h
@@ -0,0 +1,151 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski, Nikolaus Vahrenkamp
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#ifndef __MAT_GRASP_PLANNER_H__
+#define __MAT_GRASP_PLANNER_H__
+
+#include "../../GraspStudio.h"
+#include <GraspPlanning/GraspStudio.h>
+#include <GraspPlanning/GraspPlanner/GraspPlanner.h>
+#include <GraspPlanning/GraspQuality/GraspQualityMeasure.h>
+#include "powercrust.h"
+
+
+#include "MedialSphere.h"
+#include "Converter.h"
+#include "SphereHelpers.h"
+#include "LocalNeighborhood.h"
+#include "CandidateGraspGenerator.h"
+#include "CandidateGraspTester.h"
+#include "GridOfMedialSpheres.h"
+
+
+
+namespace GraspStudio
+{
+/*!
+*
+*
+* A general grasp planning class that utilizes ApprachMovementGenerator for generating grasp hypothesis and
+* GraspQualityMeasure to score them.
+*
+*/
+ class MatGraspPlanner;
+ typedef boost::shared_ptr<GraspStudio::MatGraspPlanner> MatGraspPlannerPtr;
+
+class GRASPSTUDIO_IMPORT_EXPORT MatGraspPlanner : public GraspPlanner
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /*!
+ Constructor
+ \param graspSet All planned grasps are added to this GraspSet.
+ \param eef
+ \param graspQuality The quality of generated grasps is evaluated with this object
+ \param gpConfig If not given, a standard setup is used.
+ \param minQuality The quality that must be achieved at minimum by the GraspQualityMesurement module
+ \param forceClosure When true, only force closure grasps are generated.
+ */
+ MatGraspPlanner(VirtualRobot::GraspSetPtr graspSet,
+ VirtualRobot::RobotPtr robot,
+ VirtualRobot::EndEffectorPtr eef,
+ GraspStudio::GraspQualityMeasureWrenchSpacePtr graspQuality,
+ GraspPlannerConfigurationPtr gpConfig = GraspPlannerConfigurationPtr(),
+ float minQuality = 0.0f,
+ bool forceClosure = true,
+ bool verbose = false);
+
+ // destructor
+ virtual ~MatGraspPlanner();
+
+
+ bool init();
+
+ /*!
+ Creates new grasps.
+ \param nrGrasps The number of grasps to be planned.
+ \param timeOutMS The time out in milliseconds. Planning is stopped when this time is exceeded. Disabled when zero.
+ \return Number of generated grasps.
+ */
+ virtual int plan(int nrGrasps, int timeOutMS = 0);
+
+
+ std::vector<MedialSpherePtr> getMedialSpheres();
+ std::vector<MedialSpherePtr> getMedialSpheresFiltered();
+
+ std::vector<LocalNeighborhoodPtr> getLocalNeighborhoods();
+ std::vector<CandidateGraspPtr> getCandidateGrasps();
+
+ std::vector<Eigen::Vector3f> getSurfacePoints();
+
+ /*!
+
+ */
+ void testCandidate(CandidateGraspPtr candidate);
+protected:
+
+ void executePowercrust();
+ std::vector<Eigen::Vector3f> getSurfacePoints(float scaling, bool checkForDoubledEntries=false);
+
+ std::vector<CandidateGraspPtr> computeCandidates(MedialSpherePtr ms);
+ bool computeAllCandidates();
+
+ bool timeout();
+ std::vector<Eigen::Vector3f> surfacePoints;
+ GraspStudio::PowerCrust::PowerCrustPtr pc;
+
+ VirtualRobot::GraspPtr planGrasp();
+
+ VirtualRobot::SceneObjectPtr object;
+ VirtualRobot::EndEffectorPtr eef;
+
+ bool initialized;
+
+ clock_t startTime;
+ int timeOutMS;
+
+ GraspStudio::GraspQualityMeasureWrenchSpacePtr graspQuality;
+ float minQuality;
+ bool forceClosure;
+
+ //Grasp planner components:
+ GridOfMedialSpheresPtr goms;
+ CandidateGraspGeneratorPtr cgGenerator;
+ CandidateGraspTesterPtr cgTester;
+ GraspPlannerConfigurationPtr gpConfig;
+
+ //some data structures fro the grasp planner:
+ std::vector<MedialSpherePtr> medialSpheres;
+ std::vector<MedialSpherePtr> medialSpheresFiltered;
+ std::vector<LocalNeighborhoodPtr> localNeighborhoods;
+ std::vector<CandidateGraspPtr> candidateGrasps;
+
+ //objects necessary for testing candidate grasps:
+ std::string eefName;
+ //GraspStudio::GraspQualityMeasureWrenchSpacePtr qualityMeasure;
+ VirtualRobot::RobotPtr robot;
+
+ };
+}
+
+#endif /* __MAT_GRASP_PLANNER_H__ */
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/MedialSphere.cpp b/GraspPlanning/GraspPlanner/MATPlanner/MedialSphere.cpp
new file mode 100644
index 000000000..6ad466ed5
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/MedialSphere.cpp
@@ -0,0 +1,115 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include "MedialSphere.h"
+#include "SphereHelpers.h"
+
+using namespace std;
+namespace GraspStudio
+{
+
+ MedialSphere::MedialSphere()
+ {
+ }
+
+ MedialSphere::MedialSphere(Eigen::Vector3f center, float radius, vector<Eigen::Vector3f> surfacePointCoordinates)
+ {
+ this->center = center;
+ this->radius = radius;
+ this->surfacePointCoordinates = surfacePointCoordinates;
+
+ this->computeVectorsToSurfacePointsAndObjectAngle();
+
+ }
+
+ MedialSphere::~MedialSphere()
+ {
+ }
+
+ MedialSpherePtr MedialSphere::clone()
+ {
+ MedialSpherePtr cloneSphere = MedialSpherePtr(new MedialSphere);
+ cloneSphere->center = this->center;
+ cloneSphere->radius = this->radius;
+ cloneSphere->surfacePointCoordinates = this->surfacePointCoordinates;
+ cloneSphere->vectorsToSurfacePoints = this->vectorsToSurfacePoints;
+ cloneSphere->objectAngle = this->objectAngle;
+
+ return cloneSphere;
+ }
+
+
+ void MedialSphere::computeVectorsToSurfacePointsAndObjectAngle()
+ {
+ Eigen::Vector3f v;
+
+ vectorsToSurfacePoints.clear();
+
+ for (size_t i = 0; i < surfacePointCoordinates.size(); i++)
+ {
+ v = surfacePointCoordinates.at(i) - center;
+ this->vectorsToSurfacePoints.push_back(v);
+ }
+
+ float currentAngleDeg = -1;
+ float maxAngleDeg = -1;
+
+ for (size_t j = 0; j < vectorsToSurfacePoints.size(); j++)
+ for (size_t k = j + 1; k<vectorsToSurfacePoints.size(); k++)
+ {
+ currentAngleDeg = SphereHelpers::calcAngleBetweenTwoVectorsDeg(vectorsToSurfacePoints.at(j), vectorsToSurfacePoints.at(k));
+ if (currentAngleDeg > maxAngleDeg)
+ maxAngleDeg = currentAngleDeg;
+ }
+
+ this->objectAngle = maxAngleDeg;
+
+ }
+
+ void MedialSphere::printDebug()
+ {
+ std::cout << "=== MedialSphere ===" << std::endl;
+ std::cout << "center: " << center(0) << " " << center(1) << " " << center(2) << std::endl;
+ std::cout << "radius: " << radius << std::endl;
+ std::cout << "surfacePointCoordinates: " << StrOutHelpers::toString(surfacePointCoordinates) << std::endl;
+ std::cout << "vectorsToSurfacePoints: " << StrOutHelpers::toString(vectorsToSurfacePoints) << std::endl;
+ std::cout << "objectAngle: " << objectAngle << std::endl;
+ }
+
+ void MedialSphere::scale(float scaleFactor)
+ {
+ radius = scaleFactor * radius;
+ center = scaleFactor * center;
+
+ //objectAngle stays the same! No scaling!
+
+ for (size_t i = 0; i < surfacePointCoordinates.size(); i++)
+ {
+ surfacePointCoordinates.at(i) = scaleFactor * surfacePointCoordinates.at(i);
+ }
+
+ computeVectorsToSurfacePointsAndObjectAngle();
+
+ }
+
+
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/MedialSphere.h b/GraspPlanning/GraspPlanner/MATPlanner/MedialSphere.h
new file mode 100644
index 000000000..38a1d81b6
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/MedialSphere.h
@@ -0,0 +1,67 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#ifndef MEDIALSPHERE_H
+#define MEDIALSPHERE_H
+
+#include "../../GraspStudio.h"
+#include <vector>
+#include <Eigen/Geometry>
+#include <Inventor/nodes/SoSeparator.h>
+
+#include "StrOutHelpers.h"
+
+namespace GraspStudio
+{
+
+ class MedialSphere;
+ typedef boost::shared_ptr<MedialSphere> MedialSpherePtr;
+
+ class GRASPSTUDIO_IMPORT_EXPORT MedialSphere
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ MedialSphere();
+ MedialSphere(Eigen::Vector3f center, float radius, std::vector<Eigen::Vector3f> surfacePointCoordinates);
+
+ ~MedialSphere();
+
+ MedialSpherePtr clone();
+ void printDebug();
+ void scale(float scaleFactor);
+
+ //data members
+ Eigen::Vector3f center;
+ float radius;
+
+ std::vector<Eigen::Vector3f> surfacePointCoordinates;
+ std::vector<Eigen::Vector3f> vectorsToSurfacePoints;
+
+ float objectAngle; //The biggest angle included between two vectors from the sphere's center to two different surface points.
+
+ protected:
+ void computeVectorsToSurfacePointsAndObjectAngle();
+
+ };
+}
+#endif // MEDIALSPHERE_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.cpp b/GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.cpp
new file mode 100644
index 000000000..aec423353
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.cpp
@@ -0,0 +1,227 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Nikolaus Vahrenkamp
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include "MeshConverter.h"
+
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+
+using namespace std;
+using namespace Eigen;
+using namespace VirtualRobot;
+
+namespace GraspStudio
+{
+
+
+ int MeshConverter::hasVertex(std::vector< Eigen::Vector3f> &vectList, Eigen::Vector3f &obj)
+ {
+ for (size_t j = 0; j < vectList.size(); j++)
+ {
+ if (vectList[j] == obj)
+ {
+ return j;
+ }
+ }
+ return -1;
+ }
+
+ VirtualRobot::ObstaclePtr MeshConverter::refineObjectSurface(VirtualRobot::ObstaclePtr object, float maxDist)
+ {
+ VirtualRobot::ObstaclePtr res;
+ if (!object || !object->getCollisionModel())
+ return res;
+
+ TriMeshModelPtr tm = object->getCollisionModel()->getTriMeshModel();
+ if (!tm)
+ return res;
+ VR_INFO << "Processing object with " << tm->faces.size() << " triangles" << endl;
+
+ // first create new object
+ TriMeshModelPtr triMesh2(new TriMeshModel());
+ int check;
+ for (size_t i = 0; i<tm->faces.size(); i++)
+ {
+ Eigen::Vector3f v1, v2, v3;
+ v1 = tm->vertices[tm->faces[i].id1];
+ v2 = tm->vertices[tm->faces[i].id2];
+ v3 = tm->vertices[tm->faces[i].id3];
+ unsigned int id1, id2, id3;
+ check = hasVertex(triMesh2->vertices, v1);
+ if (check>0)
+ id1 = (int)check;
+ else
+ {
+ // add vertex
+ triMesh2->addVertex(v1);
+ id1 = triMesh2->vertices.size() - 1;
+ }
+ check = hasVertex(triMesh2->vertices, v2);
+ if (check > 0)
+ id2 = (int)check;
+ else
+ {
+ // add vertex
+ triMesh2->addVertex(v2);
+ id2 = triMesh2->vertices.size() - 1;
+ }
+ check = hasVertex(triMesh2->vertices, v3);
+ if (check > 0)
+ id3 = (int)check;
+ else
+ {
+ // add vertex
+ triMesh2->addVertex(v3);
+ id3 = triMesh2->vertices.size() - 1;
+ }
+
+ // create face
+ MathTools::TriangleFace face;
+ face.id1 = id1;
+ face.id2 = id2;
+ face.id3 = id3;
+
+ face.normal = tm->faces[i].normal;
+
+ triMesh2->addFace(face);
+ }
+
+
+ for (size_t i = 0; i < triMesh2->faces.size(); i++)
+ {
+ checkAndSplitVertex(triMesh2, i, maxDist);
+ }
+ CoinVisualizationFactoryPtr cv(new CoinVisualizationFactory());
+ Eigen::Matrix4f gp = object->getGlobalPose();
+ VisualizationNodePtr visu = cv->createTriMeshModelVisualization(triMesh2, false, gp);
+ CollisionModelPtr cm(new CollisionModel(visu));
+ res.reset(new Obstacle(object->getName(), visu, cm));
+ return res;
+ }
+
+ void MeshConverter::checkAndSplitVertex(VirtualRobot::TriMeshModelPtr tm, int faceIdx, float maxDist)
+ {
+ VR_ASSERT(tm);
+ VR_ASSERT(faceIdx >= 0 && faceIdx < (int)tm->faces.size());
+
+ float d12, d13, d23;
+ Eigen::Vector3f v1, v2, v3;
+ v1 = tm->vertices[tm->faces[faceIdx].id1];
+ v2 = tm->vertices[tm->faces[faceIdx].id2];
+ v3 = tm->vertices[tm->faces[faceIdx].id3];
+ Eigen::Vector3f v4;
+ unsigned int id4;
+ size_t checkFaceIdx;
+
+ d12 = (v1 - v2).norm();
+ d23 = (v2 - v3).norm();
+ d13 = (v1 - v3).norm();
+ if (d12 > maxDist || d23 > maxDist || d13 > maxDist)
+ {
+ // split at longest edge
+ if (d12 >= d23 && d12 >= d13)
+ {
+ v4 = v1 + (v2 - v1)*0.5f;
+ tm->addVertex(v4);
+ id4 = tm->vertices.size() - 1;
+
+ // add new face
+ MathTools::TriangleFace face;
+ face.id1 = id4;
+ face.id2 = tm->faces[faceIdx].id2;
+ face.id3 = tm->faces[faceIdx].id3;
+ face.normal = tm->faces[faceIdx].normal;
+ tm->addFace(face);
+
+ // update current face
+ tm->faces[faceIdx].id2 = id4;
+ }
+ else if (d23 >= d12 && d23 >= d13)
+ {
+ v4 = v2 + (v3 - v2)*0.5f;
+ tm->addVertex(v4);
+ id4 = tm->vertices.size() - 1;
+
+ // add new face
+ MathTools::TriangleFace face;
+ face.id1 = tm->faces[faceIdx].id1;
+ face.id2 = id4;
+ face.id3 = tm->faces[faceIdx].id3;
+ face.normal = tm->faces[faceIdx].normal;
+ tm->addFace(face);
+
+ // update current face
+ tm->faces[faceIdx].id3 = id4;
+ }
+ else
+ {
+ v4 = v3 + (v1 - v3)*0.5f;
+ tm->addVertex(v4);
+ id4 = tm->vertices.size() - 1;
+
+ // add new face
+ MathTools::TriangleFace face;
+ face.id1 = tm->faces[faceIdx].id1;
+ face.id2 = tm->faces[faceIdx].id2;
+ face.id3 = id4;
+ face.normal = tm->faces[faceIdx].normal;
+ tm->addFace(face);
+
+ // update current face
+ tm->faces[faceIdx].id1 = id4;
+
+ }
+ checkFaceIdx = tm->faces.size() - 1;
+ // check new face
+ checkAndSplitVertex(tm, checkFaceIdx, maxDist);
+ // recursively check current face
+ checkAndSplitVertex(tm, faceIdx, maxDist);
+ }
+ }
+
+ float MeshConverter::getMaxVertexDistance(VirtualRobot::TriMeshModelPtr tm)
+ {
+ if (!tm)
+ return 0.0f;
+
+ float maxDist = 0;
+ for (size_t i = 0; i<tm->faces.size(); i++)
+ {
+ Eigen::Vector3f v1, v2, v3;
+ v1 = tm->vertices[tm->faces[i].id1];
+ v2 = tm->vertices[tm->faces[i].id2];
+ v3 = tm->vertices[tm->faces[i].id3];
+
+ float d12 = (v1 - v2).norm();
+ float d23 = (v2 - v3).norm();
+ float d13 = (v1 - v3).norm();
+ if (d12>maxDist)
+ maxDist = d12;
+ if (d23 > maxDist)
+ maxDist = d23;
+ if (d13 > maxDist)
+ maxDist = d13;
+ }
+ return maxDist;
+
+ }
+
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.h b/GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.h
new file mode 100644
index 000000000..edea66571
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/MeshConverter.h
@@ -0,0 +1,49 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Nikolaus Vahrenkamp
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#ifndef MESHCONVERTER_H
+#define MESHCONVERTER_H
+
+#include "../../GraspStudio.h"
+#include <vector>
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/Visualization/TriMeshModel.h>
+
+namespace GraspStudio
+{
+
+ class GRASPSTUDIO_IMPORT_EXPORT MeshConverter
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ 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);
+ static void checkAndSplitVertex(VirtualRobot::TriMeshModelPtr tm, int faceIdx, float maxDist);
+
+ static float getMaxVertexDistance(VirtualRobot::TriMeshModelPtr tm);
+
+ };
+}
+#endif // MESHCONVERTER_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/SphereHelpers.cpp b/GraspPlanning/GraspPlanner/MATPlanner/SphereHelpers.cpp
new file mode 100644
index 000000000..2ce512b5a
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/SphereHelpers.cpp
@@ -0,0 +1,280 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include "SphereHelpers.h"
+using namespace Eigen;
+
+namespace GraspStudio
+{
+
+ SphereHelpers::SphereHelpers()
+ {
+ }
+
+
+ float SphereHelpers::findMinSphereRadius(std::vector<MedialSpherePtr>& spheres)
+ {
+ float minRadius = 1000000.0;
+ for (size_t i = 0; i < spheres.size(); i++)
+ {
+ if (spheres.at(i)->radius < minRadius)
+ minRadius = spheres.at(i)->radius;
+ }
+ return minRadius;
+ }
+
+ float SphereHelpers::findMaxSphereRadius(std::vector<MedialSpherePtr>& spheres)
+ {
+ float maxRadius = -1.0;
+ for (size_t i = 0; i<spheres.size(); i++)
+ {
+ if (spheres.at(i)->radius > maxRadius)
+ maxRadius = spheres.at(i)->radius;
+ }
+ return maxRadius;
+ }
+
+ float SphereHelpers::findMinObjectAngleDegrees(std::vector<MedialSpherePtr>& spheres)
+ {
+ float minObjectAngleDegrees = 360.0;
+ for (size_t i = 0; i < spheres.size(); i++)
+ {
+ if (spheres.at(i)->objectAngle < minObjectAngleDegrees)
+ minObjectAngleDegrees = spheres.at(i)->objectAngle;
+ }
+ return minObjectAngleDegrees;
+ }
+
+ float SphereHelpers::findMaxObjectAngleDegrees(std::vector<MedialSpherePtr>& spheres)
+ {
+ float maxObjectAngleDegrees = -1.0;
+ for (size_t i = 0; i<spheres.size(); i++)
+ {
+ if (spheres.at(i)->objectAngle > maxObjectAngleDegrees)
+ maxObjectAngleDegrees = spheres.at(i)->objectAngle;
+ }
+ return maxObjectAngleDegrees;
+ }
+
+ void SphereHelpers::scaleMedialSpheres(std::vector<MedialSpherePtr>& spheres, float scaleFactor)
+ {
+ for (size_t i = 0; i < spheres.size(); i++)
+ {
+ spheres.at(i)->scale(scaleFactor);
+ }
+ }
+
+ void SphereHelpers::selectSpheresWithMinRadius(std::vector<MedialSpherePtr>& spheres, float minRadius)
+ {
+ std::vector<MedialSpherePtr> resultSpheres;
+ for (size_t i = 0; i<spheres.size(); i++)
+ {
+ if (spheres.at(i)->radius > minRadius)
+ resultSpheres.push_back(spheres.at(i));
+ }
+ spheres = resultSpheres;
+ }
+
+ void SphereHelpers::selectSpheresWithMaxRadius(std::vector<MedialSpherePtr>& spheres, float maxRadius)
+ {
+ std::vector<MedialSpherePtr> resultSpheres;
+ for (size_t i = 0; i < spheres.size(); i++)
+ {
+ if (spheres.at(i)->radius < maxRadius)
+ resultSpheres.push_back(spheres.at(i));
+ }
+ spheres = resultSpheres;
+ }
+
+ void SphereHelpers::selectSpheresWithMinObjectAngle(std::vector<MedialSpherePtr>& spheres, float minObjectAngle)
+ {
+ std::vector<MedialSpherePtr> resultSpheres;
+ for (size_t i = 0; i<spheres.size(); i++)
+ {
+ if (spheres.at(i)->objectAngle > minObjectAngle)
+ resultSpheres.push_back(spheres.at(i));
+ }
+ spheres = resultSpheres;
+ }
+
+ void SphereHelpers::selectSpheresWithMaxObjectAngle(std::vector<MedialSpherePtr>& spheres, float maxObjectAngle)
+ {
+ std::vector<MedialSpherePtr> resultSpheres;
+ for (size_t i = 0; i < spheres.size(); i++)
+ {
+ if (spheres.at(i)->objectAngle < maxObjectAngle)
+ resultSpheres.push_back(spheres.at(i));
+ }
+ spheres = resultSpheres;
+ }
+
+ void SphereHelpers::selectSpheresWithConstraints(std::vector<MedialSpherePtr>& spheres, SphereConstraintsSet constraints)
+ {
+ if (constraints.minObjectAngle > -1)
+ selectSpheresWithMinObjectAngle(spheres, constraints.minObjectAngle);
+
+ if (constraints.maxObjectAngle > -1)
+ selectSpheresWithMaxObjectAngle(spheres, constraints.maxObjectAngle);
+
+ if (constraints.minRadius > -1)
+ selectSpheresWithMinRadius(spheres, constraints.minRadius);
+
+ if (constraints.maxRadius > -1)
+ selectSpheresWithMaxRadius(spheres, constraints.maxRadius);
+ }
+
+ SphereConstraintsSet SphereHelpers::getSphereConstraintsSet(std::vector<MedialSpherePtr>& spheres)
+ {
+ float minRadius = findMinSphereRadius(spheres);
+ float maxRadius = findMaxSphereRadius(spheres);
+ float minObjectAngle = findMinObjectAngleDegrees(spheres);
+ float maxObjectAngle = findMaxObjectAngleDegrees(spheres);
+ int numSpheres = spheres.size();
+
+ SphereConstraintsSet scs(minRadius, maxRadius, minObjectAngle, maxObjectAngle, numSpheres);
+
+ return scs;
+ }
+
+ void SphereHelpers::filterSpheres(std::vector<MedialSpherePtr>& spheres, float minObjAngle, float minRadiusRelative, bool verbose)
+ {
+ if (verbose)
+ {
+ std::cout << "SphereHelpers::filterSpheres() called." << std::endl;
+ }
+ SphereConstraintsSet scs0;
+ scs0 = SphereHelpers::getSphereConstraintsSet(spheres);
+
+ SphereConstraintsSet scs1;
+ scs1.minObjectAngle = minObjAngle;
+ scs1.minRadius = minRadiusRelative * scs0.maxRadius;
+ if (verbose)
+ {
+ std::cout << "\n scs: " << std::endl;
+ scs1.printDebug();
+ }
+ SphereHelpers::selectSpheresWithConstraints(spheres, scs1);
+
+ SphereConstraintsSet scs2;
+ scs2 = SphereHelpers::getSphereConstraintsSet(spheres);
+ if (verbose)
+ {
+ scs2.printDebug();
+ std::cout << "SphereHelpers::filterSpheres() done." << std::endl;
+ }
+
+ }
+
+ std::vector<MedialSpherePtr> SphereHelpers::getSpheresInsideSearchRadius(std::vector<MedialSpherePtr>& spheresToBeFiltered, MedialSpherePtr querySphere, float searchRadius)
+ {
+ std::vector<MedialSpherePtr> spheresInsideSearchRadius;
+
+ for (size_t i = 0; i < spheresToBeFiltered.size(); i++)
+ {
+ Eigen::Vector3f distVect = querySphere->center - spheresToBeFiltered.at(i)->center;
+ float distance = distVect.norm();
+
+ //std::cout << "(getSpheresInsideSearchRadius()) distance: " << distance << std::endl;
+ if (distance <= searchRadius)
+ {
+ //std::cout << "Adding this sphere." << std::endl;
+ spheresInsideSearchRadius.push_back(spheresToBeFiltered.at(i));
+ }
+ }
+
+ // std::cout << "getSpheresInsideSearchRadius() got " << spheresInsideSearchRadius.size()
+ // << " spheres, leaving... " << std::endl;
+
+ return spheresInsideSearchRadius;
+ }
+
+ std::vector<Eigen::Vector3f> SphereHelpers::getSphereCenters(std::vector<MedialSpherePtr>& spheres)
+ {
+ std::vector<Eigen::Vector3f> sphereCenters;
+ for (size_t i = 0; i < spheres.size(); i++)
+ sphereCenters.push_back(spheres.at(i)->center);
+
+ return sphereCenters;
+ }
+
+
+ SphereConstraintsSet::SphereConstraintsSet()
+ {
+ minRadius = -1;
+ maxRadius = -1;
+ minObjectAngle = -1;
+ maxObjectAngle = -1;
+
+ numberOfSpheres = -1;
+ }
+
+ SphereConstraintsSet::SphereConstraintsSet(float r_min, float r_max, float alpha_min, float alpha_max, int numSpheres)
+ {
+ minRadius = r_min;
+ maxRadius = r_max;
+ minObjectAngle = alpha_min;
+ maxObjectAngle = alpha_max;
+
+ numberOfSpheres = numSpheres;
+ }
+
+ void SphereConstraintsSet::printDebug()
+ {
+ std::cout << "=== SphereConstraintsSet ===" << std::endl;
+ std::cout << "minRadius: " << minRadius << std::endl;
+ std::cout << "maxRadius: " << maxRadius << std::endl;
+ std::cout << "minObjectAngle: " << minObjectAngle << std::endl;
+ std::cout << "maxObjectAngle: " << maxObjectAngle << std::endl;
+ std::cout << "numberOfSpheres: " << numberOfSpheres << std::endl;
+ }
+
+
+ float SphereHelpers::calcAngleBetweenTwoVectorsRad(Vector3f a, Vector3f b)
+ {
+ float length_a = sqrt(a.dot(a));
+ float length_b = sqrt(b.dot(b));
+
+ float denominator = length_a * length_b;
+ float numerator = a.dot(b);
+
+ return acos(numerator / denominator);
+ }
+
+ float SphereHelpers::calcAngleBetweenTwoVectorsDeg(Vector3f a, Vector3f b)
+ {
+ float angleRad = calcAngleBetweenTwoVectorsRad(a, b);
+ return (angleRad*180.0f) / (float)M_PI; //TODO: PI woher?
+
+ }
+ MatrixXf SphereHelpers::toMatrix_3xN(std::vector<Vector3f> points)
+ {
+ MatrixXf m;
+ m.resize(3, points.size());
+
+ for (size_t i = 0; i<points.size(); i++)
+ {
+ m.block<3, 1>(0, i) = points.at(i);
+ }
+
+ return m;
+ }
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/SphereHelpers.h b/GraspPlanning/GraspPlanner/MATPlanner/SphereHelpers.h
new file mode 100644
index 000000000..d7acafc82
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/SphereHelpers.h
@@ -0,0 +1,87 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#ifndef SPHEREHELPERS_H
+#define SPHEREHELPERS_H
+
+#include "../../GraspStudio.h"
+#include "MedialSphere.h"
+
+namespace GraspStudio
+{
+
+ class GRASPSTUDIO_IMPORT_EXPORT SphereConstraintsSet
+ {
+ public:
+ SphereConstraintsSet();
+ SphereConstraintsSet(float r_min, float r_max, float alpha_min, float alpha_max, int numSpheres = -1);
+
+ void printDebug();
+
+ float minRadius;
+ float maxRadius;
+ float minObjectAngle;
+ float maxObjectAngle;
+
+ int numberOfSpheres;
+ };
+
+ class GRASPSTUDIO_IMPORT_EXPORT SphereHelpers
+ {
+ public:
+ SphereHelpers();
+
+ static float findMinSphereRadius(std::vector<MedialSpherePtr>& spheres);
+ static float findMaxSphereRadius(std::vector<MedialSpherePtr>& spheres);
+
+ static float findMinObjectAngleDegrees(std::vector<MedialSpherePtr>& spheres);
+ static float findMaxObjectAngleDegrees(std::vector<MedialSpherePtr>& spheres);
+
+ static void scaleMedialSpheres(std::vector<MedialSpherePtr>& spheres, float scaleFactor);
+
+ static void selectSpheresWithMinRadius(std::vector<MedialSpherePtr>& spheres, float minRadius);
+ static void selectSpheresWithMaxRadius(std::vector<MedialSpherePtr>& spheres, float maxRadius);
+
+ static void selectSpheresWithMinObjectAngle(std::vector<MedialSpherePtr>& spheres, float minObjectAngle);
+ static void selectSpheresWithMaxObjectAngle(std::vector<MedialSpherePtr>& spheres, float maxObjectAngle);
+
+ static void selectSpheresWithConstraints(std::vector<MedialSpherePtr>& spheres, SphereConstraintsSet constraints);
+
+ static SphereConstraintsSet getSphereConstraintsSet(std::vector<MedialSpherePtr>& spheres);
+
+ static void filterSpheres(std::vector<MedialSpherePtr>& spheres, float minObjAngle, float minRadiusRelative, bool verbose = false);
+
+ static std::vector<MedialSpherePtr> getSpheresInsideSearchRadius(std::vector<MedialSpherePtr>& spheresToBeFiltered, MedialSpherePtr querySphere, float searchRadius);
+
+ static std::vector<Eigen::Vector3f> getSphereCenters(std::vector<MedialSpherePtr>& spheres);
+
+ //calc angle in radians
+ static float calcAngleBetweenTwoVectorsRad(Eigen::Vector3f v1, Eigen::Vector3f v2);
+ //calc angle in degrees
+ static float calcAngleBetweenTwoVectorsDeg(Eigen::Vector3f v1, Eigen::Vector3f v2);
+
+ static Eigen::MatrixXf toMatrix_3xN(std::vector<Eigen::Vector3f> points);
+
+ };
+}
+
+#endif // SPHEREHELPERS_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/StrOutHelpers.cpp b/GraspPlanning/GraspPlanner/MATPlanner/StrOutHelpers.cpp
new file mode 100644
index 000000000..2d3e8daa0
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/StrOutHelpers.cpp
@@ -0,0 +1,87 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#include "StrOutHelpers.h"
+
+using namespace std;
+
+namespace GraspStudio
+{
+
+ StrOutHelpers::StrOutHelpers()
+ {
+ }
+
+ string StrOutHelpers::toString(vector<Eigen::Vector3f> v, bool lineBreak)
+ {
+ stringstream sstr;
+ sstr << endl;
+
+ string endLine("");
+ if (lineBreak)
+ endLine = "\n";
+
+ for (size_t i = 0; i < v.size(); i++)
+ {
+ sstr << "[" << (v.at(i))(0) << " " << (v.at(i))(1) << " " << (v.at(i))(2)
+ << "]; " << endLine;
+ }
+
+ return sstr.str();
+ }
+
+ string StrOutHelpers::toString(vector<Eigen::Vector3i> v, bool lineBreak)
+ {
+ stringstream sstr;
+ sstr << endl;
+
+ string endLine("");
+ if (lineBreak)
+ endLine = "\n";
+
+ for (size_t i = 0; i < v.size(); i++)
+ {
+ sstr << "[" << (v.at(i))(0) << " " << (v.at(i))(1) << " " << (v.at(i))(2)
+ << "]; " << endLine;
+ }
+
+ return sstr.str();
+ }
+
+ string StrOutHelpers::toString(Eigen::Vector3f v)
+ {
+ stringstream sstr;
+ sstr << endl;
+ sstr << "[" << v(0) << " " << v(1) << " " << v(2) << "]; ";
+
+ return sstr.str();
+ }
+
+ string StrOutHelpers::toString(Eigen::Vector3i v)
+ {
+ stringstream sstr;
+ sstr << endl;
+ sstr << "[" << v(0) << " " << v(1) << " " << v(2) << "]; ";
+
+ return sstr.str();
+ }
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/StrOutHelpers.h b/GraspPlanning/GraspPlanner/MATPlanner/StrOutHelpers.h
new file mode 100644
index 000000000..7f1c6c928
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/StrOutHelpers.h
@@ -0,0 +1,50 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package GraspStudio
+* @author Markus Przybylski
+* @copyright 2013 H2T,KIT
+* GNU Lesser General Public License
+*
+*/
+#ifndef STROUTHELPERS_H
+#define STROUTHELPERS_H
+
+#include "../../GraspStudio.h"
+#include <vector>
+#include <Eigen/Geometry>
+#include <sstream>
+#include <string>
+#include "VirtualRobot/VirtualRobot.h"
+namespace GraspStudio
+{
+
+ class GRASPSTUDIO_IMPORT_EXPORT StrOutHelpers
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ StrOutHelpers();
+
+ static std::string toString(std::vector<Eigen::Vector3f> v, bool lineBreak = false);
+ static std::string toString(std::vector<Eigen::Vector3i> v, bool lineBreak = false);
+
+ static std::string toString(Eigen::Vector3f v);
+ static std::string toString(Eigen::Vector3i v);
+
+ };
+}
+#endif // STROUTHELPERS_H
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/TestCases.cpp b/GraspPlanning/GraspPlanner/MATPlanner/TestCases.cpp
new file mode 100644
index 000000000..0cf9773f8
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/TestCases.cpp
@@ -0,0 +1,726 @@
+#include "TestCases.h"
+
+using namespace std;
+namespace GraspStudio
+{
+
+ TestCases::TestCases()
+ {
+ }
+
+ void TestCases::testScaleMedialSphere()
+ {
+ cout << "\n---> tcScaleMedialSphere()" << endl;
+
+ Eigen::Vector3f center(0, 2, 0);
+ float sphereRadius = 1.0f;
+ Eigen::Vector3f p1(-1, 2, 0);
+ Eigen::Vector3f p2(0, 1, 0);
+ Eigen::Vector3f p3(1, 2, 0);
+ Eigen::Vector3f p4(0, 3, 0);
+ vector<Eigen::Vector3f> surfacePoints;
+
+ surfacePoints.push_back(p1);
+ surfacePoints.push_back(p2);
+ surfacePoints.push_back(p3);
+ surfacePoints.push_back(p4);
+ MedialSphere ms(center, sphereRadius, surfacePoints);
+
+ cout << "MedialSphere BEFORE Scaling:" << std::endl;
+ ms.printDebug();
+
+ //Scaling:
+ float scaleFactor = 1000.0f;
+
+ cout << "\nScaling with factor " << scaleFactor << " ..." << std::endl;
+ ms.scale(scaleFactor);
+
+ cout << "MedialSphere AFTER Scaling:" << std::endl;
+ ms.printDebug();
+ cout << endl;
+
+ }
+
+ void TestCases::testScaleMedialSphere(MedialSpherePtr ms, float scaleFactor)
+ {
+ cout << "\n---> testScaleMedialSphere(ms)" << endl;
+
+ cout << "MedialSphere BEFORE Scaling:" << std::endl;
+ ms->printDebug();
+
+ cout << "\nScaling with factor " << scaleFactor << " ..." << std::endl;
+ ms->scale(scaleFactor);
+
+ cout << "MedialSphere AFTER Scaling:" << std::endl;
+ ms->printDebug();
+ cout << endl;
+ }
+
+ void TestCases::testSphereSelection(std::vector<MedialSpherePtr>& spheres)
+ {
+ cout << "\n---> testSphereSelection" << endl;
+
+ // cout << "Eigenschaften des aktuellen Kugelvektors (von Hand):" << endl;
+ // float minRadius = SphereHelpers::findMinSphereRadius(spheres);
+ // float maxRadius = SphereHelpers::findMaxSphereRadius(spheres);
+ // cout << "minRadius " << minRadius << endl;
+ // cout << "maxRadius " << maxRadius << endl;
+
+ // float minObjectAngle = SphereHelpers::findMinObjectAngleDegrees(spheres);
+ // float maxObjectAngle = SphereHelpers::findMaxObjectAngleDegrees(spheres);
+ // cout << "minObjectAngle " << minObjectAngle << endl;
+ // cout << "maxObjectAngle " << maxObjectAngle << endl;
+
+ //funktioniert nicht richtig...? --> Doch, aber verschachtelte Konstruktoren gehen nicht in dieser Version des Standards...
+ SphereConstraintsSet scs = SphereHelpers::getSphereConstraintsSet(spheres);
+ cout << "Eigenschaften des aktuellen Kugelvektors (mit SphereConstraintsSet scs):" << endl;
+ scs.printDebug();
+
+
+ // //funktioniert
+ // SphereConstraintsSet scs2;
+ // cout << "scs2:" << endl;
+ // scs2.minRadius = minRadius;
+ // scs2.printDebug();
+
+ // //funktioniert
+ // SphereConstraintsSet scs3(minRadius, maxRadius, minObjectAngle, maxObjectAngle);
+ // cout << "scs3:" << endl;
+ // scs3.printDebug();
+
+ // //funktioniert??
+ // SphereConstraintsSet scs4(minRadius, maxRadius, minObjectAngle, maxObjectAngle, spheres.size());
+ // cout << "scs4:" << endl;
+ // scs4.printDebug();
+
+
+ // ------
+ // cout << "\nSelektiere Kugeln mit >120 Grad Objektwinkel" << endl;
+ // SphereConstraintsSet scs5;
+ // scs5.minObjectAngle = 120;
+ // SphereHelpers::selectSpheresWithConstraints(spheres, scs5);
+ // SphereConstraintsSet scs6 = SphereHelpers::getSphereConstraintsSet(spheres);
+ // scs6.printDebug();
+
+ cout << "\n scs6: " << endl;
+ SphereConstraintsSet scs6 = SphereHelpers::getSphereConstraintsSet(spheres);
+ scs6.printDebug();
+
+ // cout << "\nSelektiere Kugeln mit mind. 30% des Maximalradius" << endl;
+ // SphereConstraintsSet scs7;
+ // scs7.minRadius = 0.3*scs6.maxRadius;
+ // cout << "\n scs7: " << endl;
+ // scs7.printDebug();
+
+ // SphereHelpers::selectSpheresWithConstraints(spheres, scs7);
+ // SphereConstraintsSet scs8 = SphereHelpers::getSphereConstraintsSet(spheres);
+ // cout << "\n scs8: " << endl;
+ // scs8.printDebug();
+
+
+ // ------
+ cout << "\nSelektiere Kugeln mit >120 Grad und mid. 30% des Maximalradius" << endl;
+ SphereConstraintsSet scs9;
+ scs9.minObjectAngle = 120;
+ scs9.minRadius = 0.3f*scs6.maxRadius;
+ cout << "\n scs9: " << endl;
+ scs9.printDebug();
+ SphereHelpers::selectSpheresWithConstraints(spheres, scs9);
+
+ SphereConstraintsSet scs10 = SphereHelpers::getSphereConstraintsSet(spheres);
+ cout << "\n scs10: " << endl;
+ scs10.printDebug();
+ }
+
+ //void TestCases::testSomeStuff()
+ //{
+ // //----------- test calc stuff
+
+ // Eigen::Vector3f a(1,2,3);
+ // Eigen::Vector3f b(0,1,2);
+ // Eigen::Vector3f c(1,0,0);
+
+
+ // float ab = a.dot(b);
+ // printf("a dot b: %f \n", ab);
+
+ // float angle_bc_rad = SphereHelpers::calcAngleBetweenTwoVectorsRad(b,c);
+ // float angle_bc_deg = SphereHelpers::calcAngleBetweenTwoVectorsDeg(b,c);
+ // printf("angle_bc_rad: %f \n", angle_bc_rad);
+ // printf("angle_bc_deg: %f \n", angle_bc_deg);
+
+ // //----------- test MedialSphere stuff
+
+ // Eigen::Vector3f center(0,0,0);
+ // float sphereRadius = 1.0f;
+ // Eigen::Vector3f p1(-1,0,0);
+ // Eigen::Vector3f p2(0,-1,0);
+ // Eigen::Vector3f p3(1,0,0);
+ // Eigen::Vector3f p4(0,1,0);
+ // vector<Eigen::Vector3f> surfacePoints;
+ // surfacePoints.push_back(p1);
+ // surfacePoints.push_back(p2);
+ // surfacePoints.push_back(p3);
+ // surfacePoints.push_back(p4);
+
+ // MedialSphere myTestSphere(center, sphereRadius, surfacePoints);
+
+ // myTestSphere.printDebug();
+
+ // //----------- test Grid stuff
+
+ //// GridOfMedialSpheres goms;
+ //// vector< Eigen::Vector3f > surfacePointsNew;
+ //// vector<MedialSphere> spheres;
+
+ // //goms.setup(surfacePointsNew, spheres);
+
+ // //TODO: HIER WEITER!
+
+ // //--------- test nested vectors (MP 2013-06-20)
+ //// vector<int> myTestVect;
+ //// myTestVect.reserve(100);
+ ////// myTestVect.push_back(1);
+ ////// myTestVect.push_back(3);
+ ////// myTestVect.push_back(7);
+
+ //// for (int i=0; i<myTestVect.capacity(); i++)
+ //// myTestVect.push_back(0);
+
+ //// myTestVect.at(0) = 1;
+ //// myTestVect.at(1) = 3;
+ //// myTestVect.at(2) = 7;
+
+ //// cout << "myTestVect.capacity: " << myTestVect.capacity() << endl;
+ //// cout << "myTestVect.size: " << myTestVect.size() << endl;
+ //// myTestVect.at(9) = 12;
+ //// cout << "myTestVect.size: " << myTestVect.size() << endl;
+
+ //// cout << "myTestVect contents: " << endl;
+ //// for (int i=0; i<myTestVect.size(); i++)
+ //// cout << myTestVect.at(i) << " ";
+ //// cout << endl;
+
+ // //---------- now 2D
+
+ //// vector< vector<int> > myTestVect2D;
+ //// myTestVect2D.reserve(5);
+ //// cout << "myTestVect2D.capacity: " << myTestVect2D.capacity() << endl;
+ //// cout << "myTestVect2D.size: " << myTestVect2D.size() << endl;
+ ////// cout << "myTestVect2D.at(0).capacity: " << myTestVect2D.at(0).capacity() << endl;
+ ////// cout << "myTestVect2D.at(0).size: " << myTestVect2D.at(0).size() << endl;
+
+
+ //// vector<int> vTest1;
+ //// vector<int> vTest2;
+ //// vTest1.reserve(2);
+ //// vTest2.reserve(2);
+ //// vTest1.push_back(1);
+ //// vTest2.push_back(2);
+ //// vTest1.push_back(1);
+ //// vTest2.push_back(2);
+
+
+ //// for (int i=0; i<myTestVect2D.capacity(); i++)
+ //// {
+
+
+ //// myTestVect2D.push_back(vTest1);
+ //// //myTestVect2D.push_back(vTest2);
+ //// //myTestVect2D.at(i).push_back(i);
+ //// }
+
+ //// cout << "myTestVect2D.capacity: " << myTestVect2D.capacity() << endl;
+ //// cout << "myTestVect2D.size: " << myTestVect2D.size() << endl;
+ //// myTestVect2D.at(3).push_back(900);
+ //// myTestVect2D.at(2).at(1) = 400;
+ //// myTestVect2D[1][0] = 200;
+ //// cout << "myTestVect2D.size: " << myTestVect2D.size() << endl;
+
+ //// cout << "myTestVect2D contents: " << endl;
+ //// for (int i=0; i<myTestVect2D.size(); i++)
+ //// {
+ //// for (int j=0; j<myTestVect2D.at(i).size(); j++)
+ //// cout << myTestVect2D.at(i).at(j) << " ";
+ //// cout << endl;
+ //// }
+ //// cout << endl;
+
+ //// for (int i=0; i<myTestVect2D.size(); i++)
+ //// {
+ //// cout << "myTestVect2D.at(i).capacity: " << myTestVect2D.at(i).capacity() << endl;
+ //// cout << "myTestVect2D.at(i).size: " << myTestVect2D.at(i).size() << endl;
+ //// }
+
+ //// // ---------------------- now 3D
+
+ //// int xdim = 2;
+ //// int ydim = 3;
+ //// int zdim = 4;
+
+ //// vector<int> v1D;
+ //// vector< vector<int> > v2D;
+ //// vector< vector< vector<int> > > v3D;
+
+ //// v1D.reserve(xdim);
+ //// v1D.push_back(1);
+ //// v1D.push_back(1);
+
+ //// v2D.reserve(ydim);
+ //// for (int j=0; j<v2D.capacity(); j++)
+ //// v2D.push_back(v1D);
+
+ //// v3D.reserve(zdim);
+ //// for (int k=0; k<v3D.capacity(); k++)
+ //// v3D.push_back(v2D);
+
+ //// v3D.at(3).at(0).push_back(900);
+ //// v3D.at(2).at(1).at(0) = 400;
+ //// v3D[1][0][0] = 200;
+
+ //// for (int x=0; x<v3D.size(); x++)
+ //// {
+ //// for (int y=0; y<v3D.at(x).size(); y++)
+ //// {
+ //// for (int z=0; z<v3D.at(x).at(y).size(); z++)
+ //// cout << v3D.at(x).at(y).at(z) << " ";
+ //// cout << endl;
+ //// }
+ //// cout << endl;
+ //// }
+
+ // // ---------------------- now 4D
+
+ //// int xdim = 2;
+ //// int ydim = 3;
+ //// int zdim = 4;
+ //// int listLength = 10;
+
+ //// vector<int> v1D;
+ //// vector< vector<int> > v2D;
+ //// vector< vector< vector<int> > > v3D;
+ //// vector< vector< vector< vector<int> > > > v4D;
+
+ //// v1D.reserve(listLength);
+ //// //v1D.push_back(1);
+ //// //v1D.push_back(1);
+
+ //// //v2D.reserve(xdim);
+ //// v2D.reserve(zdim);
+ //// for (int j=0; j<v2D.capacity(); j++)
+ //// v2D.push_back(v1D);
+
+ //// v3D.reserve(ydim);
+ //// for (int k=0; k<v3D.capacity(); k++)
+ //// v3D.push_back(v2D);
+
+ //// //v4D.reserve(zdim);
+ //// v4D.reserve(xdim);
+ //// for (int l=0; l<v4D.capacity(); l++)
+ //// v4D.push_back(v3D);
+
+ //// //v3D.at(3).at(0).push_back(900);
+ //// v4D.at(0).at(1).at(2).push_back(400);
+ //// v4D[1][0][0].push_back(200);
+ //// v4D[1][0][0].push_back(300);
+
+ //// for (int x=0; x<v4D.size(); x++)
+ //// {
+ //// for (int y=0; y<v4D.at(x).size(); y++)
+ //// {
+ //// for (int z=0; z<v4D.at(x).at(y).size(); z++)
+ //// {
+ //// cout << "capacity v4D.at(x).at(y).at(z): ["
+ //// << x << " " << y << " " << z << " " << "] "
+ //// << v4D.at(x).at(y).at(z).capacity()
+ //// << " size v4D.at(x).at(y).at(z): " << v4D.at(x).at(y).at(z).size() << endl;
+ //// if (v4D.at(x).at(y).at(z).size() > 0)
+ //// {
+ //// cout << "Content: ";
+ //// for (int q=0; q<v4D.at(x).at(y).at(z).size(); q++)
+ //// cout <<v4D.at(x).at(y).at(z).at(q) << " ";
+ //// cout << endl;
+ //// }
+
+ //// }
+
+ //// cout << endl;
+ //// }
+ //// cout << endl;
+ //// }
+
+
+ // //------- test build grid
+
+ // Eigen::Vector3i nCells(2,3,4);
+ //// nCells.push_back(2);
+ //// nCells.push_back(3);
+ //// nCells.push_back(4);
+
+
+ //// //von oben...
+ //// Eigen::Vector3f center2(0,0,0);
+ //// float sphereRadius2 = 1.0f;
+
+ // std::vector<MedialSphere> spheres;
+
+ // //Kugel 1
+ // center << 0, 1, 0;
+ // sphereRadius = 1.0f;
+ // p1 << 0, 2, 0;
+ // p2 << 0, 1, 1;
+ // surfacePoints.clear();
+ // surfacePoints.push_back(p1);
+ // surfacePoints.push_back(p2);
+ // MedialSphere sphere1(center, sphereRadius, surfacePoints);
+ // spheres.push_back(sphere1);
+
+ // //Kugel 2
+ // center << 0, 1, 1;
+ // sphereRadius = 2.0f;
+ // p1 << 0, 3, 1;
+ // p2 << 0, -1, 1;
+ // surfacePoints.clear();
+ // surfacePoints.push_back(p1);
+ // surfacePoints.push_back(p2);
+ // MedialSphere sphere2(center, sphereRadius, surfacePoints);
+ // spheres.push_back(sphere2);
+
+ // //Kugel 3
+ // center << 1, 0, 0;
+ // sphereRadius = 1.0f;
+ // p1 << 1, 0, 1;
+ // p2 << 1, 1, 0;
+ // p3 << 2, 0, 0;
+ // surfacePoints.clear();
+ // surfacePoints.push_back(p1);
+ // surfacePoints.push_back(p2);
+ // surfacePoints.push_back(p3);
+ // MedialSphere sphere3(center, sphereRadius, surfacePoints);
+ // spheres.push_back(sphere3);
+
+ // for (int i=0; i<spheres.size(); i++)
+ // spheres.at(i).printDebug();
+
+
+
+ //// Eigen::Vector3f p1_2(-1,0,0);
+ //// Eigen::Vector3f p2_2(0,-1,0);
+ //// Eigen::Vector3f p3_2(1,0,0);
+ //// Eigen::Vector3f p4_2(0,1,0);
+
+ //// vector<Eigen::Vector3f> surfacePoints2;
+ //// surfacePoints2.push_back(p1_2);
+ //// surfacePoints2.push_back(p2_2);
+ //// surfacePoints2.push_back(p3_2);
+ //// surfacePoints2.push_back(p4_2);
+
+
+
+ // GridOfMedialSpheres goms;
+ // goms.allocateGrid(nCells);
+ // for (int i=0; i<spheres.size(); i++)
+ // goms.insertSphere(spheres.at(i));
+
+ // //------- test cube index computation
+
+ //// Eigen::Vector3i seedIndex(0,1,0);
+ //// Eigen::Vector3i maxCoords(2,2,3);
+ //// int cubeRadiusToSearch = 1;
+ //// std::vector<Eigen::Vector3i> cubeIndices;
+
+ //// cubeIndices = goms.computeCubeIndices(seedIndex, maxCoords, cubeRadiusToSearch);
+
+ //// std::cout << "\nTest computeCubeIndices()" << std::endl;
+ //// std::cout << "seedIndex: " << StrOutHelpers::toString(seedIndex)
+ //// << "maxCoords: " << StrOutHelpers::toString(maxCoords)
+ //// << "cubeRadiusToSearch: " << cubeRadiusToSearch
+ //// << std::endl;
+ //// std::cout << StrOutHelpers::toString(cubeIndices);
+
+ //}
+
+ void TestCases::testSetupGrid(std::vector<MedialSpherePtr>& spheres, std::vector<Eigen::Vector3f> surfacePoints)
+ {
+ cout << "\n-----\nTestCases::testSetupGrid() called." << endl;
+
+ GridOfMedialSpheres goms;
+ int gridConstant = 4;
+ goms.setup(surfacePoints, spheres, gridConstant); //hoho...!
+
+ goms.printDebugGridCells();
+ // HIER WEITER!!
+
+ //----------
+ cout << "\n Now get some spheres from the grid." << endl;
+ //Tested with the salt box test object.
+
+ Eigen::Vector3i index3d(7, 5, 10);
+ std::vector<Eigen::Vector3i> gridIndices;
+ gridIndices.push_back(index3d);
+ cout << "Get spheres from cell " << StrOutHelpers::toString(index3d) << endl;
+ std::vector<MedialSpherePtr> spheresInOneCell = goms.getSpheresFromGrid(gridIndices);
+ cout << "I fetched " << spheresInOneCell.size() << " spheres." << endl;
+ gridIndices.clear();
+ spheresInOneCell.clear();
+
+ index3d << 7, 4, 2;
+ gridIndices.push_back(index3d);
+ cout << "Get spheres from cell " << StrOutHelpers::toString(index3d) << endl;
+ spheresInOneCell = goms.getSpheresFromGrid(gridIndices);
+ cout << "I fetched " << spheresInOneCell.size() << " spheres." << endl;
+ gridIndices.clear();
+ spheresInOneCell.clear();
+
+ index3d << 6, 2, 0;
+ gridIndices.push_back(index3d);
+ cout << "Get spheres from cell " << StrOutHelpers::toString(index3d) << endl;
+ spheresInOneCell = goms.getSpheresFromGrid(gridIndices);
+ cout << "I fetched " << spheresInOneCell.size() << " spheres." << endl;
+ gridIndices.clear();
+ spheresInOneCell.clear();
+
+ index3d << 0, 0, 0;
+ gridIndices.push_back(index3d);
+ cout << "Get spheres from cell " << StrOutHelpers::toString(index3d) << endl;
+ spheresInOneCell = goms.getSpheresFromGrid(gridIndices);
+ cout << "I fetched " << spheresInOneCell.size() << " spheres." << endl;
+ gridIndices.clear();
+ spheresInOneCell.clear();
+
+ //-----
+ cout << "\nTest isGridIndexValid()" << endl;
+ gridIndices.clear();
+
+ index3d << 0, 0, 0;
+ gridIndices.push_back(index3d);
+ index3d << -1, 0, 0;
+ gridIndices.push_back(index3d);
+ index3d << 0, -1, 0;
+ gridIndices.push_back(index3d);
+ index3d << 0, 0, -1;
+ gridIndices.push_back(index3d);
+ index3d << 8, 6, 17;
+ gridIndices.push_back(index3d);
+ index3d << 8, -1, 17;
+ gridIndices.push_back(index3d);
+ index3d << 8, 6, -1;
+ gridIndices.push_back(index3d);
+ index3d << -19, 6, 17;
+ gridIndices.push_back(index3d);
+ index3d << 8, 6, 17;
+ gridIndices.push_back(index3d);
+ index3d << 7, 5, 16;
+ gridIndices.push_back(index3d);
+ index3d << 8, 5, 16;
+ gridIndices.push_back(index3d);
+ index3d << 3, 6, 9;
+ gridIndices.push_back(index3d);
+ index3d << 8, 6, 17;
+ gridIndices.push_back(index3d);
+ index3d << 8, 6, 18;
+ gridIndices.push_back(index3d);
+ index3d << 8, 7, 17;
+ gridIndices.push_back(index3d);
+ index3d << 9, 6, 17;
+ gridIndices.push_back(index3d);
+ index3d << -1, 6, 18;
+ gridIndices.push_back(index3d);
+ index3d << 8, 7, -1;
+ gridIndices.push_back(index3d);
+ index3d << 9, -1, 17;
+ gridIndices.push_back(index3d);
+
+ //bool isValid;
+ // for (int i=0; i<gridIndices.size(); i++)
+ // {
+ // isValid = goms.isGridIndexValid(gridIndices.at(i));
+ // cout << "gridIndex " << StrOutHelpers::toString(gridIndices.at(i))
+ // << " valid? " << isValid << endl;
+ // }
+
+ //---- Test computeCubeIndices() ----
+ cout << "\n\nNow testing computeCubeIndices()" << endl;
+
+ int maxCubeRadiusToSearch = 2;
+ Eigen::Vector3i maxCubeIndex(7, 5, 10); //entspricht numCells(i)-1
+
+ Eigen::Vector3i seedIndex(2, 2, 2); //entspricht numCells(i)-1
+ // testComputeCubeIndices(goms, seedIndex, maxCubeIndex, maxCubeRadiusToSearch);
+
+ // seedIndex << 0,0,0;
+ // testComputeCubeIndices(goms, seedIndex, maxCubeIndex, maxCubeRadiusToSearch);
+
+ // seedIndex << 7,5,10;
+ // testComputeCubeIndices(goms, seedIndex, maxCubeIndex, maxCubeRadiusToSearch);
+
+
+ //---- Test getSpheresFromGrid()----
+ //cout << "\n\n Now testing getSpheresFromGrid()" << endl;
+
+ int cubeRadius = 1;
+ std::vector<Eigen::Vector3i> cubeIndices;
+ std::vector<MedialSpherePtr> returnSpheres;
+
+ seedIndex << 2, 2, 2;
+ cubeIndices = testGetComputeCubeIndices(goms, seedIndex,
+ maxCubeIndex, cubeRadius);
+ returnSpheres = goms.getSpheresFromGrid(cubeIndices);
+ cout << "I got " << returnSpheres.size() << " spheres from the grid. " << endl;
+ cubeIndices.clear();
+ returnSpheres.clear();
+
+ seedIndex << 0, 0, 0;
+ cubeIndices = testGetComputeCubeIndices(goms, seedIndex,
+ maxCubeIndex, cubeRadius);
+ returnSpheres = goms.getSpheresFromGrid(cubeIndices);
+ cout << "I got " << returnSpheres.size() << " spheres from the grid. " << endl;
+ cubeIndices.clear();
+ returnSpheres.clear();
+
+
+ seedIndex << 7, 5, 10;
+ cubeIndices = testGetComputeCubeIndices(goms, seedIndex,
+ maxCubeIndex, cubeRadius);
+ returnSpheres = goms.getSpheresFromGrid(cubeIndices);
+ cout << "I got " << returnSpheres.size() << " spheres from the grid. " << endl;
+ cubeIndices.clear();
+ returnSpheres.clear();
+
+
+
+ }
+
+ void TestCases::testComputeCubeIndices(GridOfMedialSpheres& goms, Eigen::Vector3i seedIndex, Eigen::Vector3i maxCubeIndex, int maxCubeRadiusToSearch)
+ {
+ std::vector<Eigen::Vector3i> cubeIndices;
+
+ for (int cubeRadius = 0; cubeRadius <= maxCubeRadiusToSearch; cubeRadius++)
+ {
+ cubeIndices = goms.computeCubeIndices(seedIndex, maxCubeIndex, cubeRadius);
+ cout << "\nmaxCubeIndex " << StrOutHelpers::toString(maxCubeIndex) << endl;
+ cout << "seedIndex " << StrOutHelpers::toString(seedIndex) << endl;
+ cout << "cubeRadiusToSearch " << cubeRadius << endl;
+ cout << "I got the following cubeIndices back, " << cubeIndices.size()
+ << " total: " << StrOutHelpers::toString(cubeIndices, false) << endl;
+ }
+ }
+
+ std::vector<Eigen::Vector3i> TestCases::testGetComputeCubeIndices(GridOfMedialSpheres& goms, Eigen::Vector3i seedIndex, Eigen::Vector3i maxCubeIndex, int cubeRadius)
+ {
+ std::vector<Eigen::Vector3i> cubeIndices;
+
+ cubeIndices = goms.computeCubeIndices(seedIndex, maxCubeIndex, cubeRadius);
+ cout << "\nmaxCubeIndex " << StrOutHelpers::toString(maxCubeIndex) << endl;
+ cout << "seedIndex " << StrOutHelpers::toString(seedIndex) << endl;
+ cout << "cubeRadiusToSearch " << cubeRadius << endl;
+ cout << "I got the following cubeIndices back, " << cubeIndices.size()
+ << " total: " << StrOutHelpers::toString(cubeIndices, false) << endl;
+
+ return cubeIndices;
+ }
+
+ void TestCases::testEigenMathFunctions()
+ {
+ Eigen::Matrix3f m;
+ m << 1, 2, 3,
+ 4, 5, 6,
+ 7, 8, 9;
+ std::cout << "matrix m: " << std::endl;
+ std::cout << m << std::endl;
+
+ float sumOfMatrix = m.sum();
+ std::cout << "sum: " << sumOfMatrix << std::endl;
+
+ float meanOfMatrix = m.mean();
+ std::cout << "mean: " << meanOfMatrix << std::endl;
+
+ Eigen::Vector3f colSum = m.colwise().sum();
+ //std::cout << "colSum: " << StrOutHelpers::toString(colSum) << std::endl;
+ std::cout << "colSum: " << colSum << std::endl;
+
+ Eigen::Vector3f rowSum = m.rowwise().sum();
+ //std::cout << "rowSum: " << StrOutHelpers::toString(rowSum) << std::endl;
+ std::cout << "rowSum: " << rowSum << std::endl;
+
+ Eigen::Vector3f colMean = m.colwise().mean();
+ //std::cout << "colMean: " << StrOutHelpers::toString(colMean) << std::endl;
+ std::cout << "colMean: " << colMean << std::endl;
+
+ Eigen::Vector3f rowMean = m.colwise().mean();
+ //std::cout << "rowMean: " << StrOutHelpers::toString(rowMean) << std::endl;
+ std::cout << "rowMean: " << rowMean << std::endl;
+
+ //Eigen::Matrix<float, 3, Eigen::Dynamic> m2;
+ //Eigen::MatrixXf m2(3,2);
+ //Eigen::MatrixXf m2(3,5);
+ Eigen::MatrixXf m2;
+ m2.resize(3, 2);
+
+ m2 << rowSum, rowSum;
+ std::cout << "m2: " << std::endl;
+ std::cout << m2 << std::endl;
+
+ m2.resize(3, 4);
+ m2.setZero();
+
+ std::cout << "m2: " << std::endl;
+ std::cout << m2 << std::endl;
+
+ m2.block<3, 1>(0, 2) = rowMean;
+ m2.block<3, 1>(0, 3) = rowSum;
+
+ // m2 << rowMean;
+ // m2 << rowMean;
+
+
+ // m2 << m;
+ std::cout << "m2: " << std::endl;
+ std::cout << m2 << std::endl;
+
+ // ----
+ std::vector<Eigen::Vector3f> myTestVect;
+ myTestVect.push_back(rowSum);
+ myTestVect.push_back(rowMean);
+ myTestVect.push_back(rowSum);
+ myTestVect.push_back(rowMean);
+ myTestVect.push_back(rowSum);
+ myTestVect.push_back(colSum);
+ myTestVect.push_back(colMean);
+
+
+ Eigen::MatrixXf m3 = SphereHelpers::toMatrix_3xN(myTestVect);
+ std::cout << "m3: " << std::endl;
+ std::cout << m3 << std::endl;
+
+ Eigen::Vector3f cog = m3.rowwise().mean();
+ std::cout << "mean of points in m3: " << cog << std::endl;
+
+ // --- Eigenvalues and eigenvectors via SVD:
+ //Eigen::MatrixXf m4 = Eigen::MatrixXf::Random(3,2);
+ Eigen::MatrixXf m4;
+ m4.resize(3, 6);
+ m4 << 10, 2, 3, 4, 5, 14,
+ 10, 2, 3, 4, 5, 14,
+ 0, 0, 0, 0, 0, 0;
+
+ std::cout << "Here is the matrix m4: " << std::endl << m4 << std::endl;
+ Eigen::JacobiSVD<Eigen::MatrixXf> svd(m4, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ std::cout << "Its singular values are: " << std::endl << svd.singularValues() << std::endl;
+ std::cout << "Its left singular vectors are the columns of the thin U matrix: "
+ << std::endl << svd.matrixU() << std::endl;
+ std::cout << "Its right singular vectors are the columns of the thin V matrix: "
+ << std::endl << svd.matrixV() << std::endl;
+ Eigen::Vector3f rhs(1, 0, 0);
+ std::cout << "Now consider this rhs vector: " << std::endl << rhs << std::endl;
+ std::cout << "A least-squares solution of m*x = rhs is: " << std::endl << svd.solve(rhs)
+ << std::endl;
+
+
+ //HIER WEITER!
+
+
+
+ }
+
+}
diff --git a/GraspPlanning/GraspPlanner/MATPlanner/TestCases.h b/GraspPlanning/GraspPlanner/MATPlanner/TestCases.h
new file mode 100644
index 000000000..478b2134b
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/MATPlanner/TestCases.h
@@ -0,0 +1,32 @@
+#ifndef TESTCASES_H
+#define TESTCASES_H
+
+#include "../../GraspStudio.h"
+#include "SphereHelpers.h"
+#include "GridOfMedialSpheres.h"
+namespace GraspStudio
+{
+
+ class GRASPSTUDIO_IMPORT_EXPORT TestCases
+ {
+ public:
+ TestCases();
+
+ //static void testSomeStuff();
+
+ static void testScaleMedialSphere();
+ static void testScaleMedialSphere(MedialSpherePtr ms, float scaleFactor);
+
+ static void testSphereSelection(std::vector<MedialSpherePtr>& spheres);
+
+ static void testSetupGrid(std::vector<MedialSpherePtr>& spheres, std::vector<Eigen::Vector3f> surfacePoints);
+
+ static void testComputeCubeIndices(GridOfMedialSpheres& goms, Eigen::Vector3i seedIndex, Eigen::Vector3i maxCubeIndex, int maxCubeRadiusToSearch);
+ static std::vector<Eigen::Vector3i> testGetComputeCubeIndices(GridOfMedialSpheres& goms, Eigen::Vector3i seedIndex, Eigen::Vector3i maxCubeIndex, int maxCubeRadiusToSearch);
+
+ static void testEigenMathFunctions();
+
+
+ };
+}
+#endif // TESTCASES_H
--
GitLab