Grasp Evaluation PoseUncertainty

GraspPlanning/CMakeLists.txt

@@ -51,6 +51,7 @@ ApproachMovementSurfaceNormal.cpp
 MeshConverter.cpp
 GraspPlanner/GraspPlanner.cpp
 GraspPlanner/GenericGraspPlanner.cpp
+GraspQuality/GraspEvaluationPoseUncertainty.cpp
 GraspQuality/GraspQualityMeasure.cpp
 GraspQuality/GraspQualityMeasureWrenchSpace.cpp
 Visualization/ConvexHullVisualization.cpp
@@ -65,6 +66,7 @@ ApproachMovementSurfaceNormal.h
 MeshConverter.h
 GraspPlanner/GraspPlanner.h
 GraspPlanner/GenericGraspPlanner.h
+GraspQuality/GraspEvaluationPoseUncertainty.h
 GraspQuality/GraspQualityMeasure.h
 GraspQuality/GraspQualityMeasureWrenchSpace.h
 Visualization/ConvexHullVisualization.h

GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp

+#include <Eigen/Geometry>
+#include "GraspEvaluationPoseUncertainty.h"
+
+#include <algorithm>
+#include <float.h>
+#include <cstdlib>
+
+using namespace Eigen;
+using namespace VirtualRobot;
+
+namespace GraspStudio {
+
+GraspEvaluationPoseUncertainty::GraspEvaluationPoseUncertainty(const PoseUncertaintyConfig& config)
+{
+	this->config = config;
+}
+
+GraspEvaluationPoseUncertainty::~GraspEvaluationPoseUncertainty()
+{
+
+}
+
+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<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 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 dist[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];
+            dist[i] = end[i] - start[i];
+        }
+        else
+        {
+            start[i] = initPoseRPY[i];
+            end[i] = initPoseRPY[i];
+            dist[i] = 0.0f;
+        }
+    }
+
+    Eigen::Matrix4f m;
+
+    for (int j=0;j<numPoses; j++)
+    {
+        for (int i = 0; i < 6; i++)
+        {
+            float r = float(rand()) / float(RAND_MAX);
+            tmpPose[i] = start[i] + r*dist[i];
+        }
+        MathTools::posrpy2eigen4f(tmpPose, m);
+        m = m * trafoGraspCenterToObjectCenter;
+        result.push_back(m);
+    }
+    return result;
+}
+
+}

GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h

+/**
+* 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 _GraspStudio_GraspEvaluationPoseUncertainty_h_
+#define _GraspStudio_GraspEvaluationPoseUncertainty_h_
+
+#include <VirtualRobot/VirtualRobotCommon.h>
+
+#include <string>
+#include <vector>
+
+#include <boost/shared_ptr.hpp>
+
+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 GraspEvaluationPoseUncertainty : public boost::enable_shared_from_this<GraspEvaluationPoseUncertainty>
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+	struct PoseUncertaintyConfig
+    {
+        PoseUncertaintyConfig()
+        {
+            init();
+        }
+
+        void init(float maxPosDelta = 10.0f, float maxOriDelta = 5.0f)
+		{
+			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
+			}
+		}
+
+		bool enableDimension[6];
+		float dimExtends[6];
+		float stepSize[6];
+	};
+
+
+	/*!
+		@brief Initialize the pose quality calculation
+
+	*/
+	GraspEvaluationPoseUncertainty(const PoseUncertaintyConfig& config);
+	
+	virtual ~GraspEvaluationPoseUncertainty();
+
+	/*!
+        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);
+	
+    /*!
+        Computes a set of poses by randomly sampling within the exetnds 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);
+
+protected:
+
+	PoseUncertaintyConfig config;
+	
+};
+
+typedef boost::shared_ptr<GraspEvaluationPoseUncertainty> GraspEvaluationPoseUncertaintyPtr;
+
+}
+
+#endif

GraspPlanning/examples/GraspQuality/GraspQuality.ui

@@ -80,7 +80,7 @@
         <rect>
          <x>10</x>
          <y>510</y>
-         <width>81</width>
+         <width>121</width>
          <height>16</height>
         </rect>
        </property>
@@ -173,7 +173,7 @@
          <rect>
           <x>11</x>
           <y>20</y>
-          <width>81</width>
+          <width>121</width>
           <height>16</height>
          </rect>
         </property>