diff --git a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
index d3f1b7bb07ba3749fba87c380901ad85353f8f9c..5d960bc17a643c1a5f419b0e67b99eb53efad2e0 100644
--- a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
+++ b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
@@ -2,6 +2,7 @@
#include "GraspEvaluationPoseUncertainty.h"
#include <VirtualRobot/Random.h>
#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/math/ClampedNormalDistribution.hpp>
#include <algorithm>
#include <random>
@@ -15,9 +16,10 @@ namespace GraspStudio {
GraspEvaluationPoseUncertainty::GraspEvaluationPoseUncertainty(const PoseUncertaintyConfig& config)
{
- this->config = config;
+ this->_config = config;
}
+
GraspEvaluationPoseUncertainty::~GraspEvaluationPoseUncertainty()
= default;
@@ -26,7 +28,7 @@ std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(
const Eigen::Matrix4f &objectGP, const Eigen::Matrix4f &graspCenterGP)
{
std::vector<Eigen::Matrix4f> result;
- Eigen::Matrix4f trafoGraspCenterToObjectCenter = objectGP * math::Helpers::InvertedPose(graspCenterGP);
+ Eigen::Matrix4f trafoGraspCenterToObjectCenter = objectGP * graspCenterGP.inverse();
Eigen::Matrix4f initPose = graspCenterGP;
Eigen::Vector3f rpy;
MathTools::eigen4f2rpy(initPose, rpy);
@@ -46,11 +48,11 @@ std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(
for (int i = 0; i < 6; i++)
{
- if (config.enableDimension[i])
+ if (_config.enableDimension[i])
{
- start[i] = initPoseRPY[i] - config.dimExtends[i];
- end[i] = initPoseRPY[i] + config.dimExtends[i];
- step[i] = config.stepSize[i];
+ start[i] = initPoseRPY[i] - _config.dimExtends[i];
+ end[i] = initPoseRPY[i] + _config.dimExtends[i];
+ step[i] = _config.stepSize[i];
}
else
{
@@ -89,7 +91,22 @@ std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(
}
}
}
- return result;
+ return result;
+}
+
+std::vector<Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(const Matrix4f& objectGP, const EndEffector::ContactInfoVector& contacts)
+{
+ Eigen::Vector3f centerPos = getMean(contacts);
+ if (centerPos.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));
}
@@ -117,12 +134,12 @@ std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(
for (int i = 0; i < 6; i++)
{
start[i] = initPoseRPY[i];
- if (config.enableDimension[i])
+ if (_config.enableDimension[i])
{
- if (i<3)
- dist[i] = config.posDeltaMM;
+ if (i < 3)
+ dist[i] = _config.posDeltaMM;
else
- dist[i] = config.oriDeltaDeg/180.0f*float(M_PI);
+ dist[i] = _config.oriDeltaDeg * static_cast<float>(M_PI / 180.0);
}
else
{
@@ -131,16 +148,17 @@ std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(
}
Eigen::Matrix4f m;
- std::normal_distribution<float> normalDistribution(0.0, 0.5);
- std::uniform_real_distribution<float> uniformDistribution(0.0, 1);
+
+ 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];
+ float r = _config.useNormalDistribution ? normalDistribution(VirtualRobot::PRNG64Bit())
+ : uniformDistribution(VirtualRobot::PRNG64Bit());
+ tmpPose[i] = start[i] + r * dist[i];
}
MathTools::posrpy2eigen4f(tmpPose, m);
m = m * trafoGraspCenterToObjectCenter;
@@ -154,22 +172,14 @@ std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(
const VirtualRobot::EndEffector::ContactInfoVector &contacts,
int numPoses)
{
- Eigen::Vector3f centerPose = centerPose.Zero();
- if (contacts.empty())
+ Eigen::Vector3f centerPose = getMean(contacts);
+ if (centerPose.hasNaN())
{
VR_ERROR << "No contacts" << endl;
return std::vector<Eigen::Matrix4f>();
}
- for (size_t i = 0; i < contacts.size(); i++)
- {
- if (config.verbose)
- cout << "contact point:" << i << ": \n" << contacts[i].contactPointObstacleGlobal << endl;
- centerPose += contacts[i].contactPointObstacleGlobal;
- }
- centerPose /= contacts.size();
-
- if (config.verbose)
+ if (_config.verbose)
cout << "using contact center pose:\n" << centerPose << endl;
return generatePoses(objectGP, math::Helpers::Pose(centerPose), numPoses);
@@ -367,6 +377,37 @@ GraspEvaluationPoseUncertainty::PoseEvalResults GraspEvaluationPoseUncertainty::
return res;
}
+Vector3f GraspEvaluationPoseUncertainty::getMean(const EndEffector::ContactInfoVector& contacts) const
+{
+ Eigen::Vector3f mean = mean.Zero();
+ if (contacts.empty())
+ {
+ VR_ERROR << "No contacts" << endl;
+ return Eigen::Vector3f::Constant(std::nanf(""));
+ }
+
+ 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;
+}
+
+
+GraspEvaluationPoseUncertainty::PoseUncertaintyConfig&GraspEvaluationPoseUncertainty::config()
+{
+ return _config;
+}
+
+const GraspEvaluationPoseUncertainty::PoseUncertaintyConfig&GraspEvaluationPoseUncertainty::config() const
+{
+ return _config;
+}
+
+
std::ostream& operator<<(std::ostream& os, const GraspEvaluationPoseUncertainty::PoseEvalResults& rhs)
{
os << "Robustness analysis" << endl;
@@ -386,5 +427,4 @@ std::ostream& operator<<(std::ostream& os, const GraspEvaluationPoseUncertainty:
return os;
}
-
}
diff --git a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h
index 722df01ef82e720e5b9f91986430524a50a56919..e1078aaa7909c849843c2feaea33e487474b3c2b 100644
--- a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h
+++ b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h
@@ -53,21 +53,24 @@ public:
init();
}
- void init(float maxPosDelta = 10.0f, float maxOriDelta = 5.0f, bool normalDistribution = true)
+ 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 = maxOriDelta;
+ oriDeltaDeg = maxOriDeltaDeg;
for (int i = 0; i < 6; i++)
{
enableDimension[i] = true;
}
for (int i = 0; i < 3; i++)
{
- dimExtends[i] = maxPosDelta; //mm
- dimExtends[i + 3] = maxOriDelta / 180.0f * float(M_PI); // degrees
- stepSize[i] = maxPosDelta * 0.5f;
- stepSize[i + 3] = maxOriDelta * 0.5f / 180.0f * float(M_PI); // 3 degree
+ 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
}
}
@@ -111,6 +114,11 @@ public:
virtual ~GraspEvaluationPoseUncertainty();
+ // Config
+
+ PoseUncertaintyConfig& config();
+ const PoseUncertaintyConfig& config() const;
+
// Pose generation
/**
@@ -120,6 +128,11 @@ public:
*/
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.
@@ -153,7 +166,9 @@ public:
protected:
- PoseUncertaintyConfig config;
+ Eigen::Vector3f getMean(const VirtualRobot::EndEffector::ContactInfoVector &contacts) const;
+
+ PoseUncertaintyConfig _config;
};