Adding helper methods.

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@205 042f3d55-54a8-47e9-b7fb-15903f145c44

VirtualRobot/ReachabilitySpace.cpp

@@ -527,6 +527,39 @@ int ReachabilitySpace::getMaxEntry()
 	return data->getMaxEntry();
 }
 
+int ReachabilitySpace::getMaxEntry( const Eigen::Vector3f &position_global )
+{
+	Eigen::Matrix4f gp;
+	gp.setIdentity();
+	gp.block(0,3,3,1) = position_global;
+
+	// get voxels
+	unsigned int v[6];
+
+	if (!getVoxelFromPose(gp,v))
+		return 0;
+	return getMaxEntry(v[0],v[1],v[2]);
+}
+
+int ReachabilitySpace::getMaxEntry( int x0, int x1, int x2 )
+{
+	int maxValue = 0;
+	for(int a = 0; a < getNumVoxels(3); a+=1)
+	{
+		for(int b = 0; b < getNumVoxels(4); b+=1)
+		{
+			for(int c = 0; c < getNumVoxels(5); c+=1)
+			{
+				int value = data->get(x0,x1,x2,a, b, c);
+				if (value>=maxValue)
+					maxValue = value;
+			}
+		}
+	}
+	return maxValue;
+
+}
+
 float ReachabilitySpace::getVoxelSize(int dim)
 {
 	if(dim < 0 || dim > 6)
@@ -628,6 +661,18 @@ bool ReachabilitySpace::getVoxelFromPose(float x[6], unsigned int v[6])
 	return true;
 }
 
+bool ReachabilitySpace::getVoxelFromPose( const Eigen::Matrix4f &globalPose, unsigned int v[6] )
+{
+	float x[6];
+
+	Eigen::Matrix4f p = globalPose;
+	if (baseNode)
+		p = baseNode->toLocalCoordinateSystem(p);
+
+	MathTools::eigen4f2rpy(p,x);
+	return getVoxelFromPose(x,v);
+}
+
 void ReachabilitySpace::addRandomTCPPoses( unsigned int loops, bool checkForSelfCollisions )
 {
 	THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "Reachability space not initialized");
@@ -838,16 +883,10 @@ unsigned char ReachabilitySpace::getEntry( const Eigen::Matrix4f &globalPose )
 		VR_ERROR << "NULL DATA" << endl;
 		return 0;
 	}
-	Eigen::Matrix4f p = globalPose;
-	if (baseNode)
-		p = baseNode->toLocalCoordinateSystem(p);
-
-	float x[6];
-	MathTools::eigen4f2rpy(p,x);
 
 	// get voxels
 	unsigned int v[6];
-	if (getVoxelFromPose(x,v))
+	if (getVoxelFromPose(globalPose,v))
 	{
 		return data->get(v);
 	} else
@@ -985,5 +1024,23 @@ unsigned char ReachabilitySpace::getVoxelEntry(unsigned int a, unsigned int b, u
 	return data->get(a,b,c,d,e,f);
 }
 
+int ReachabilitySpace::getMaxSummedAngleReachablity()
+{
+	int maxValue = 0;
+	for(int a = 0; a < getNumVoxels(0); a+=1)
+	{
+		for(int b = 0; b < getNumVoxels(1); b+=1)
+		{
+			for(int c = 0; c < getNumVoxels(2); c+=1)
+			{
+				int value = sumAngleReachabilities(a, b, c);
+				if (value>=maxValue)
+					maxValue = value;
+			}
+		}
+	}
+	return maxValue;
+}
+
 
 } // namespace VirtualRobot

VirtualRobot/ReachabilitySpace.h

@@ -272,6 +272,33 @@ public:
 
 	unsigned char getVoxelEntry(unsigned int a, unsigned int b, unsigned int c, unsigned int d, unsigned int e, unsigned int f);
 
+	/*!
+		Sums all angle (x3,x4,x5) entries for the given position.
+	*/
+	int sumAngleReachabilities(int x0, int x1, int x2);
+
+	/*!
+		Searches all angle entries (x3,x4,x5) for maximum entry.
+		(x0,x1,x2) is the voxel position.
+	*/
+	int getMaxEntry(int x0, int x1, int x2);
+
+	/*!
+		Returns the maximum reachability entry that can be achieved by an arbitrary orientation at the given position.
+	*/
+	int getMaxEntry( const Eigen::Vector3f &position_global );
+
+	/*!
+		Get the corresponding voxel. 
+		If false is returned the pose is outside the covered workspace.
+	*/
+	bool getVoxelFromPose(const Eigen::Matrix4f &globalPose, unsigned int v[6]);
+
+	/*!
+		Returns the maximum that can be achieved by calling sumAngleReachabilities() 
+	*/
+	int getMaxSummedAngleReachablity();
+
 
 protected:
 	//! Specific methods to read/write strings from/to reachability files
@@ -291,9 +318,9 @@ protected:
 	//! Refetch the base joint's transformation, in case this joint has moved
 	void updateBaseTransformation();
 
-	int sumAngleReachabilities(int x0, int x1, int x2);
-
 	bool getVoxelFromPose(float x[6], unsigned int v[6]);
+
+
 	RobotPtr robot;
 	RobotNodePtr baseNode;
 	RobotNodePtr tcpNode;