diff --git a/GraspPlanning/CMakeLists.txt b/GraspPlanning/CMakeLists.txt
index b6eaf765d5b8fe11f22f0e10a2fea8dbb1e9f4ab..28e0d022abc0a80e9d4cfba6c1811277107706a2 100644
--- a/GraspPlanning/CMakeLists.txt
+++ b/GraspPlanning/CMakeLists.txt
@@ -8,6 +8,9 @@ endif()
MESSAGE (STATUS "***** CONFIGURING Simox project GraspStudio *****")
+INCLUDE (Cxx11Test)
+SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CXX11_FLAG}")
+
MACRO(ADD_GRASPSTUDIO_TEST TEST_NAME)
if (NOT Boost_USE_STATIC_LIBS)
ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK)
@@ -66,6 +69,7 @@ ApproachMovementSurfaceNormal.h
MeshConverter.h
GraspPlanner/GraspPlanner.h
GraspPlanner/GenericGraspPlanner.h
+GraspPlanner/GraspPlannerEvaluation.h
GraspQuality/GraspEvaluationPoseUncertainty.h
GraspQuality/GraspQualityMeasure.h
GraspQuality/GraspQualityMeasureWrenchSpace.h
diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
index a187b393a0c34abd5356fd2551759429335df838..69187374b5a40e87e81321b90b9201253be59161 100644
--- a/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
+++ b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
@@ -9,6 +9,7 @@
#include "../GraspQuality/GraspQualityMeasure.h"
#include "../ApproachMovementGenerator.h"
+#include <chrono>
using namespace std;
namespace GraspStudio
@@ -27,6 +28,8 @@ namespace GraspStudio
THROW_VR_EXCEPTION_IF(!eef, "NULL eef in approach...");
THROW_VR_EXCEPTION_IF(!graspSet, "NULL graspSet...");
verbose = true;
+ eval.fcCheck = forceClosure;
+ eval.minQuality = minQuality;
}
GenericGraspPlanner::~GenericGraspPlanner()
@@ -76,7 +79,7 @@ namespace GraspStudio
VirtualRobot::GraspPtr GenericGraspPlanner::planGrasp(VirtualRobot::SceneObjectSetPtr obstacles)
{
-
+ auto start_time = chrono::high_resolution_clock::now();
std::string sGraspPlanner("Simox - GraspStudio - ");
sGraspPlanner += graspQuality->getName();
std::string sGraspNameBase = "Grasp ";
@@ -88,15 +91,11 @@ namespace GraspStudio
VR_ASSERT(tcp);
+ // GENERATE APPROACH POSE
bool bRes = approach->setEEFToRandomApproachPose();
-
- if (!bRes)
- {
- return VirtualRobot::GraspPtr();
- }
-
- if (obstacles)
+ if (bRes && obstacles)
{
+ // CHECK VALID APPROCH POSE
VirtualRobot::CollisionCheckerPtr colChecker = eef->getCollisionChecker();
VR_ASSERT(eef->getRobot());
VR_ASSERT(obstacles);
@@ -104,26 +103,48 @@ namespace GraspStudio
if (colChecker->checkCollision(eef->createSceneObjectSet(), obstacles))
{
// GRASPSTUDIO_INFO << ": Collision detected before closing fingers" << endl;
- return VirtualRobot::GraspPtr();
+ //return VirtualRobot::GraspPtr();
+ bRes = false;
}
}
+ // CHECK CONTACTS
+ if (bRes)
+ {
+ contacts = eef->closeActors(object);
- contacts = eef->closeActors(object);
+ eef->addStaticPartContacts(object, contacts, approach->getApproachDirGlobal());
- eef->addStaticPartContacts(object, contacts, approach->getApproachDirGlobal());
+ if (obstacles)
+ {
+ VirtualRobot::CollisionCheckerPtr colChecker = eef->getCollisionChecker();
+ VR_ASSERT(eef->getRobot());
+ VR_ASSERT(obstacles);
- if (obstacles)
+ if (colChecker->checkCollision(eef->createSceneObjectSet(), obstacles))
+ {
+ // GRASPSTUDIO_INFO << ": Collision detected after closing fingers" << endl;
+ //return VirtualRobot::GraspPtr();
+ bRes = false;
+ }
+ }
+ }
+
+ // eval data
+ eval.graspTypePower.push_back(true);
+ eval.nrGraspsGenerated++;
+
+ if (!bRes)
{
- VirtualRobot::CollisionCheckerPtr colChecker = eef->getCollisionChecker();
- VR_ASSERT(eef->getRobot());
- VR_ASSERT(obstacles);
+ // result not valid due to collision
+ auto end_time = chrono::high_resolution_clock::now();
+ float ms = float(chrono::duration_cast<chrono::milliseconds>(end_time - start_time).count());
+ eval.graspScore.push_back(0.0f);
+ eval.graspValid.push_back(false);
+ eval.nrGraspsInvalidCollision++;
+ eval.timeGraspMS.push_back(ms);
- if (colChecker->checkCollision(eef->createSceneObjectSet(), obstacles))
- {
- // GRASPSTUDIO_INFO << ": Collision detected after closing fingers" << endl;
- return VirtualRobot::GraspPtr();
- }
+ return VirtualRobot::GraspPtr();
}
if (contacts.size() < 2)
@@ -132,20 +153,40 @@ namespace GraspStudio
{
GRASPSTUDIO_INFO << ": ignoring grasp hypothesis, low number of contacts" << endl;
}
-
+ // result not valid due to low number of contacts
+ auto end_time = chrono::high_resolution_clock::now();
+ float ms = float(chrono::duration_cast<chrono::milliseconds>(end_time - start_time).count());
+ eval.graspScore.push_back(0.0f);
+ eval.graspValid.push_back(false);
+ eval.nrGraspsInvalidContacts++;
+ eval.timeGraspMS.push_back(ms);
return VirtualRobot::GraspPtr();
}
graspQuality->setContactPoints(contacts);
float score = graspQuality->getGraspQuality();
- if (score < minQuality)
+ if (forceClosure && !graspQuality->isGraspForceClosure())
{
+ // not force closure
+ auto end_time = chrono::high_resolution_clock::now();
+ float ms = float(chrono::duration_cast<chrono::milliseconds>(end_time - start_time).count());
+ eval.graspScore.push_back(0.0f);
+ eval.graspValid.push_back(false);
+ eval.nrGraspsInvalidFC++;
+ eval.timeGraspMS.push_back(ms);
return VirtualRobot::GraspPtr();
}
- if (forceClosure && !graspQuality->isGraspForceClosure())
+ if (score < minQuality)
{
+ // min quality not reached
+ auto end_time = chrono::high_resolution_clock::now();
+ float ms = float(chrono::duration_cast<chrono::milliseconds>(end_time - start_time).count());
+ eval.graspScore.push_back(score);
+ eval.graspValid.push_back(false);
+ eval.nrGraspsInvalidFC++;
+ eval.timeGraspMS.push_back(ms);
return VirtualRobot::GraspPtr();
}
@@ -155,6 +196,15 @@ namespace GraspStudio
GRASPSTUDIO_INFO << ": Found grasp with " << contacts.size() << " contacts, score: " << score << endl;
}
+ auto end_time = chrono::high_resolution_clock::now();
+ float ms = float(chrono::duration_cast<chrono::milliseconds>(end_time - start_time).count());
+ eval.graspScore.push_back(score);
+ eval.graspValid.push_back(true);
+ eval.nrGraspsValid++;
+ // only power grasps
+ eval.nrGraspsValidPower++;
+ eval.timeGraspMS.push_back(ms);
+
std::stringstream ss;
ss << sGraspNameBase << (graspSet->getSize() + 1);
std::string sGraspName = ss.str();
@@ -167,11 +217,18 @@ namespace GraspStudio
g->setConfiguration(configValues);
return g;
}
+
VirtualRobot::EndEffector::ContactInfoVector GenericGraspPlanner::getContacts() const
{
return contacts;
}
+ void GenericGraspPlanner::setParameters(float minQuality, bool forceClosure)
+ {
+ this->minQuality = minQuality;
+ this->forceClosure = forceClosure;
+ }
+
bool GenericGraspPlanner::timeout()
{
diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.h b/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
index b0c1523730ed3288f77f55a33ecd94236ce795c1..cfcadf85eabdb27032ecca3219449d9b0b3856eb 100644
--- a/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
+++ b/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
@@ -65,6 +65,8 @@ namespace GraspStudio
VirtualRobot::EndEffector::ContactInfoVector getContacts() const;
+ void setParameters(float minQuality, bool forceClosure);
+
protected:
bool timeout();
diff --git a/GraspPlanning/GraspPlanner/GraspPlanner.cpp b/GraspPlanning/GraspPlanner/GraspPlanner.cpp
index 1d0be833d2456d3f661b8ff9085bb84648fc5c45..aa7369158c43e4f30f1a2df5f020d3a4365cd332 100644
--- a/GraspPlanning/GraspPlanner/GraspPlanner.cpp
+++ b/GraspPlanning/GraspPlanner/GraspPlanner.cpp
@@ -23,4 +23,8 @@ namespace GraspStudio
return plannedGrasps;
}
+ GraspPlannerEvaluation GraspPlanner::getEvaluation()
+ {
+ return eval;
+ }
}
diff --git a/GraspPlanning/GraspPlanner/GraspPlanner.h b/GraspPlanning/GraspPlanner/GraspPlanner.h
index 0900b8d16f9c12546039b960097b290d9f4496a5..89655ca2cf5d9ead8aae158691fcb1ce852fa79a 100644
--- a/GraspPlanning/GraspPlanner/GraspPlanner.h
+++ b/GraspPlanning/GraspPlanner/GraspPlanner.h
@@ -23,7 +23,8 @@
#ifndef __GENERAL_GRASP_PLANNER_H__
#define __GENERAL_GRASP_PLANNER_H__
-#include "../GraspStudio.h"
+#include <GraspPlanning/GraspStudio.h>
+#include <GraspPlanning/GraspPlanner/GraspPlannerEvaluation.h>
#include <VirtualRobot/EndEffector/EndEffector.h>
#include <VirtualRobot/SceneObject.h>
#include <VirtualRobot/Grasping/GraspSet.h>
@@ -31,7 +32,6 @@
namespace GraspStudio
{
-
/*!
*
* \brief An interface for grasp planners.
@@ -66,10 +66,18 @@ namespace GraspStudio
*/
std::vector<VirtualRobot::GraspPtr> getPlannedGrasps();
+ /*!
+ * \brief getEvaluation
+ * \return The current evaluation of the grasp planner.
+ */
+ GraspPlannerEvaluation getEvaluation();
+
protected:
bool verbose;
VirtualRobot::GraspSetPtr graspSet;
std::vector<VirtualRobot::GraspPtr> plannedGrasps;
+
+ GraspPlannerEvaluation eval;
};
}
diff --git a/GraspPlanning/GraspPlanner/GraspPlannerEvaluation.h b/GraspPlanning/GraspPlanner/GraspPlannerEvaluation.h
new file mode 100644
index 0000000000000000000000000000000000000000..96904224accae092a33d8e90b65fba7008ebe029
--- /dev/null
+++ b/GraspPlanning/GraspPlanner/GraspPlannerEvaluation.h
@@ -0,0 +1,193 @@
+/**
+* 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 2017 Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#ifndef __GENERAL_GRASP_PLANNER_EVALUATION_H__
+#define __GENERAL_GRASP_PLANNER_EVALUATION_H__
+
+#include "../GraspStudio.h"
+#include <vector>
+
+namespace GraspStudio
+{
+
+struct GraspPlannerEvaluation
+{
+ GraspPlannerEvaluation()
+ {
+ nrGraspsGenerated = 0;
+ nrGraspsValid = 0;
+ nrGraspsInvalidCollision = 0;
+ nrGraspsInvalidFC = 0;
+ nrGraspsInvalidContacts = 0;
+ nrGraspsValidPrecision = 0;
+ nrGraspsValidPower = 0;
+ fcCheck = false;
+ minQuality = 0.0f;
+ }
+
+ void print()
+ {
+ std::cout << "---------------- GRASP PLANNER EVALUATION -----------" << std::endl;
+ if (fcCheck)
+ std::cout << "ForceClosure check: true" << std::endl;
+ else
+ std::cout << "ForceClosure check: false" << std::endl;
+ std::cout << "Min Quality: " << minQuality << std::endl;
+
+ std::cout << "nrGraspsGenerated:" << nrGraspsGenerated << std::endl;
+ std::cout << "nrGraspsValid:" << nrGraspsValid << std::endl;
+ std::cout << "nrGraspsInValid:" << nrGraspsInvalidCollision+nrGraspsInvalidFC+nrGraspsInvalidContacts << std::endl;
+ std::cout << "nrGraspsValidPrecision:" << nrGraspsValidPrecision << std::endl;
+ std::cout << "nrGraspsValidPower:" << nrGraspsValidPower << std::endl;
+ std::cout << "nrGraspsInvalidCollision:" << nrGraspsInvalidCollision << std::endl;
+ std::cout << "nrGraspsInvalidFC:" << nrGraspsInvalidFC << std::endl;
+ std::cout << "nrGraspsInvalidContacts:" << nrGraspsInvalidContacts << std::endl;
+ VR_ASSERT (timeGraspMS.size() == graspScore.size());
+ VR_ASSERT (timeGraspMS.size() == graspValid.size());
+ VR_ASSERT (timeGraspMS.size() == graspTypePower.size());
+ float timeAcc = 0;
+ float scoreAcc = 0;
+ int nrGrasps = (int)timeGraspMS.size();
+ int nrValid = 0;
+ int nrPower = 0;
+ int nrPrecision = 0;
+ for (size_t i=0;i<timeGraspMS.size();i++)
+ {
+ timeAcc+=timeGraspMS.at(i);
+ if (graspValid.at(i))
+ {
+ nrValid++;
+ scoreAcc+=graspScore.at(i);
+ }
+ if (graspTypePower.at(i))
+ nrPower++;
+ else
+ nrPrecision++;
+ }
+ std::cout << "Time complete: " << timeAcc << " ms" << std::endl;
+ if (nrGrasps>0)
+ {
+ std::cout << "Avg time per grasp (valid&invalid):" << (float)timeAcc / (float)nrGrasps << " ms" << std::endl;
+ float percPower = (float)nrPower / (float)nrGrasps;
+ float percPrec = (float)nrPrecision / (float)nrGrasps;
+ std::cout << "Precision grasps:" << nrPrecision << " -> " << percPrec*100 << "%" << std::endl;
+ std::cout << "Power grasps:" << nrPower << " -> " << percPower*100 << "%" << std::endl;
+ }
+ if (nrValid>0)
+ {
+ std::cout << "Avg score (valid):" << scoreAcc / nrValid << std::endl;
+ }
+ }
+
+ static std::string GetCSVHeader()
+ {
+ std::stringstream fs;
+ fs << "minQuality" << ","
+ << "nrGraspsGenerated" << ","
+ << "nrGraspsValid" << ","
+ << "nrGraspsInValid" << ","
+ << "nrGraspsValidPrecision" << ","
+ << "nrGraspsValidPower" << ","
+ << "nrGraspsInvalidCollision" << ","
+ << "nrGraspsInvalidFC" << ","
+ << "nrGraspsInvalidContacts" << ","
+ << "AverageDurationMS" << ","
+ << "percPowerGrasps" << ","
+ << "percPrecGraps" << ","
+ << "avgScore";
+ return fs.str();
+ }
+
+ std::string toCSVString() const
+ {
+ float timeAcc = 0;
+ float scoreAcc = 0;
+ int nrGrasps = (int)timeGraspMS.size();
+ int nrValid = 0;
+ int nrPower = 0;
+ int nrPrecision = 0;
+ for (size_t i=0;i<timeGraspMS.size();i++)
+ {
+ timeAcc+=timeGraspMS.at(i);
+ if (graspValid.at(i))
+ {
+ nrValid++;
+ scoreAcc+=graspScore.at(i);
+ }
+ if (graspTypePower.at(i))
+ nrPower++;
+ else
+ nrPrecision++;
+ }
+ float percPower = 0;
+ float percPrec = 0;
+ if (nrGrasps>0)
+ {
+ percPower = (float)nrPower / (float)nrGrasps;
+ percPrec = (float)nrPrecision / (float)nrGrasps;
+ }
+ float avgScore = 0;
+ if (nrValid>0)
+ {
+ avgScore = scoreAcc / nrValid ;
+ }
+
+
+ std::stringstream fs;
+ fs << minQuality << ","
+ << nrGraspsGenerated << ","
+ << nrGraspsValid << ","
+ << (nrGraspsInvalidCollision+nrGraspsInvalidFC+nrGraspsInvalidContacts) << ","
+ << nrGraspsValidPrecision << ","
+ << nrGraspsValidPower << ","
+ << nrGraspsInvalidCollision << ","
+ << nrGraspsInvalidFC << ","
+ << nrGraspsInvalidContacts << ","
+ << (float)timeAcc / (float)nrGrasps << ","
+ << percPower << ","
+ << percPrec << ","
+ << avgScore;
+ return fs.str();
+ }
+
+
+ int nrGraspsGenerated;
+ int nrGraspsValid;
+ int nrGraspsInvalidCollision;
+ int nrGraspsInvalidFC; // or quality
+ int nrGraspsInvalidContacts;
+
+ int nrGraspsValidPrecision;
+ int nrGraspsValidPower;
+
+ std::vector<float> timeGraspMS; // time per grasp generation
+ std::vector<float> graspScore; //grasp quality
+ std::vector<bool> graspValid; //grasp valid
+ std::vector<bool> graspTypePower; //grasp type
+
+ bool fcCheck;
+ float minQuality;
+};
+
+}
+
+#endif
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
index f03e295341a037adcc52fd7dbd84bd0628c481b9..d0c67efb7827e44e20e32bb4524b700cd90ef5ad 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
@@ -368,7 +368,10 @@ void GraspPlannerWindow::plan()
bool forceClosure = UI.checkBoxFoceClosure->isChecked();
float quality = (float)UI.doubleSpinBoxQuality->value();
int nrGrasps = UI.spinBoxGraspNumber->value();
- planner.reset(new GraspStudio::GenericGraspPlanner(grasps, qualityMeasure, approach, quality, forceClosure));
+ if (planner)
+ planner->setParameters(quality, forceClosure);
+ else
+ planner.reset(new GraspStudio::GenericGraspPlanner(grasps, qualityMeasure, approach, quality, forceClosure));
int nr = planner->plan(nrGrasps, timeout);
VR_INFO << " Grasp planned:" << nr << endl;
@@ -403,6 +406,8 @@ void GraspPlannerWindow::plan()
openEEF();
closeEEF();
}
+
+ planner->getEvaluation().print();
}