diff --git a/GraspPlanning/ApproachMovementGenerator.cpp b/GraspPlanning/ApproachMovementGenerator.cpp
index 3d5093a60a26edfd84baec6cc9bd51c6942a7676..4949963730cd0221172af53f97e440ba2856c7d9 100644
--- a/GraspPlanning/ApproachMovementGenerator.cpp
+++ b/GraspPlanning/ApproachMovementGenerator.cpp
@@ -44,7 +44,7 @@ namespace GraspStudio
}
ApproachMovementGenerator::~ApproachMovementGenerator()
- = default;
+ = default;
VirtualRobot::RobotPtr ApproachMovementGenerator::getEEFRobotClone()
@@ -58,9 +58,13 @@ namespace GraspStudio
if (!graspPreshape.empty()
&& eef_cloned->hasPreshape(graspPreshape)
&& eef_cloned->getPreshape(graspPreshape)->getTCP())
+ {
tcp = eef_cloned->getPreshape(graspPreshape)->getTCP();
+ }
else
+ {
tcp = eef_cloned->getGCP();
+ }
eefRobot->setGlobalPoseForRobotNode(tcp, pose);
return true;
}
@@ -83,9 +87,13 @@ namespace GraspStudio
{
VirtualRobot::RobotNodePtr tcp;
if (!graspPreshape.empty() && eef_cloned->hasPreshape(graspPreshape) && eef_cloned->getPreshape(graspPreshape)->getTCP())
+ {
tcp = eef_cloned->getPreshape(graspPreshape)->getTCP();
+ }
else
+ {
tcp = eef_cloned->getGCP();
+ }
return tcp->getGlobalPose();
}
diff --git a/GraspPlanning/ApproachMovementSurfaceNormal.cpp b/GraspPlanning/ApproachMovementSurfaceNormal.cpp
index d5264e9e595d10a1e0a3b99e1e7fa27254ad666a..8acde22453f9c79598539ef019167d833dd32a2a 100644
--- a/GraspPlanning/ApproachMovementSurfaceNormal.cpp
+++ b/GraspPlanning/ApproachMovementSurfaceNormal.cpp
@@ -25,7 +25,7 @@ namespace GraspStudio
distribRetreatDistance(0, maxRetreatDist)
{
name = "ApproachMovementSurfaceNormal";
-
+
if (useFaceAreaDistribution)
{
std::vector<float> areas = objectModel->getFaceAreas();
@@ -34,7 +34,7 @@ namespace GraspStudio
}
ApproachMovementSurfaceNormal::~ApproachMovementSurfaceNormal()
- = default;
+ = default;
bool ApproachMovementSurfaceNormal::getPositionOnObject(Eigen::Vector3f& storePos, Eigen::Vector3f& storeApproachDir)
{
@@ -44,28 +44,28 @@ namespace GraspStudio
}
std::size_t faceIndex = useFaceAreasDistrib ? distribFaceAreas(randomEngine)
- : distribUniform(randomEngine);
+ : distribUniform(randomEngine);
std::size_t nVert1 = (objectModel->faces[faceIndex]).id1;
std::size_t nVert2 = (objectModel->faces[faceIndex]).id2;
std::size_t nVert3 = (objectModel->faces[faceIndex]).id3;
storePos = VirtualRobot::MathTools::randomPointInTriangle(objectModel->vertices[nVert1],
- objectModel->vertices[nVert2],
- objectModel->vertices[nVert3]);
-
+ objectModel->vertices[nVert2],
+ objectModel->vertices[nVert3]);
+
//storePos = (objectModel->vertices[nVert1] + objectModel->vertices[nVert2] + objectModel->vertices[nVert3]) / 3.0f;
/*position(0) = (objectModel->vertices[nVert1].x + objectModel->vertices[nVert2].x + objectModel->vertices[nVert3].x) / 3.0f;
position(1) = (objectModel->vertices[nVert1].y + objectModel->vertices[nVert2].y + objectModel->vertices[nVert3].y) / 3.0f;
position(2) = (objectModel->vertices[nVert1].z + objectModel->vertices[nVert2].z + objectModel->vertices[nVert3].z) / 3.0f;*/
-
+
storeApproachDir = (objectModel->faces[faceIndex]).normal;
if (std::abs(storeApproachDir.squaredNorm() - 1) > 1e-6f)
{
std::cout << "Normal in trimesh not normalized!";
storeApproachDir.normalize();
}
-
+
return true;
}
@@ -77,7 +77,7 @@ namespace GraspStudio
Eigen::Vector3f position;
Eigen::Vector3f approachDir;
-
+
if (!getPositionOnObject(position, approachDir))
{
GRASPSTUDIO_ERROR << "no position on object?!" << endl;
@@ -111,12 +111,12 @@ namespace GraspStudio
// restore original pose
setEEFPose(pose);
-
+
return poseB;
}
bool ApproachMovementSurfaceNormal::setEEFToApproachPose(
- const Eigen::Vector3f& position, const Eigen::Vector3f& approachDir)
+ const Eigen::Vector3f& position, const Eigen::Vector3f& approachDir)
{
VirtualRobot::RobotNodePtr graspNode = eef_cloned->getGCP();
@@ -195,22 +195,25 @@ namespace GraspStudio
poseFinal.block(0, 2, 3, 1) = z;
setEEFPose(poseFinal);
-
+
return true;
}
void ApproachMovementSurfaceNormal::moveEEFAway(
- const Eigen::Vector3f& approachDir, float step, int maxLoops)
+ const Eigen::Vector3f& approachDir, float step, int maxLoops)
{
VirtualRobot::SceneObjectSetPtr sceneObjectSet = eef_cloned->createSceneObjectSet();
- if (!sceneObjectSet) return;
+ if (!sceneObjectSet)
+ {
+ return;
+ }
CollisionModelPtr objectColModel = object->getCollisionModel();
CollisionCheckerPtr eefCollChecker = eef_cloned->getCollisionChecker();
Eigen::Vector3f delta = approachDir * step;
int loop = 0;
-
+
while (loop < maxLoops && eefCollChecker->checkCollision(objectColModel, sceneObjectSet))
{
updateEEFPose(delta);
@@ -222,9 +225,13 @@ namespace GraspStudio
{
RobotNodePtr tcp;
if (!graspPreshape.empty() && eef_cloned->hasPreshape(graspPreshape) && eef_cloned->getPreshape(graspPreshape)->getTCP())
+ {
tcp = eef_cloned->getPreshape(graspPreshape)->getTCP();
+ }
else
+ {
tcp = eef_cloned->getGCP();
+ }
return tcp->getGlobalPose();
}
@@ -232,11 +239,15 @@ namespace GraspStudio
{
RobotNodePtr tcp;
if (!graspPreshape.empty() && eef_cloned->hasPreshape(graspPreshape) && eef_cloned->getPreshape(graspPreshape)->getTCP())
+ {
tcp = eef_cloned->getPreshape(graspPreshape)->getTCP();
+ }
else
+ {
tcp = eef_cloned->getGCP();
+ }
eefRobot->setGlobalPoseForRobotNode(tcp, pose);
return true;
}
-
+
}
diff --git a/GraspPlanning/ApproachMovementSurfaceNormal.h b/GraspPlanning/ApproachMovementSurfaceNormal.h
index e8ccd3289f8f857b54f01c829054915ebab56edc..bc976fdf72a87ea07fb5071661b0f127d28af924 100644
--- a/GraspPlanning/ApproachMovementSurfaceNormal.h
+++ b/GraspPlanning/ApproachMovementSurfaceNormal.h
@@ -35,7 +35,7 @@
namespace GraspStudio
{
/*!
- * This class generates grasping configs by sampling a random surface
+ * This class generates grasping configs by sampling a random surface
* position of the object and setting the EEF to a surface normal aligned
* position.
* The remaining free DoF (the rotation around the normal) is set randomly.
@@ -84,24 +84,24 @@ namespace GraspStudio
protected:
-
+
/// The random engine.
std::default_random_engine randomEngine { std::random_device{}() };
-
+
/// Uniform distribiton over face indices.
std::uniform_int_distribution<std::size_t> distribUniform;
-
+
/// Indicates whether distribFaceAreas shall be used.
bool useFaceAreasDistrib = false;
-
+
/// Distribution with probability of a face proportional to its area.
/// Only initialized if useFaceAreasDistrib is true.
std::discrete_distribution<std::size_t> distribFaceAreas;
-
+
/// Distribution to draw random retreat distances from.
std::uniform_real_distribution<float> distribRetreatDistance;
-
+
};
}
diff --git a/GraspPlanning/ConvexHullGenerator.cpp b/GraspPlanning/ConvexHullGenerator.cpp
index a6bfd1dbe5737e34a87fdf10004706639c5bcaf4..411996399038fecf8ce0969bed471b84b991c1cf 100644
--- a/GraspPlanning/ConvexHullGenerator.cpp
+++ b/GraspPlanning/ConvexHullGenerator.cpp
@@ -183,7 +183,7 @@ namespace GraspStudio
double pCenter[3];
- for (double & u : pCenter)
+ for (double& u : pCenter)
{
u = 0;
}
@@ -200,7 +200,7 @@ namespace GraspStudio
}
if (nVcertexCount > 0)
- for (double & u : pCenter)
+ for (double& u : pCenter)
{
u /= (float)nVcertexCount;
}
@@ -360,14 +360,14 @@ namespace GraspStudio
/*int convexNumVert2 =*/ qh_setsize(qh_facetvertices(facet_list, nullptr, false));
double pCenter[6];
- for (double & u : pCenter)
+ for (double& u : pCenter)
{
u = 0;
}
double pZero[6];
- for (double & u : pZero)
+ for (double& u : pZero)
{
u = 0;
}
@@ -384,7 +384,7 @@ namespace GraspStudio
}
if (nVcertexCount > 0)
- for (double & u : pCenter)
+ for (double& u : pCenter)
{
u /= (float)nVcertexCount;
}
diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
index 2ef3d9de9400ddf97c7efc3a75e2ec1332af3624..606c4e81cca13c1ad5907e634f2eec249cd58582 100644
--- a/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
+++ b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
@@ -36,7 +36,7 @@ namespace GraspStudio
}
GenericGraspPlanner::~GenericGraspPlanner()
- = default;
+ = default;
int GenericGraspPlanner::plan(int nrGrasps, int timeOutMS, VirtualRobot::SceneObjectSetPtr obstacles)
{
@@ -48,7 +48,7 @@ namespace GraspStudio
if (verbose)
{
- GRASPSTUDIO_INFO << ": Searching " << nrGrasps << " grasps for EEF '" << approach->getEEF()->getName()
+ GRASPSTUDIO_INFO << ": Searching " << nrGrasps << " grasps for EEF '" << approach->getEEF()->getName()
<< "' and object '" << graspQuality->getObject()->getName() << "'.\n";
GRASPSTUDIO_INFO << ": Approach movements are generated with " << approach->getName() << endl;
GRASPSTUDIO_INFO << ": Grasps are evaluated with " << graspQuality->getName() << endl;
@@ -108,7 +108,7 @@ namespace GraspStudio
break;
}
auto contacts = eef->closeActors(object);
- if (contacts.size()>=2)
+ if (contacts.size() >= 2)
{
approach->openHand();
finishedContactsOK = true;
@@ -130,13 +130,13 @@ namespace GraspStudio
VR_ASSERT(robot);
VR_ASSERT(tcp);
-
+
// GENERATE APPROACH POSE
// Eigen::Matrix4f pose = approach->createNewApproachPose();
// bRes = approach->setEEFPose(pose);
-
+
bool bRes = approach->setEEFToRandomApproachPose();
-
+
if (bRes && obstacles)
{
// CHECK VALID APPROACH POSE
@@ -145,7 +145,7 @@ namespace GraspStudio
VR_ASSERT(obstacles);
if (colChecker->checkCollision(eef->createSceneObjectSet(), obstacles))
- {
+ {
// GRASPSTUDIO_INFO << ": Collision detected before closing fingers" << endl;
//return VirtualRobot::GraspPtr();
bRes = false;
@@ -153,7 +153,7 @@ namespace GraspStudio
}
contacts.clear();
-
+
// CHECK CONTACTS
if (bRes)
{
@@ -190,19 +190,19 @@ namespace GraspStudio
// construct grasp
std::stringstream graspName;
graspName << graspNameBase << (graspSet->getSize() + 1);
-
+
Eigen::Matrix4f poseObject = object->getGlobalPose();
Eigen::Matrix4f poseTcp = tcp->toLocalCoordinateSystem(poseObject);
-
+
VirtualRobot::GraspPtr grasp(new VirtualRobot::Grasp(
- graspName.str(), robot->getType(), eef->getName(), poseTcp,
- graspPlannerName, 0 /*score*/ ));
-
+ graspName.str(), robot->getType(), eef->getName(), poseTcp,
+ graspPlannerName, 0 /*score*/));
+
// set joint config
grasp->setConfiguration(eef->getConfiguration()->getRobotNodeJointValueMap());
-
+
bool returnNullWhenInvalid = true;
-
+
// eval data
eval.graspTypePower.push_back(true);
eval.nrGraspsGenerated++;
@@ -213,7 +213,7 @@ namespace GraspStudio
float ms = chrono::duration_cast<chrono::milliseconds>(end_time - start_time).count();
return ms;
};
-
+
if (!bRes)
{
// result not valid due to collision
@@ -238,14 +238,14 @@ namespace GraspStudio
eval.graspValid.push_back(false);
eval.nrGraspsInvalidContacts++;
eval.timeGraspMS.push_back(ms);
-
+
return returnNullWhenInvalid ? nullptr : grasp;
}
graspQuality->setContactPoints(contacts);
float score = graspQuality->getGraspQuality();
grasp->setQuality(score);
-
+
if (forceClosure && !graspQuality->isGraspForceClosure())
{
// not force closure
@@ -254,7 +254,7 @@ namespace GraspStudio
eval.graspValid.push_back(false);
eval.nrGraspsInvalidFC++;
eval.timeGraspMS.push_back(ms);
-
+
return returnNullWhenInvalid ? nullptr : grasp;
}
@@ -266,7 +266,7 @@ namespace GraspStudio
eval.graspValid.push_back(false);
eval.nrGraspsInvalidFC++;
eval.timeGraspMS.push_back(ms);
-
+
return returnNullWhenInvalid ? nullptr : grasp;
}
@@ -277,7 +277,7 @@ namespace GraspStudio
}
float ms = getTimeMS();
-
+
eval.graspScore.push_back(score);
eval.graspValid.push_back(true);
eval.nrGraspsValid++;
diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.h b/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
index 7e426ab20dff76a45454be4041f19cb43198863d..0903780fe0eb6ec0aec7f7b27465b9dcda7ce41d 100644
--- a/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
+++ b/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
@@ -73,7 +73,7 @@ namespace GraspStudio
void setParameters(float minQuality, bool forceClosure);
- /**
+ /**
* if enabled (default), the planner retreates the hand if the number of contacts is <2.
* During retreat, the contacts are checked if a better situation can be achieved.
* This procedure helps in case the object is small.
@@ -92,7 +92,7 @@ namespace GraspStudio
std::chrono::system_clock::time_point startTime;
std::chrono::milliseconds timeOutDuration;
-
+
VirtualRobot::EndEffector::ContactInfoVector contacts;
GraspStudio::GraspQualityMeasurePtr graspQuality;
GraspStudio::ApproachMovementGeneratorPtr approach;
diff --git a/GraspPlanning/GraspPlanner/GraspPlanner.cpp b/GraspPlanning/GraspPlanner/GraspPlanner.cpp
index c8c40989ca4923cb75e49e2f1fb060092aa681fd..da9a50c2c6fbfd09c46326538758b950f1191a10 100644
--- a/GraspPlanning/GraspPlanner/GraspPlanner.cpp
+++ b/GraspPlanning/GraspPlanner/GraspPlanner.cpp
@@ -9,7 +9,7 @@ namespace GraspStudio
}
GraspPlanner::~GraspPlanner()
- = default;
+ = default;
void GraspPlanner::setVerbose(bool enable)
{
@@ -25,7 +25,7 @@ namespace GraspStudio
{
return eval;
}
-
+
void GraspPlanner::clearEvaluation()
{
this->eval = {};
diff --git a/GraspPlanning/GraspPlanner/GraspPlanner.h b/GraspPlanning/GraspPlanner/GraspPlanner.h
index d4668daa6a8837da2caee97aeae1cbf1337c9c3f..b38cc46520ac099f282c4d56a46f6db0e7d2bb46 100644
--- a/GraspPlanning/GraspPlanner/GraspPlanner.h
+++ b/GraspPlanning/GraspPlanner/GraspPlanner.h
@@ -72,10 +72,10 @@ namespace GraspStudio
GraspPlannerEvaluation getEvaluation() const;
/// Clear the evaluation.
void clearEvaluation();
-
-
+
+
protected:
-
+
bool verbose;
VirtualRobot::GraspSetPtr graspSet;
std::vector<VirtualRobot::GraspPtr> plannedGrasps;
diff --git a/GraspPlanning/GraspPlanner/GraspPlannerEvaluation.cpp b/GraspPlanning/GraspPlanner/GraspPlannerEvaluation.cpp
index 3a24f3f6e4e2ea342d0296a6e9cdb7141a74d8c7..6f65ce1a48155350dbbf9d4940c6361891193d69 100644
--- a/GraspPlanning/GraspPlanner/GraspPlannerEvaluation.cpp
+++ b/GraspPlanning/GraspPlanner/GraspPlannerEvaluation.cpp
@@ -25,158 +25,170 @@
namespace GraspStudio
{
-GraspPlannerEvaluation::GraspPlannerEvaluation()
-{
- nrGraspsGenerated = 0;
- nrGraspsValid = 0;
- nrGraspsInvalidCollision = 0;
- nrGraspsInvalidFC = 0;
- nrGraspsInvalidContacts = 0;
- nrGraspsValidPrecision = 0;
- nrGraspsValidPower = 0;
- fcCheck = false;
- minQuality = 0.0f;
-}
-
-void GraspPlannerEvaluation::print()
-{
- std::cout << (*this) << std::endl;
-}
-
-std::string GraspPlannerEvaluation::GetCSVHeader()
-{
- std::stringstream fs;
- fs << "minQuality" << ","
- << "nrGraspsGenerated" << ","
- << "nrGraspsValid" << ","
- << "nrGraspsInValid" << ","
- << "nrGraspsValidPrecision" << ","
- << "nrGraspsValidPower" << ","
- << "nrGraspsInvalidCollision" << ","
- << "nrGraspsInvalidFC" << ","
- << "nrGraspsInvalidContacts" << ","
- << "AverageDurationMS" << ","
- << "percPowerGrasps" << ","
- << "percPrecGraps" << ","
- << "avgScore";
- return fs.str();
-}
-
-std::string GraspPlannerEvaluation::toCSVString() const
-{
- float timeAcc = 0;
- float avgTime = 0;
- float scoreAcc = 0;
- int nrGrasps = static_cast<int>(timeGraspMS.size());
- int nrValid = 0;
- int nrPower = 0;
- int nrPrecision = 0;
- for (size_t i = 0; i < timeGraspMS.size(); i++)
+ GraspPlannerEvaluation::GraspPlannerEvaluation()
{
- timeAcc+=timeGraspMS.at(i);
- if (graspValid.at(i))
- {
- nrValid++;
- scoreAcc+=graspScore.at(i);
- }
- if (graspTypePower.at(i))
- nrPower++;
- else
- nrPrecision++;
+ nrGraspsGenerated = 0;
+ nrGraspsValid = 0;
+ nrGraspsInvalidCollision = 0;
+ nrGraspsInvalidFC = 0;
+ nrGraspsInvalidContacts = 0;
+ nrGraspsValidPrecision = 0;
+ nrGraspsValidPower = 0;
+ fcCheck = false;
+ minQuality = 0.0f;
}
- float percPower = 0;
- float percPrec = 0;
- if (nrGrasps > 0)
+
+ void GraspPlannerEvaluation::print()
{
- percPower = static_cast<float>(nrPower) / static_cast<float>(nrGrasps);
- percPrec = static_cast<float>(nrPrecision) / static_cast<float>(nrGrasps);
- avgTime = timeAcc / static_cast<float>(nrGrasps);
+ std::cout << (*this) << std::endl;
}
- float avgScore = 0;
- if (nrValid > 0)
+
+ std::string GraspPlannerEvaluation::GetCSVHeader()
{
- avgScore = scoreAcc / nrValid;
+ std::stringstream fs;
+ fs << "minQuality" << ","
+ << "nrGraspsGenerated" << ","
+ << "nrGraspsValid" << ","
+ << "nrGraspsInValid" << ","
+ << "nrGraspsValidPrecision" << ","
+ << "nrGraspsValidPower" << ","
+ << "nrGraspsInvalidCollision" << ","
+ << "nrGraspsInvalidFC" << ","
+ << "nrGraspsInvalidContacts" << ","
+ << "AverageDurationMS" << ","
+ << "percPowerGrasps" << ","
+ << "percPrecGraps" << ","
+ << "avgScore";
+ return fs.str();
}
-
-
- std::stringstream fs;
- fs << minQuality << ","
- << nrGraspsGenerated << ","
- << nrGraspsValid << ","
- << (nrGraspsInvalidCollision+nrGraspsInvalidFC+nrGraspsInvalidContacts) << ","
- << nrGraspsValidPrecision << ","
- << nrGraspsValidPower << ","
- << nrGraspsInvalidCollision << ","
- << nrGraspsInvalidFC << ","
- << nrGraspsInvalidContacts << ","
- << avgTime << ","
- << percPower << ","
- << percPrec << ","
- << avgScore;
- return fs.str();
-}
-std::ostream& operator<<(std::ostream& os, const GraspPlannerEvaluation& rhs)
-{
- os << "---------------- GRASP PLANNER EVALUATION -----------" << std::endl;
- if (rhs.fcCheck)
- os << "ForceClosure check: true" << std::endl;
- else
- os << "ForceClosure check: false" << std::endl;
- os << "Min Quality: " << rhs.minQuality << std::endl;
-
- os << "nrGraspsGenerated: " << rhs.nrGraspsGenerated << std::endl;
- os << "nrGraspsValid: " << rhs.nrGraspsValid << std::endl;
- os << "nrGraspsInValid: " << (rhs.nrGraspsInvalidCollision + rhs.nrGraspsInvalidFC
- + rhs.nrGraspsInvalidContacts) << std::endl;
- os << "nrGraspsValidPrecision: " << rhs.nrGraspsValidPrecision << std::endl;
- os << "nrGraspsValidPower: " << rhs.nrGraspsValidPower << std::endl;
- os << "nrGraspsInvalidCollision: " << rhs.nrGraspsInvalidCollision << std::endl;
- os << "nrGraspsInvalidFC: " << rhs.nrGraspsInvalidFC << std::endl;
- os << "nrGraspsInvalidContacts: " << rhs.nrGraspsInvalidContacts << std::endl;
- VR_ASSERT (rhs.timeGraspMS.size() == rhs.graspScore.size());
- VR_ASSERT (rhs.timeGraspMS.size() == rhs.graspValid.size());
- VR_ASSERT (rhs.timeGraspMS.size() == rhs.graspTypePower.size());
- float timeAcc = 0;
- float scoreAcc = 0;
- int nrGrasps = static_cast<int>(rhs.timeGraspMS.size());
- int nrValid = 0;
- int nrPower = 0;
- int nrPrecision = 0;
- for (size_t i = 0; i < rhs.timeGraspMS.size(); i++)
+ std::string GraspPlannerEvaluation::toCSVString() const
{
- timeAcc += rhs.timeGraspMS.at(i);
- if (rhs.graspValid.at(i))
+ float timeAcc = 0;
+ float avgTime = 0;
+ float scoreAcc = 0;
+ int nrGrasps = static_cast<int>(timeGraspMS.size());
+ int nrValid = 0;
+ int nrPower = 0;
+ int nrPrecision = 0;
+ for (size_t i = 0; i < timeGraspMS.size(); i++)
{
- nrValid++;
- scoreAcc += rhs.graspScore.at(i);
+ timeAcc += timeGraspMS.at(i);
+ if (graspValid.at(i))
+ {
+ nrValid++;
+ scoreAcc += graspScore.at(i);
+ }
+ if (graspTypePower.at(i))
+ {
+ nrPower++;
+ }
+ else
+ {
+ nrPrecision++;
+ }
}
- if (rhs.graspTypePower.at(i))
- nrPower++;
- else
- nrPrecision++;
- }
- os << "Time complete: " << timeAcc << " ms" << std::endl;
- if (nrGrasps > 0)
- {
- os << "Avg time per grasp (valid&invalid): " <<
- static_cast<float>(timeAcc) / static_cast<float>(nrGrasps) << " ms" << std::endl;
- float percPower = static_cast<float>(nrPower) / static_cast<float>(nrGrasps);
- float percPrec = static_cast<float>(nrPrecision) / static_cast<float>(nrGrasps);
- os << "Precision grasps: " << nrPrecision << " -> " << percPrec*100 << "%" << std::endl;
- os << "Power grasps: " << nrPower << " -> " << percPower*100 << "%" << std::endl;
+ float percPower = 0;
+ float percPrec = 0;
+ if (nrGrasps > 0)
+ {
+ percPower = static_cast<float>(nrPower) / static_cast<float>(nrGrasps);
+ percPrec = static_cast<float>(nrPrecision) / static_cast<float>(nrGrasps);
+ avgTime = timeAcc / static_cast<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 << ","
+ << avgTime << ","
+ << percPower << ","
+ << percPrec << ","
+ << avgScore;
+ return fs.str();
}
- if (nrValid > 0)
+
+ std::ostream& operator<<(std::ostream& os, const GraspPlannerEvaluation& rhs)
{
- os << "Avg score (valid):" << scoreAcc / nrValid << std::endl;
+ os << "---------------- GRASP PLANNER EVALUATION -----------" << std::endl;
+ if (rhs.fcCheck)
+ {
+ os << "ForceClosure check: true" << std::endl;
+ }
+ else
+ {
+ os << "ForceClosure check: false" << std::endl;
+ }
+ os << "Min Quality: " << rhs.minQuality << std::endl;
+
+ os << "nrGraspsGenerated: " << rhs.nrGraspsGenerated << std::endl;
+ os << "nrGraspsValid: " << rhs.nrGraspsValid << std::endl;
+ os << "nrGraspsInValid: " << (rhs.nrGraspsInvalidCollision + rhs.nrGraspsInvalidFC
+ + rhs.nrGraspsInvalidContacts) << std::endl;
+ os << "nrGraspsValidPrecision: " << rhs.nrGraspsValidPrecision << std::endl;
+ os << "nrGraspsValidPower: " << rhs.nrGraspsValidPower << std::endl;
+ os << "nrGraspsInvalidCollision: " << rhs.nrGraspsInvalidCollision << std::endl;
+ os << "nrGraspsInvalidFC: " << rhs.nrGraspsInvalidFC << std::endl;
+ os << "nrGraspsInvalidContacts: " << rhs.nrGraspsInvalidContacts << std::endl;
+ VR_ASSERT(rhs.timeGraspMS.size() == rhs.graspScore.size());
+ VR_ASSERT(rhs.timeGraspMS.size() == rhs.graspValid.size());
+ VR_ASSERT(rhs.timeGraspMS.size() == rhs.graspTypePower.size());
+ float timeAcc = 0;
+ float scoreAcc = 0;
+ int nrGrasps = static_cast<int>(rhs.timeGraspMS.size());
+ int nrValid = 0;
+ int nrPower = 0;
+ int nrPrecision = 0;
+ for (size_t i = 0; i < rhs.timeGraspMS.size(); i++)
+ {
+ timeAcc += rhs.timeGraspMS.at(i);
+ if (rhs.graspValid.at(i))
+ {
+ nrValid++;
+ scoreAcc += rhs.graspScore.at(i);
+ }
+ if (rhs.graspTypePower.at(i))
+ {
+ nrPower++;
+ }
+ else
+ {
+ nrPrecision++;
+ }
+ }
+ os << "Time complete: " << timeAcc << " ms" << std::endl;
+ if (nrGrasps > 0)
+ {
+ os << "Avg time per grasp (valid&invalid): " <<
+ static_cast<float>(timeAcc) / static_cast<float>(nrGrasps) << " ms" << std::endl;
+ float percPower = static_cast<float>(nrPower) / static_cast<float>(nrGrasps);
+ float percPrec = static_cast<float>(nrPrecision) / static_cast<float>(nrGrasps);
+ os << "Precision grasps: " << nrPrecision << " -> " << percPrec * 100 << "%" << std::endl;
+ os << "Power grasps: " << nrPower << " -> " << percPower * 100 << "%" << std::endl;
+ }
+ if (nrValid > 0)
+ {
+ os << "Avg score (valid):" << scoreAcc / nrValid << std::endl;
+ }
+ if (rhs.nrGraspsGenerated > 0)
+ os << "Percentage of valid grasps: "
+ << static_cast<float>(rhs.nrGraspsValid) / static_cast<float>(rhs.nrGraspsGenerated) * 100.0f
+ << "%" << std::endl;
+
+ return os;
}
- if (rhs.nrGraspsGenerated > 0)
- os << "Percentage of valid grasps: "
- << static_cast<float>(rhs.nrGraspsValid) / static_cast<float>(rhs.nrGraspsGenerated) * 100.0f
- << "%" << std::endl;
-
- return os;
-}
}
diff --git a/GraspPlanning/GraspPlanner/GraspPlannerEvaluation.h b/GraspPlanning/GraspPlanner/GraspPlannerEvaluation.h
index c57642688440e1adcab139fe63a6dedf16114b16..b84c3a0f2f86b2d745e91c5325e6cc8313c705bf 100644
--- a/GraspPlanning/GraspPlanner/GraspPlannerEvaluation.h
+++ b/GraspPlanning/GraspPlanner/GraspPlannerEvaluation.h
@@ -33,34 +33,34 @@ namespace GraspStudio
struct GraspPlannerEvaluation
{
GraspPlannerEvaluation();
-
+
void print();
-
+
static std::string GetCSVHeader();
-
+
std::string toCSVString() const;
-
-
+
+
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;
-
+
};
-
+
std::ostream& operator<< (std::ostream& os, const GraspPlannerEvaluation& rhs);
}
diff --git a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
index 5d960bc17a643c1a5f419b0e67b99eb53efad2e0..e822ef4fccf474aad6867531ba9ce7387724f17c 100644
--- a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
+++ b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
@@ -12,419 +12,437 @@
using namespace Eigen;
using namespace VirtualRobot;
-namespace GraspStudio {
-
-GraspEvaluationPoseUncertainty::GraspEvaluationPoseUncertainty(const PoseUncertaintyConfig& config)
+namespace GraspStudio
{
- this->_config = config;
-}
-
-GraspEvaluationPoseUncertainty::~GraspEvaluationPoseUncertainty()
-= default;
-
-
-std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(
- const Eigen::Matrix4f &objectGP, const Eigen::Matrix4f &graspCenterGP)
-{
- std::vector<Eigen::Matrix4f> result;
- Eigen::Matrix4f trafoGraspCenterToObjectCenter = objectGP * graspCenterGP.inverse();
- Eigen::Matrix4f initPose = graspCenterGP;
- Eigen::Vector3f rpy;
- MathTools::eigen4f2rpy(initPose, rpy);
-
- float initPoseRPY[6];
- initPoseRPY[0] = initPose(0, 3);
- initPoseRPY[1] = initPose(1, 3);
- initPoseRPY[2] = initPose(2, 3);
- initPoseRPY[3] = rpy(0);
- initPoseRPY[4] = rpy(1);
- initPoseRPY[5] = rpy(2);
-
- float start[6];
- float end[6];
- float step[6];
- float tmpPose[6];
-
- for (int i = 0; i < 6; i++)
- {
- if (_config.enableDimension[i])
- {
- start[i] = initPoseRPY[i] - _config.dimExtends[i];
- end[i] = initPoseRPY[i] + _config.dimExtends[i];
- step[i] = _config.stepSize[i];
- }
- else
- {
- start[i] = initPoseRPY[i];
- end[i] = initPoseRPY[i];
- step[i] = 1.0f;
- }
- }
-
- Eigen::Matrix4f m;
-
- for (float a = start[0]; a <= end[0]; a += step[0])
- {
- tmpPose[0] = a;
- for (float b = start[1]; b <= end[1]; b += step[1])
- {
- tmpPose[1] = b;
- for (float c = start[2]; c <= end[2]; c += step[2])
- {
- tmpPose[2] = c;
- for (float d = start[3]; d <= end[3]; d += step[3])
- {
- tmpPose[3] = d;
- for (float e = start[4]; e <= end[4]; e += step[4])
- {
- tmpPose[4] = e;
- for (float f = start[5]; f <= end[5]; f += step[5])
- {
- tmpPose[5] = f;
- MathTools::posrpy2eigen4f(tmpPose, m);
- m = m * trafoGraspCenterToObjectCenter;
- result.push_back(m);
- }
- }
- }
- }
- }
- }
- return result;
-}
-
-std::vector<Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(const Matrix4f& objectGP, const EndEffector::ContactInfoVector& contacts)
-{
- Eigen::Vector3f centerPos = getMean(contacts);
- if (centerPos.hasNaN())
+ GraspEvaluationPoseUncertainty::GraspEvaluationPoseUncertainty(const PoseUncertaintyConfig& config)
{
- VR_ERROR << "No contacts" << endl;
- return std::vector<Eigen::Matrix4f>();
+ this->_config = config;
}
-
- if (_config.verbose)
- cout << "using contact center pose:\n" << centerPos << endl;
- return generatePoses(objectGP, math::Helpers::Pose(centerPos));
-}
+ GraspEvaluationPoseUncertainty::~GraspEvaluationPoseUncertainty()
+ = default;
-std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(
- const Eigen::Matrix4f &objectGP, const Eigen::Matrix4f &graspCenterGP, int numPoses)
-{
- std::vector<Eigen::Matrix4f> result;
- //Eigen::Matrix4f trafoGraspCenterToObjectCenter = objectGP * graspCenterGP.inverse();
- Eigen::Matrix4f trafoGraspCenterToObjectCenter = math::Helpers::InvertedPose(graspCenterGP) * objectGP;
- Eigen::Matrix4f initPose = graspCenterGP;
- Eigen::Vector3f rpy;
- MathTools::eigen4f2rpy(initPose, rpy);
-
- float initPoseRPY[6];
- initPoseRPY[0] = initPose(0, 3);
- initPoseRPY[1] = initPose(1, 3);
- initPoseRPY[2] = initPose(2, 3);
- initPoseRPY[3] = rpy(0);
- initPoseRPY[4] = rpy(1);
- initPoseRPY[5] = rpy(2);
-
- float start[6];
- float dist[6];
- float tmpPose[6];
- for (int i = 0; i < 6; i++)
+
+ std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(
+ const Eigen::Matrix4f& objectGP, const Eigen::Matrix4f& graspCenterGP)
{
- start[i] = initPoseRPY[i];
- if (_config.enableDimension[i])
+ std::vector<Eigen::Matrix4f> result;
+ Eigen::Matrix4f trafoGraspCenterToObjectCenter = objectGP * graspCenterGP.inverse();
+ Eigen::Matrix4f initPose = graspCenterGP;
+ Eigen::Vector3f rpy;
+ MathTools::eigen4f2rpy(initPose, rpy);
+
+ float initPoseRPY[6];
+ initPoseRPY[0] = initPose(0, 3);
+ initPoseRPY[1] = initPose(1, 3);
+ initPoseRPY[2] = initPose(2, 3);
+ initPoseRPY[3] = rpy(0);
+ initPoseRPY[4] = rpy(1);
+ initPoseRPY[5] = rpy(2);
+
+ float start[6];
+ float end[6];
+ float step[6];
+ float tmpPose[6];
+
+ for (int i = 0; i < 6; i++)
{
- if (i < 3)
- dist[i] = _config.posDeltaMM;
+ if (_config.enableDimension[i])
+ {
+ start[i] = initPoseRPY[i] - _config.dimExtends[i];
+ end[i] = initPoseRPY[i] + _config.dimExtends[i];
+ step[i] = _config.stepSize[i];
+ }
else
- dist[i] = _config.oriDeltaDeg * static_cast<float>(M_PI / 180.0);
+ {
+ start[i] = initPoseRPY[i];
+ end[i] = initPoseRPY[i];
+ step[i] = 1.0f;
+ }
}
- else
+
+ Eigen::Matrix4f m;
+
+ for (float a = start[0]; a <= end[0]; a += step[0])
{
- dist[i] = 0.0f;
+ tmpPose[0] = a;
+ for (float b = start[1]; b <= end[1]; b += step[1])
+ {
+ tmpPose[1] = b;
+ for (float c = start[2]; c <= end[2]; c += step[2])
+ {
+ tmpPose[2] = c;
+ for (float d = start[3]; d <= end[3]; d += step[3])
+ {
+ tmpPose[3] = d;
+ for (float e = start[4]; e <= end[4]; e += step[4])
+ {
+ tmpPose[4] = e;
+ for (float f = start[5]; f <= end[5]; f += step[5])
+ {
+ tmpPose[5] = f;
+ MathTools::posrpy2eigen4f(tmpPose, m);
+ m = m * trafoGraspCenterToObjectCenter;
+ result.push_back(m);
+ }
+ }
+ }
+ }
+ }
}
+ return result;
}
- Eigen::Matrix4f m;
-
- VirtualRobot::ClampedNormalDistribution<float> normalDistribution(-1, 1);
- std::uniform_real_distribution<float> uniformDistribution(-1, 1);
-
- for (int j = 0; j < numPoses; j++)
+ std::vector<Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(const Matrix4f& objectGP, const EndEffector::ContactInfoVector& contacts)
{
- for (int i = 0; i < 6; i++)
+ Eigen::Vector3f centerPos = getMean(contacts);
+ if (centerPos.hasNaN())
{
- float r = _config.useNormalDistribution ? normalDistribution(VirtualRobot::PRNG64Bit())
- : uniformDistribution(VirtualRobot::PRNG64Bit());
- tmpPose[i] = start[i] + r * dist[i];
+ VR_ERROR << "No contacts" << endl;
+ return std::vector<Eigen::Matrix4f>();
}
- MathTools::posrpy2eigen4f(tmpPose, m);
- m = m * trafoGraspCenterToObjectCenter;
- result.push_back(m);
- }
- return result;
-}
-std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(
- const Eigen::Matrix4f &objectGP,
- const VirtualRobot::EndEffector::ContactInfoVector &contacts,
- int numPoses)
-{
- Eigen::Vector3f centerPose = getMean(contacts);
- if (centerPose.hasNaN())
- {
- VR_ERROR << "No contacts" << endl;
- return std::vector<Eigen::Matrix4f>();
+ if (_config.verbose)
+ {
+ cout << "using contact center pose:\n" << centerPos << endl;
+ }
+
+ return generatePoses(objectGP, math::Helpers::Pose(centerPos));
}
-
- if (_config.verbose)
- cout << "using contact center pose:\n" << centerPose << endl;
- return generatePoses(objectGP, math::Helpers::Pose(centerPose), numPoses);
-}
-GraspEvaluationPoseUncertainty::PoseEvalResult GraspEvaluationPoseUncertainty::evaluatePose(
- EndEffectorPtr eef, ObstaclePtr object, const Matrix4f &objectPose,
- GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape)
-{
- PoseEvalResult result;
- result.forceClosure = false;
- result.quality = 0.0f;
- result.initialCollision = false;
+ std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(
+ const Eigen::Matrix4f& objectGP, const Eigen::Matrix4f& graspCenterGP, int numPoses)
+ {
+ std::vector<Eigen::Matrix4f> result;
+ //Eigen::Matrix4f trafoGraspCenterToObjectCenter = objectGP * graspCenterGP.inverse();
+ Eigen::Matrix4f trafoGraspCenterToObjectCenter = math::Helpers::InvertedPose(graspCenterGP) * objectGP;
+ Eigen::Matrix4f initPose = graspCenterGP;
+ Eigen::Vector3f rpy;
+ MathTools::eigen4f2rpy(initPose, rpy);
+
+ float initPoseRPY[6];
+ initPoseRPY[0] = initPose(0, 3);
+ initPoseRPY[1] = initPose(1, 3);
+ initPoseRPY[2] = initPose(2, 3);
+ initPoseRPY[3] = rpy(0);
+ initPoseRPY[4] = rpy(1);
+ initPoseRPY[5] = rpy(2);
+
+ float start[6];
+ float dist[6];
+ float tmpPose[6];
+ for (int i = 0; i < 6; i++)
+ {
+ start[i] = initPoseRPY[i];
+ if (_config.enableDimension[i])
+ {
+ if (i < 3)
+ {
+ dist[i] = _config.posDeltaMM;
+ }
+ else
+ {
+ dist[i] = _config.oriDeltaDeg * static_cast<float>(M_PI / 180.0);
+ }
+ }
+ else
+ {
+ dist[i] = 0.0f;
+ }
+ }
- SceneObjectSetPtr eefColModel = eef->createSceneObjectSet();
+ Eigen::Matrix4f m;
- if (!eef || !qm)
- {
- VR_ERROR << "Missing parameters" << endl;
+ VirtualRobot::ClampedNormalDistribution<float> normalDistribution(-1, 1);
+ std::uniform_real_distribution<float> uniformDistribution(-1, 1);
+
+ for (int j = 0; j < numPoses; j++)
+ {
+ for (int i = 0; i < 6; i++)
+ {
+ float r = _config.useNormalDistribution ? normalDistribution(VirtualRobot::PRNG64Bit())
+ : uniformDistribution(VirtualRobot::PRNG64Bit());
+ tmpPose[i] = start[i] + r * dist[i];
+ }
+ MathTools::posrpy2eigen4f(tmpPose, m);
+ m = m * trafoGraspCenterToObjectCenter;
+ result.push_back(m);
+ }
return result;
}
- if (preshape)
- {
- eef->getRobot()->setJointValues(preshape);
- } else
+ std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(
+ const Eigen::Matrix4f& objectGP,
+ const VirtualRobot::EndEffector::ContactInfoVector& contacts,
+ int numPoses)
{
- eef->openActors();
+ Eigen::Vector3f centerPose = getMean(contacts);
+ if (centerPose.hasNaN())
+ {
+ VR_ERROR << "No contacts" << endl;
+ return std::vector<Eigen::Matrix4f>();
+ }
+
+ if (_config.verbose)
+ {
+ cout << "using contact center pose:\n" << centerPose << endl;
+ }
+
+ return generatePoses(objectGP, math::Helpers::Pose(centerPose), numPoses);
}
- object->setGlobalPose(objectPose);
- // check for initial collision
- if (object->getCollisionChecker()->checkCollision(object->getCollisionModel(), eefColModel))
+ GraspEvaluationPoseUncertainty::PoseEvalResult GraspEvaluationPoseUncertainty::evaluatePose(
+ EndEffectorPtr eef, ObstaclePtr object, const Matrix4f& objectPose,
+ GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape)
{
- result.initialCollision = true;
- return result;
- }
+ PoseEvalResult result;
+ result.forceClosure = false;
+ result.quality = 0.0f;
+ result.initialCollision = false;
- // collision free
- EndEffector::ContactInfoVector cont = eef->closeActors(object);
- qm->setContactPoints(cont);
+ SceneObjectSetPtr eefColModel = eef->createSceneObjectSet();
+
+ if (!eef || !qm)
+ {
+ VR_ERROR << "Missing parameters" << endl;
+ return result;
+ }
- result.quality = qm->getGraspQuality();
- result.forceClosure = qm->isGraspForceClosure();
+ if (preshape)
+ {
+ eef->getRobot()->setJointValues(preshape);
+ }
+ else
+ {
+ eef->openActors();
+ }
+ object->setGlobalPose(objectPose);
- return result;
-}
+ // check for initial collision
+ if (object->getCollisionChecker()->checkCollision(object->getCollisionModel(), eefColModel))
+ {
+ result.initialCollision = true;
+ return result;
+ }
-GraspEvaluationPoseUncertainty::PoseEvalResults GraspEvaluationPoseUncertainty::evaluatePoses(
- EndEffectorPtr eef, ObstaclePtr object, const std::vector<Eigen::Matrix4f> &objectPoses,
- GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape)
-{
- PoseEvalResults res;
- res.avgQuality = 0.0f;
- res.forceClosureRate = 0.0f;
- res.avgQualityCol = 0.0f;
- res.forceClosureRateCol = 0.0f;
- res.numPosesTested = 0;
- res.numValidPoses = 0;
- res.numColPoses = 0;
+ // collision free
+ EndEffector::ContactInfoVector cont = eef->closeActors(object);
+ qm->setContactPoints(cont);
+ result.quality = qm->getGraspQuality();
+ result.forceClosure = qm->isGraspForceClosure();
- if (!eef || !qm)
- {
- VR_ERROR << "Missing parameters" << endl;
- return res;
+ return result;
}
- if (!eef->getRobot())
+ GraspEvaluationPoseUncertainty::PoseEvalResults GraspEvaluationPoseUncertainty::evaluatePoses(
+ EndEffectorPtr eef, ObstaclePtr object, const std::vector<Eigen::Matrix4f>& objectPoses,
+ GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape)
{
- VR_WARNING << "missing eef->robot" << endl;
- return res;
- }
+ PoseEvalResults res;
+ res.avgQuality = 0.0f;
+ res.forceClosureRate = 0.0f;
+ res.avgQualityCol = 0.0f;
+ res.forceClosureRateCol = 0.0f;
+ res.numPosesTested = 0;
+ res.numValidPoses = 0;
+ res.numColPoses = 0;
- Eigen::Matrix4f eefRobotPoseInit = eef->getRobot()->getGlobalPose();
- Eigen::Matrix4f objectPoseInit = object->getGlobalPose();
- VirtualRobot::RobotConfigPtr initialConf = eef->getConfiguration();
- std::vector<PoseEvalResult> results;
- for (const auto& objectPose : objectPoses)
- {
- results.push_back(evaluatePose(eef, object, objectPose, qm, preshape));
- }
+ if (!eef || !qm)
+ {
+ VR_ERROR << "Missing parameters" << endl;
+ return res;
+ }
- if (results.empty())
- return res;
+ if (!eef->getRobot())
+ {
+ VR_WARNING << "missing eef->robot" << endl;
+ return res;
+ }
- res.numPosesTested = static_cast<int>(results.size());
- for (const auto& result : results)
- {
- if (result.initialCollision)
+ Eigen::Matrix4f eefRobotPoseInit = eef->getRobot()->getGlobalPose();
+ Eigen::Matrix4f objectPoseInit = object->getGlobalPose();
+ VirtualRobot::RobotConfigPtr initialConf = eef->getConfiguration();
+
+ std::vector<PoseEvalResult> results;
+ for (const auto& objectPose : objectPoses)
{
- res.numColPoses++;
+ results.push_back(evaluatePose(eef, object, objectPose, qm, preshape));
}
- else
+
+ if (results.empty())
{
- res.numValidPoses++;
- res.avgQuality += result.quality;
- res.avgQualityCol += result.quality;
- if (result.forceClosure)
+ return res;
+ }
+
+ res.numPosesTested = static_cast<int>(results.size());
+ for (const auto& result : results)
+ {
+ if (result.initialCollision)
+ {
+ res.numColPoses++;
+ }
+ else
{
- res.forceClosureRate += 1.0f;
- res.forceClosureRateCol += 1.0f;
- res.numForceClosurePoses++;
+ res.numValidPoses++;
+ res.avgQuality += result.quality;
+ res.avgQualityCol += result.quality;
+ if (result.forceClosure)
+ {
+ res.forceClosureRate += 1.0f;
+ res.forceClosureRateCol += 1.0f;
+ res.numForceClosurePoses++;
+ }
}
}
- }
- if (res.numValidPoses > 0)
- {
- res.forceClosureRate /= static_cast<float>(res.numValidPoses);
- res.avgQuality /= static_cast<float>(res.numValidPoses);
- }
- if (res.numPosesTested > 0)
- {
- res.forceClosureRateCol /= static_cast<float>(res.numPosesTested);
- res.avgQualityCol /= static_cast<float>(res.numPosesTested);
- }
-
- // restore setup
- eef->getRobot()->setGlobalPose(eefRobotPoseInit);
- object->setGlobalPose(objectPoseInit);
- eef->getRobot()->setConfig(initialConf);
+ if (res.numValidPoses > 0)
+ {
+ res.forceClosureRate /= static_cast<float>(res.numValidPoses);
+ res.avgQuality /= static_cast<float>(res.numValidPoses);
+ }
+ if (res.numPosesTested > 0)
+ {
+ res.forceClosureRateCol /= static_cast<float>(res.numPosesTested);
+ res.avgQualityCol /= static_cast<float>(res.numPosesTested);
+ }
- return res;
-}
+ // restore setup
+ eef->getRobot()->setGlobalPose(eefRobotPoseInit);
+ object->setGlobalPose(objectPoseInit);
+ eef->getRobot()->setConfig(initialConf);
-GraspEvaluationPoseUncertainty::PoseEvalResults GraspEvaluationPoseUncertainty::evaluateGrasp(
- VirtualRobot::GraspPtr grasp, VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object,
- GraspQualityMeasurePtr qm, int numPoses)
-{
- PoseEvalResults res;
- res.avgQuality = 0.0f;
- res.forceClosureRate = 0.0f;
- res.numPosesTested = 0;
- res.numValidPoses = 0;
- res.numColPoses = 0;
-
- if (!grasp || !eef || !object || !qm)
- {
- VR_WARNING << "missing parameters"<< endl;
return res;
}
- if (!eef->getRobot())
+
+ GraspEvaluationPoseUncertainty::PoseEvalResults GraspEvaluationPoseUncertainty::evaluateGrasp(
+ VirtualRobot::GraspPtr grasp, VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object,
+ GraspQualityMeasurePtr qm, int numPoses)
{
- VR_WARNING << "missing eef->robot" << endl;
- return res;
- }
+ PoseEvalResults res;
+ res.avgQuality = 0.0f;
+ res.forceClosureRate = 0.0f;
+ res.numPosesTested = 0;
+ res.numValidPoses = 0;
+ res.numColPoses = 0;
+
+ if (!grasp || !eef || !object || !qm)
+ {
+ VR_WARNING << "missing parameters" << endl;
+ return res;
+ }
+ if (!eef->getRobot())
+ {
+ VR_WARNING << "missing eef->robot" << endl;
+ return res;
+ }
- Eigen::Matrix4f eefRobotPoseInit = eef->getRobot()->getGlobalPose();
- Eigen::Matrix4f objectPoseInit = object->getGlobalPose();
- VirtualRobot::RobotConfigPtr initialConf = eef->getConfiguration();
+ Eigen::Matrix4f eefRobotPoseInit = eef->getRobot()->getGlobalPose();
+ Eigen::Matrix4f objectPoseInit = object->getGlobalPose();
+ VirtualRobot::RobotConfigPtr initialConf = eef->getConfiguration();
- std::string graspPreshapeName = grasp->getPreshapeName();
- VirtualRobot::RobotConfigPtr graspPS;
- if (eef->hasPreshape(graspPreshapeName))
- graspPS = eef->getPreshape(graspPreshapeName);
+ std::string graspPreshapeName = grasp->getPreshapeName();
+ VirtualRobot::RobotConfigPtr graspPS;
+ if (eef->hasPreshape(graspPreshapeName))
+ {
+ graspPS = eef->getPreshape(graspPreshapeName);
+ }
- Eigen::Matrix4f mGrasp = grasp->getTcpPoseGlobal(object->getGlobalPose());
+ Eigen::Matrix4f mGrasp = grasp->getTcpPoseGlobal(object->getGlobalPose());
- // apply grasp
- eef->getRobot()->setGlobalPoseForRobotNode(eef->getTcp(), mGrasp);
+ // apply grasp
+ eef->getRobot()->setGlobalPoseForRobotNode(eef->getTcp(), mGrasp);
- if (graspPS)
- {
- eef->getRobot()->setJointValues(graspPS);
- }
- else
- {
- eef->openActors();
- }
-
+ if (graspPS)
+ {
+ eef->getRobot()->setJointValues(graspPS);
+ }
+ else
+ {
+ eef->openActors();
+ }
- auto contacts = eef->closeActors(object);
- if (contacts.empty())
- {
- VR_INFO << "No contacts for grasp " << grasp->getName() << " found" << std::endl;
+
+ auto contacts = eef->closeActors(object);
+ if (contacts.empty())
+ {
+ VR_INFO << "No contacts for grasp " << grasp->getName() << " found" << std::endl;
+ return res;
+ }
+
+ auto poses = generatePoses(object->getGlobalPose(), contacts, numPoses);
+ if (poses.empty())
+ {
+ VR_INFO << "No poses for grasp found" << std::endl;
+ return res;
+ }
+ res = evaluatePoses(eef, object, poses, qm, graspPS);
+
+ // restore setup
+ eef->getRobot()->setGlobalPose(eefRobotPoseInit);
+ object->setGlobalPose(objectPoseInit);
+ eef->getRobot()->setConfig(initialConf);
+
return res;
}
-
- auto poses = generatePoses(object->getGlobalPose(), contacts, numPoses);
- if (poses.empty())
+
+ Vector3f GraspEvaluationPoseUncertainty::getMean(const EndEffector::ContactInfoVector& contacts) const
{
- VR_INFO << "No poses for grasp found" << std::endl;
- return res;
- }
- res = evaluatePoses(eef, object, poses, qm, graspPS);
+ Eigen::Vector3f mean = mean.Zero();
+ if (contacts.empty())
+ {
+ VR_ERROR << "No contacts" << endl;
+ return Eigen::Vector3f::Constant(std::nanf(""));
+ }
- // restore setup
- eef->getRobot()->setGlobalPose(eefRobotPoseInit);
- object->setGlobalPose(objectPoseInit);
- eef->getRobot()->setConfig(initialConf);
+ for (size_t i = 0; i < contacts.size(); i++)
+ {
+ if (_config.verbose)
+ {
+ cout << "contact point:" << i << ": \n" << contacts[i].contactPointObstacleGlobal << endl;
+ }
+ mean += contacts[i].contactPointObstacleGlobal;
+ }
+ mean /= contacts.size();
+ return mean;
+ }
- return res;
-}
-Vector3f GraspEvaluationPoseUncertainty::getMean(const EndEffector::ContactInfoVector& contacts) const
-{
- Eigen::Vector3f mean = mean.Zero();
- if (contacts.empty())
+ GraspEvaluationPoseUncertainty::PoseUncertaintyConfig& GraspEvaluationPoseUncertainty::config()
{
- VR_ERROR << "No contacts" << endl;
- return Eigen::Vector3f::Constant(std::nanf(""));
+ return _config;
}
-
- for (size_t i = 0; i < contacts.size(); i++)
+
+ const GraspEvaluationPoseUncertainty::PoseUncertaintyConfig& GraspEvaluationPoseUncertainty::config() const
{
- if (_config.verbose)
- cout << "contact point:" << i << ": \n" << contacts[i].contactPointObstacleGlobal << endl;
- mean += contacts[i].contactPointObstacleGlobal;
+ return _config;
}
- mean /= contacts.size();
- return mean;
-}
-GraspEvaluationPoseUncertainty::PoseUncertaintyConfig&GraspEvaluationPoseUncertainty::config()
-{
- return _config;
-}
+ std::ostream& operator<<(std::ostream& os, const GraspEvaluationPoseUncertainty::PoseEvalResults& rhs)
+ {
+ os << "Robustness analysis" << endl;
+ os << "Num Poses Tested: \t" << rhs.numPosesTested << endl;
+ os << "Num Poses Valid: \t" << rhs.numValidPoses << endl;
-const GraspEvaluationPoseUncertainty::PoseUncertaintyConfig&GraspEvaluationPoseUncertainty::config() const
-{
- return _config;
-}
+ float colPercent = 0.0f;
+ if (rhs.numPosesTested > 0)
+ {
+ colPercent = float(rhs.numColPoses) / float(rhs.numPosesTested) * 100.0f;
+ }
+ os << "Num Poses initially in collision:\t" << rhs.numColPoses << " (" << colPercent << "%)" << endl;
+ os << "Avg Quality (only col freeposes):\t" << rhs.avgQuality << endl;
+ os << "FC rate (only col free poses): \t" << rhs.forceClosureRate * 100.0f << "%" << endl;
+ os << "Avg Quality (all poses): \t" << rhs.avgQualityCol << endl;
+ os << "FC rate (all poses): \t" << rhs.forceClosureRateCol * 100.0f << "%" << endl;
-std::ostream& operator<<(std::ostream& os, const GraspEvaluationPoseUncertainty::PoseEvalResults& rhs)
-{
- os << "Robustness analysis" << endl;
- os << "Num Poses Tested: \t" << rhs.numPosesTested << endl;
- os << "Num Poses Valid: \t" << rhs.numValidPoses << endl;
-
- float colPercent = 0.0f;
- if (rhs.numPosesTested > 0)
- colPercent = float(rhs.numColPoses) / float(rhs.numPosesTested) * 100.0f;
-
- os << "Num Poses initially in collision:\t" << rhs.numColPoses << " (" << colPercent << "%)" << endl;
- os << "Avg Quality (only col freeposes):\t" << rhs.avgQuality << endl;
- os << "FC rate (only col free poses): \t" << rhs.forceClosureRate * 100.0f << "%" << endl;
- os << "Avg Quality (all poses): \t" << rhs.avgQualityCol << endl;
- os << "FC rate (all poses): \t" << rhs.forceClosureRateCol * 100.0f << "%" << endl;
-
- return os;
-}
+ return os;
+ }
}
diff --git a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h
index e1078aaa7909c849843c2feaea33e487474b3c2b..6ae74d6867317c9b32ca3fcc15de0e52c9b07172 100644
--- a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h
+++ b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h
@@ -34,145 +34,148 @@
namespace GraspStudio
{
-/**
- *
- * This class implements the paper:
- * Jonathan Weisz and Peter K. Allen,
- * "Pose Error Robust Grasping from Contact Wrench Space Metrics",
- * 2012 IEEE International Conference on Robotics and Automation.
- */
-class GRASPSTUDIO_IMPORT_EXPORT GraspEvaluationPoseUncertainty : public boost::enable_shared_from_this<GraspEvaluationPoseUncertainty>
-{
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- struct PoseUncertaintyConfig
+ /**
+ *
+ * This class implements the paper:
+ * Jonathan Weisz and Peter K. Allen,
+ * "Pose Error Robust Grasping from Contact Wrench Space Metrics",
+ * 2012 IEEE International Conference on Robotics and Automation.
+ */
+ class GRASPSTUDIO_IMPORT_EXPORT GraspEvaluationPoseUncertainty : public boost::enable_shared_from_this<GraspEvaluationPoseUncertainty>
{
- PoseUncertaintyConfig()
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ struct PoseUncertaintyConfig
{
- init();
- }
-
- void init(float maxPosDelta = 10.0f, float maxOriDeltaDeg = 5.0f, bool normalDistribution = true,
- float stepFactorPos = 0.5f, float stepFactorOri = 0.5f)
- {
- useNormalDistribution = normalDistribution;
- posDeltaMM = maxPosDelta;
- oriDeltaDeg = maxOriDeltaDeg;
- for (int i = 0; i < 6; i++)
- {
- enableDimension[i] = true;
- }
- for (int i = 0; i < 3; i++)
- {
- dimExtends[i] = maxPosDelta; // mm
- stepSize[i] = maxPosDelta * stepFactorPos; // 10 mm => 5 mm steps
-
- float maxOriDeltaRad = maxOriDeltaDeg * static_cast<float>(M_PI / 180.0);
- dimExtends[i + 3] = maxOriDeltaRad;
- stepSize[i + 3] = maxOriDeltaRad * stepFactorOri; // 5 deg => degree
- }
- }
-
- bool enableDimension[6];
- float dimExtends[6];
- float stepSize[6];
- bool useNormalDistribution; // normal or uniform distribution
- bool verbose = false;
-
- float posDeltaMM;
- float oriDeltaDeg;
- };
-
- struct PoseEvalResult
- {
- bool forceClosure;
- float quality;
- bool initialCollision; // ignored due to initial collision
- };
+ PoseUncertaintyConfig()
+ {
+ init();
+ }
+
+ void init(float maxPosDelta = 10.0f, float maxOriDeltaDeg = 5.0f, bool normalDistribution = true,
+ float stepFactorPos = 0.5f, float stepFactorOri = 0.5f)
+ {
+ useNormalDistribution = normalDistribution;
+ posDeltaMM = maxPosDelta;
+ oriDeltaDeg = maxOriDeltaDeg;
+ for (int i = 0; i < 6; i++)
+ {
+ enableDimension[i] = true;
+ }
+ for (int i = 0; i < 3; i++)
+ {
+ dimExtends[i] = maxPosDelta; // mm
+ stepSize[i] = maxPosDelta * stepFactorPos; // 10 mm => 5 mm steps
+
+ float maxOriDeltaRad = maxOriDeltaDeg * static_cast<float>(M_PI / 180.0);
+ dimExtends[i + 3] = maxOriDeltaRad;
+ stepSize[i + 3] = maxOriDeltaRad * stepFactorOri; // 5 deg => degree
+ }
+ }
+
+ bool enableDimension[6];
+ float dimExtends[6];
+ float stepSize[6];
+ bool useNormalDistribution; // normal or uniform distribution
+ bool verbose = false;
+
+ float posDeltaMM;
+ float oriDeltaDeg;
+ };
+
+ struct PoseEvalResult
+ {
+ bool forceClosure;
+ float quality;
+ bool initialCollision; // ignored due to initial collision
+ };
- struct PoseEvalResults
- {
- int numPosesTested = 0.0;
- int numValidPoses = 0.0;
- int numColPoses = 0.0; // poses with initial collision
- int numForceClosurePoses = 0.0; // poses that have force closure
- float forceClosureRate = 0.0; // without collision poses
- float avgQuality = 0.0; // without collision poses
- float forceClosureRateCol = 0.0; // with collision poses
- float avgQualityCol = 0.0; // with collision poses
-
- void print() { VR_INFO << *this << std::endl; }
- friend std::ostream& operator<<(std::ostream& os, const PoseEvalResults& rhs);
- };
+ struct PoseEvalResults
+ {
+ int numPosesTested = 0.0;
+ int numValidPoses = 0.0;
+ int numColPoses = 0.0; // poses with initial collision
+ int numForceClosurePoses = 0.0; // poses that have force closure
+ float forceClosureRate = 0.0; // without collision poses
+ float avgQuality = 0.0; // without collision poses
+ float forceClosureRateCol = 0.0; // with collision poses
+ float avgQualityCol = 0.0; // with collision poses
-
- /// Construct with the given configuration.
- GraspEvaluationPoseUncertainty(const PoseUncertaintyConfig& config);
-
- /// Destructor.
- virtual ~GraspEvaluationPoseUncertainty();
-
-
- // Config
-
- PoseUncertaintyConfig& config();
- const PoseUncertaintyConfig& config() const;
-
- // Pose generation
-
- /**
- * Computes the full set of poses according to configuration.
- * \param objectGP The pose of the object.
- * \param graspCenterGP This could be the pose of the object or the center of the contact points (as proposed in the paper)
- */
- std::vector<Eigen::Matrix4f> generatePoses(
- const Eigen::Matrix4f &objectGP, const Eigen::Matrix4f &graspCenterGP);
-
- /// Uses the mean of the contact points as grasp center point.
- std::vector<Eigen::Matrix4f> generatePoses(
- const Eigen::Matrix4f &objectGP,
- const VirtualRobot::EndEffector::ContactInfoVector &contacts);
-
- /**
- * Computes a set of poses by randomly sampling within the extends of the configuration.
- * \param objectGP The pose of the object.
- * \param graspCenterGP This could be the pose of the object or the center of the contact points (as proposed in the paper)
- * \param numPoses Number of poses to generate
- */
- std::vector<Eigen::Matrix4f> generatePoses(
- const Eigen::Matrix4f &objectGP, const Eigen::Matrix4f &graspCenterGP, int numPoses);
-
- /// Uses the mean of the contact points as grasp center point.
- std::vector<Eigen::Matrix4f> generatePoses(
- const Eigen::Matrix4f &objectGP,
- const VirtualRobot::EndEffector::ContactInfoVector &contacts, int numPoses);
-
-
- // Pose evaluation
-
- PoseEvalResult evaluatePose(
- VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object, const Eigen::Matrix4f &objectPose,
+ void print()
+ {
+ VR_INFO << *this << std::endl;
+ }
+ friend std::ostream& operator<<(std::ostream& os, const PoseEvalResults& rhs);
+ };
+
+
+ /// Construct with the given configuration.
+ GraspEvaluationPoseUncertainty(const PoseUncertaintyConfig& config);
+
+ /// Destructor.
+ virtual ~GraspEvaluationPoseUncertainty();
+
+
+ // Config
+
+ PoseUncertaintyConfig& config();
+ const PoseUncertaintyConfig& config() const;
+
+ // Pose generation
+
+ /**
+ * Computes the full set of poses according to configuration.
+ * \param objectGP The pose of the object.
+ * \param graspCenterGP This could be the pose of the object or the center of the contact points (as proposed in the paper)
+ */
+ std::vector<Eigen::Matrix4f> generatePoses(
+ const Eigen::Matrix4f& objectGP, const Eigen::Matrix4f& graspCenterGP);
+
+ /// Uses the mean of the contact points as grasp center point.
+ std::vector<Eigen::Matrix4f> generatePoses(
+ const Eigen::Matrix4f& objectGP,
+ const VirtualRobot::EndEffector::ContactInfoVector& contacts);
+
+ /**
+ * Computes a set of poses by randomly sampling within the extends of the configuration.
+ * \param objectGP The pose of the object.
+ * \param graspCenterGP This could be the pose of the object or the center of the contact points (as proposed in the paper)
+ * \param numPoses Number of poses to generate
+ */
+ std::vector<Eigen::Matrix4f> generatePoses(
+ const Eigen::Matrix4f& objectGP, const Eigen::Matrix4f& graspCenterGP, int numPoses);
+
+ /// Uses the mean of the contact points as grasp center point.
+ std::vector<Eigen::Matrix4f> generatePoses(
+ const Eigen::Matrix4f& objectGP,
+ const VirtualRobot::EndEffector::ContactInfoVector& contacts, int numPoses);
+
+
+ // Pose evaluation
+
+ PoseEvalResult evaluatePose(
+ VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object, const Eigen::Matrix4f& objectPose,
GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape = {});
-
- PoseEvalResults evaluatePoses(
- VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object, const std::vector<Eigen::Matrix4f> &objectPoses,
+
+ PoseEvalResults evaluatePoses(
+ VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object, const std::vector<Eigen::Matrix4f>& objectPoses,
GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape = {});
- PoseEvalResults evaluateGrasp(
+ PoseEvalResults evaluateGrasp(
VirtualRobot::GraspPtr grasp, VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object,
GraspQualityMeasurePtr qm, int numPoses);
-
-protected:
- Eigen::Vector3f getMean(const VirtualRobot::EndEffector::ContactInfoVector &contacts) const;
-
- PoseUncertaintyConfig _config;
-
-};
+ protected:
+
+ Eigen::Vector3f getMean(const VirtualRobot::EndEffector::ContactInfoVector& contacts) const;
+
+ PoseUncertaintyConfig _config;
+
+ };
-typedef boost::shared_ptr<GraspEvaluationPoseUncertainty> GraspEvaluationPoseUncertaintyPtr;
+ typedef boost::shared_ptr<GraspEvaluationPoseUncertainty> GraspEvaluationPoseUncertaintyPtr;
}
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp b/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp
index 6abbfeea8179b0f8dba15b860e73097838c73ba7..28e982d7fdbd2e6591bdc77e2af17f024a6fd144 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp
@@ -30,7 +30,7 @@ namespace GraspStudio
}
GraspQualityMeasure::~GraspQualityMeasure()
- = default;
+ = default;
bool GraspQualityMeasure::sampleObjectPoints(int nMaxFaces)
{
@@ -99,7 +99,7 @@ namespace GraspStudio
return p;
}
- for (auto & sampledObjectPoint : sampledObjectPoints)
+ for (auto& sampledObjectPoint : sampledObjectPoints)
{
p.p += sampledObjectPoint.p;
p.n += sampledObjectPoint.n;
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasure.h b/GraspPlanning/GraspQuality/GraspQualityMeasure.h
index 26e88fd0eb3682064f4eb01ba926cb4eee60b956..ba8a9379f79ce666fae603b5d057f2787f490e7d 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasure.h
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasure.h
@@ -66,13 +66,13 @@ namespace GraspStudio
bool isValid() override;
virtual ContactConeGeneratorPtr getConeGenerator();
-
+
protected:
// Methods
bool sampleObjectPoints(int nMaxFaces = 400);
-
+
// Friction cone relevant parameters
float unitForce;
float frictionCoeff;
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.h b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.h
index a26b121c23a2fcbf494391fc0d74a494a0c611ce..e000eb38084bc180c9ca8ffd3a3f1f4d9c4fe11f 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.h
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.h
@@ -49,7 +49,10 @@ namespace GraspStudio
//! This method is used to compute a reference value that describes a perfect grasp
- bool calculateObjectProperties() override { return true; }
+ bool calculateObjectProperties() override
+ {
+ return true;
+ }
/*!
Returns f_max_gws
diff --git a/GraspPlanning/MeshConverter.cpp b/GraspPlanning/MeshConverter.cpp
index a558616b1142c9746490d226648ca510b0ef4e65..913b504c6d80cb477c92e516d2e24acef1da1dfc 100644
--- a/GraspPlanning/MeshConverter.cpp
+++ b/GraspPlanning/MeshConverter.cpp
@@ -138,9 +138,9 @@ namespace GraspStudio
for (size_t i = 0; i < tm->faces.size(); i++)
{
- Eigen::Vector3f &v1 = tm->vertices[tm->faces[i].id1];
- Eigen::Vector3f &v2 = tm->vertices[tm->faces[i].id2];
- Eigen::Vector3f &v3 = tm->vertices[tm->faces[i].id3];
+ Eigen::Vector3f& v1 = tm->vertices[tm->faces[i].id1];
+ Eigen::Vector3f& v2 = tm->vertices[tm->faces[i].id2];
+ Eigen::Vector3f& v3 = tm->vertices[tm->faces[i].id3];
unsigned int id1, id2, id3;
check = hasVertex(triMesh2->vertices, v1);
@@ -218,9 +218,9 @@ namespace GraspStudio
VR_ASSERT(faceIdx >= 0 && faceIdx < (int)tm->faces.size());
float d12, d13, d23;
- Eigen::Vector3f &v1 = tm->vertices[tm->faces[faceIdx].id1];
- Eigen::Vector3f &v2 = tm->vertices[tm->faces[faceIdx].id2];
- Eigen::Vector3f &v3 = tm->vertices[tm->faces[faceIdx].id3];
+ Eigen::Vector3f& v1 = tm->vertices[tm->faces[faceIdx].id1];
+ Eigen::Vector3f& v2 = tm->vertices[tm->faces[faceIdx].id2];
+ Eigen::Vector3f& v3 = tm->vertices[tm->faces[faceIdx].id3];
Eigen::Vector3f v4;
unsigned int id4;
size_t checkFaceIdx;
diff --git a/GraspPlanning/Visualization/ConvexHullVisualization.cpp b/GraspPlanning/Visualization/ConvexHullVisualization.cpp
index 9247ea7b836b737669f0afba6f6c64ff99315067..1e3739303fd1b5fcb78f6377ffbaee43828fbb6a 100644
--- a/GraspPlanning/Visualization/ConvexHullVisualization.cpp
+++ b/GraspPlanning/Visualization/ConvexHullVisualization.cpp
@@ -20,6 +20,6 @@ namespace GraspStudio
ConvexHullVisualization::~ConvexHullVisualization()
- = default;
+ = default;
} // namespace GraspStudio
diff --git a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
index b37d488380de618356f1cc4a8f9cd3c3d29cad6e..b15776f219530217a47b57e871b11fe60c977379 100644
--- a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
+++ b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
@@ -244,12 +244,16 @@ void GraspQualityWindow::evalRobustness()
openEEF();
closeEEF();
if (contacts.size() == 0)
+ {
return;
- evalPoses = eval->generatePoses(object->getGlobalPose(), contacts, numSamples );
+ }
+ evalPoses = eval->generatePoses(object->getGlobalPose(), contacts, numSamples);
- if (evalPoses.size()==0)
+ if (evalPoses.size() == 0)
+ {
return;
+ }
/*
int r = rand() % evalPoses.size();
@@ -261,7 +265,9 @@ void GraspQualityWindow::evalRobustness()
*/
GraspStudio::GraspEvaluationPoseUncertainty::PoseEvalResults re = eval->evaluatePoses(eef, object, evalPoses, qualityMeasure);
if (eef && grasp)
+ {
VR_INFO << "#### Robustness for eef " << eef->getName() << ", grasp " << grasp->getName() << endl;
+ }
re.print();
}
@@ -293,16 +299,22 @@ void GraspQualityWindow::evalRobustnessAll()
VR_INFO << contacts.size() << endl;
if (contacts.size() == 0)
+ {
continue;
- std::vector<Eigen::Matrix4f> evalPoses = eval->generatePoses(object->getGlobalPose(), contacts, numSamples );
+ }
+ std::vector<Eigen::Matrix4f> evalPoses = eval->generatePoses(object->getGlobalPose(), contacts, numSamples);
VR_INFO << evalPoses.size() << endl;
- if (evalPoses.size()==0)
+ if (evalPoses.size() == 0)
+ {
continue;
+ }
GraspStudio::GraspEvaluationPoseUncertainty::PoseEvalResults re = eval->evaluatePoses(eef, object, evalPoses, qualityMeasure);
if (eef && grasp)
+ {
VR_INFO << "#### Robustness for eef " << eef->getName() << ", grasp " << grasp->getName() << endl;
+ }
re.print();
grasp->setQuality(re.avgQuality);
@@ -325,9 +337,11 @@ void GraspQualityWindow::loadObject()
if (!objectFile.empty())
{
- try {
+ try
+ {
object = ObjectIO::loadManipulationObject(objectFile);
- } catch (...)
+ }
+ catch (...)
{
VR_ERROR << "Could not load file " << objectFile << endl;
}
@@ -459,7 +473,7 @@ void GraspQualityWindow::setGraspComboBox()
grasps = object->getGraspSet(eef);
- if (!grasps || grasps->getSize()==0)
+ if (!grasps || grasps->getSize() == 0)
{
VR_INFO << "No grasps found for eef " << eef->getName() << endl;
return;
@@ -488,7 +502,7 @@ void GraspQualityWindow::setEEFComboBox()
robot->getEndEffectors(eefs);
- for (auto & eef : eefs)
+ for (auto& eef : eefs)
{
QString nameEEF(eef->getName().c_str());
UI.comboBoxEEF->addItem(nameEEF);
@@ -750,55 +764,55 @@ void GraspQualityWindow::showGWS()
{
return;
}
-/*
- if (UI.checkBoxGWS1->isChecked())
- {
- GraspStudio::CoinConvexHullVisualizationPtr v(new GraspStudio::CoinConvexHullVisualization(ch, true));
- SoSeparator* s = v->getCoinVisualization();
-
- if (s)
+ /*
+ if (UI.checkBoxGWS1->isChecked())
{
- gws1Sep->addChild(mt);
- gws1Sep->addChild(s);
+ GraspStudio::CoinConvexHullVisualizationPtr v(new GraspStudio::CoinConvexHullVisualization(ch, true));
+ SoSeparator* s = v->getCoinVisualization();
+
+ if (s)
+ {
+ gws1Sep->addChild(mt);
+ gws1Sep->addChild(s);
+ }
}
- }
-
- if (UI.checkBoxGWS2->isChecked())
- {
- GraspStudio::CoinConvexHullVisualizationPtr v(new GraspStudio::CoinConvexHullVisualization(chOWS, false));
- SoSeparator* s = v->getCoinVisualization();
- if (s)
+ if (UI.checkBoxGWS2->isChecked())
{
- gws2Sep->addChild(mt);
- gws2Sep->addChild(s);
- }
+ GraspStudio::CoinConvexHullVisualizationPtr v(new GraspStudio::CoinConvexHullVisualization(chOWS, false));
+ SoSeparator* s = v->getCoinVisualization();
- }
+ if (s)
+ {
+ gws2Sep->addChild(mt);
+ gws2Sep->addChild(s);
+ }
- if (UI.checkBoxOWS1->isChecked())
- {
- GraspStudio::CoinConvexHullVisualizationPtr v(new GraspStudio::CoinConvexHullVisualization(chOWS, true));
- SoSeparator* s = v->getCoinVisualization();
+ }
- if (s)
+ if (UI.checkBoxOWS1->isChecked())
{
- ows1Sep->addChild(mt);
- ows1Sep->addChild(s);
+ GraspStudio::CoinConvexHullVisualizationPtr v(new GraspStudio::CoinConvexHullVisualization(chOWS, true));
+ SoSeparator* s = v->getCoinVisualization();
+
+ if (s)
+ {
+ ows1Sep->addChild(mt);
+ ows1Sep->addChild(s);
+ }
}
- }
- if (UI.checkBoxOWS2->isChecked())
- {
- GraspStudio::CoinConvexHullVisualizationPtr v(new GraspStudio::CoinConvexHullVisualization(chOWS, false));
- SoSeparator* s = v->getCoinVisualization();
-
- if (s)
+ if (UI.checkBoxOWS2->isChecked())
{
- ows2Sep->addChild(mt);
- ows2Sep->addChild(s);
- }
- }*/
+ GraspStudio::CoinConvexHullVisualizationPtr v(new GraspStudio::CoinConvexHullVisualization(chOWS, false));
+ SoSeparator* s = v->getCoinVisualization();
+
+ if (s)
+ {
+ ows2Sep->addChild(mt);
+ ows2Sep->addChild(s);
+ }
+ }*/
}
void GraspQualityWindow::showOWS()