Added generate poses (full) with contact point.

Added step params.
Fixed distrubition range and use ClampedNormalDistribution

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;
 }
 
-
 }

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;
 	
 };