From 0921d3e1bd68157af26a227ee0fb67b0ce5e3f81 Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Fri, 9 Mar 2012 15:06:23 +0000
Subject: [PATCH] Changing Reachability implementation. Adding dynamic tree
data structure for voxelized workspace representations.
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@206 042f3d55-54a8-47e9-b7fb-15903f145c44
---
VirtualRobot/CMakeLists.txt | 11 +-
VirtualRobot/IK/IKSolver.cpp | 8 +-
VirtualRobot/IK/IKSolver.h | 6 +-
VirtualRobot/IK/PoseQualityManipulability.cpp | 2 +-
VirtualRobot/VirtualRobot.h | 10 +-
.../CoinVisualizationFactory.cpp | 12 +-
.../CoinVisualizationFactory.h | 12 +-
VirtualRobot/Workspace/Reachability.cpp | 98 +++++
VirtualRobot/Workspace/Reachability.h | 88 +++++
VirtualRobot/Workspace/VoxelTree6D.hpp | 122 ++++++
VirtualRobot/Workspace/WorkspaceData.cpp | 127 ++++++
VirtualRobot/Workspace/WorkspaceData.h | 153 ++++++++
.../WorkspaceRepresentation.cpp} | 368 ++++++------------
.../WorkspaceRepresentation.h} | 235 ++++-------
VirtualRobot/Workspace/tests/CMakeLists.txt | 1 +
.../Workspace/tests/TransformationTest.cpp | 55 +++
.../Workspace/tests/VoxelTreeTest.cpp | 58 +++
.../GraspEditor/GraspEditorWindow.cpp | 2 +-
.../examples/RobotViewer/showRobotWindow.cpp | 2 +-
.../examples/SceneViewer/showSceneWindow.cpp | 2 +-
.../reachability/reachabilityScene.cpp | 4 +-
.../reachability/reachabilityWindow.cpp | 6 +-
.../reachability/reachabilityWindow.h | 4 +-
.../examples/stability/stabilityWindow.cpp | 2 +-
24 files changed, 934 insertions(+), 454 deletions(-)
create mode 100644 VirtualRobot/Workspace/Reachability.cpp
create mode 100644 VirtualRobot/Workspace/Reachability.h
create mode 100644 VirtualRobot/Workspace/VoxelTree6D.hpp
create mode 100644 VirtualRobot/Workspace/WorkspaceData.cpp
create mode 100644 VirtualRobot/Workspace/WorkspaceData.h
rename VirtualRobot/{ReachabilitySpace.cpp => Workspace/WorkspaceRepresentation.cpp} (68%)
rename VirtualRobot/{ReachabilitySpace.h => Workspace/WorkspaceRepresentation.h} (51%)
create mode 100644 VirtualRobot/Workspace/tests/CMakeLists.txt
create mode 100644 VirtualRobot/Workspace/tests/TransformationTest.cpp
create mode 100644 VirtualRobot/Workspace/tests/VoxelTreeTest.cpp
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 90e31214b..721ef21dd 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -38,6 +38,9 @@ IK/GenericIKSolver.cpp
IK/CoMIK.cpp
IK/PoseQualityMeasurement.cpp
IK/PoseQualityManipulability.cpp
+Workspace/WorkspaceData.cpp
+Workspace/WorkspaceRepresentation.cpp
+Workspace/Reachability.cpp
MathTools.cpp
Robot.cpp
RobotConfig.cpp
@@ -49,7 +52,6 @@ SceneObjectSet.cpp
Scene.cpp
Obstacle.cpp
VirtualRobotException.cpp
-ReachabilitySpace.cpp
Grasp.cpp
GraspSet.cpp
ManipulationObject.cpp
@@ -92,6 +94,11 @@ IK/GenericIKSolver.h
IK/CoMIK.h
IK/PoseQualityMeasurement.h
IK/PoseQualityManipulability.h
+Workspace/WorkspaceData.h
+Workspace/WorkspaceRepresentation.h
+Workspace/Reachability.h
+Workspace/VoxelTree6D.hpp
+Workspace/VoxelTree6DElement.hpp
AbstractFactoryMethod.h
VirtualRobot.h
MathTools.h
@@ -107,7 +114,6 @@ Obstacle.h
VirtualRobotException.h
VirtualRobotImportExport.h
VirtualRobotTest.h
-ReachabilitySpace.h
Grasp.h
GraspSet.h
ManipulationObject.h
@@ -117,6 +123,7 @@ CompressionRLE.h
)
ADD_SUBDIRECTORY(Visualization/tests)
+ADD_SUBDIRECTORY(Workspace/tests)
if (SIMOX_VISUALIZATION AND SIMOX_USE_COIN_VISUALIZATION)
diff --git a/VirtualRobot/IK/IKSolver.cpp b/VirtualRobot/IK/IKSolver.cpp
index 8bc1c1ec3..927bf2fd2 100644
--- a/VirtualRobot/IK/IKSolver.cpp
+++ b/VirtualRobot/IK/IKSolver.cpp
@@ -9,7 +9,7 @@
#include "../ManipulationObject.h"
#include "../Grasp.h"
#include "../GraspSet.h"
-#include "../ReachabilitySpace.h"
+#include "../Workspace/Reachability.h"
#include "../EndEffector/EndEffector.h"
#include "../RobotConfig.h"
#include "../CollisionDetection/CollisionChecker.h"
@@ -207,18 +207,18 @@ void IKSolver::setMaximumError( float maxErrorPositionMM /*= 1.0f*/, float maxEr
this->maxErrorOrientationRad = maxErrorOrientationRad;
}
-void IKSolver::setReachabilityCheck( ReachabilitySpacePtr reachabilitySpace )
+void IKSolver::setReachabilityCheck( ReachabilityPtr reachabilitySpace )
{
this->reachabilitySpace = reachabilitySpace;
if (reachabilitySpace)
{
if (reachabilitySpace->getTCP() != tcp)
{
- VR_ERROR << "Reachability Space has different tcp RobotNode (" << reachabilitySpace->getTCP()->getName() << ") than IK solver (" << tcp->getName() << ") ?! " << endl << "Reachability results may not be valid!" << endl;
+ VR_ERROR << "Reachability representation has different tcp RobotNode (" << reachabilitySpace->getTCP()->getName() << ") than IK solver (" << tcp->getName() << ") ?! " << endl << "Reachability results may not be valid!" << endl;
}
if (reachabilitySpace->getNodeSet() != rns)
{
- VR_ERROR << "Reachability Space is defined for a different RobotNodeSet (" << reachabilitySpace->getNodeSet()->getName() << ") than IK solver uses (" << rns->getName() << ") ?! " << endl << "Reachability results may not be valid!" << endl;
+ VR_ERROR << "Reachability representation is defined for a different RobotNodeSet (" << reachabilitySpace->getNodeSet()->getName() << ") than IK solver uses (" << rns->getName() << ") ?! " << endl << "Reachability results may not be valid!" << endl;
}
}
}
diff --git a/VirtualRobot/IK/IKSolver.h b/VirtualRobot/IK/IKSolver.h
index 49cd56598..f838d1666 100644
--- a/VirtualRobot/IK/IKSolver.h
+++ b/VirtualRobot/IK/IKSolver.h
@@ -93,9 +93,9 @@ public:
/*!
When set, the reachability data is used to quickly decide if a given pose or grasp is reachable or not.
- This option can be enabled by setting the reachability space and it can be disabled by setting an empty ReachabilitySpacePtr.
+ This option can be enabled by setting the reachability space and it can be disabled by setting an empty ReachabilityPtr.
*/
- virtual void setReachabilityCheck(ReachabilitySpacePtr reachabilitySpace);
+ virtual void setReachabilityCheck(ReachabilityPtr reachabilitySpace);
/*!
This method solves the IK up to the specified max error. On success, the joints of the the corresponding RobotNodeSet are set to the IK solution.
@@ -162,7 +162,7 @@ protected:
float maxErrorPositionMM;
float maxErrorOrientationRad;
- ReachabilitySpacePtr reachabilitySpace;
+ ReachabilityPtr reachabilitySpace;
};
typedef boost::shared_ptr<IKSolver> IKSolverPtr;
diff --git a/VirtualRobot/IK/PoseQualityManipulability.cpp b/VirtualRobot/IK/PoseQualityManipulability.cpp
index 0e621a696..f628ff954 100644
--- a/VirtualRobot/IK/PoseQualityManipulability.cpp
+++ b/VirtualRobot/IK/PoseQualityManipulability.cpp
@@ -15,7 +15,7 @@ PoseQualityManipulability::PoseQualityManipulability(VirtualRobot::RobotNodeSetP
{
jacobian.reset(new VirtualRobot::DifferentialIK(rns,rns->getTCP()));
jacobian->convertModelScalingtoM(convertMMtoM);
- penalizeRotationFactor = 0.05f; // translation<->rotation factors
+ penalizeRotationFactor = 0.15f; // translation<->rotation factors
}
diff --git a/VirtualRobot/VirtualRobot.h b/VirtualRobot/VirtualRobot.h
index f0eab1039..124deb4c7 100644
--- a/VirtualRobot/VirtualRobot.h
+++ b/VirtualRobot/VirtualRobot.h
@@ -143,14 +143,15 @@ namespace VirtualRobot
class Visualization;
class VisualizationNode;
class VisualizationFactory;
- class ReachabilitySpace;
- class ReachabilitySpaceData;
class Scene;
class RobotConfig;
class Grasp;
class GraspSet;
class ManipulationObject;
class CDManager;
+ class Reachability;
+ class WorkspaceRepresentation;
+ class WorkspaceData;
typedef boost::shared_ptr<RobotNode> RobotNodePtr;
typedef boost::shared_ptr<RobotNodeSet> RobotNodeSetPtr;
@@ -170,8 +171,9 @@ namespace VirtualRobot
typedef boost::shared_ptr<Visualization> VisualizationPtr;
typedef boost::shared_ptr<VisualizationNode> VisualizationNodePtr;
typedef boost::shared_ptr<VisualizationFactory> VisualizationFactoryPtr;
- typedef boost::shared_ptr<ReachabilitySpace> ReachabilitySpacePtr;
- typedef boost::shared_ptr<ReachabilitySpaceData> ReachabilitySpaceDataPtr;
+ typedef boost::shared_ptr<WorkspaceData> WorkspaceDataPtr;
+ typedef boost::shared_ptr<WorkspaceRepresentation> WorkspaceRepresentationPtr;
+ typedef boost::shared_ptr<Reachability> ReachabilityPtr;
typedef boost::shared_ptr<Scene> ScenePtr;
typedef boost::shared_ptr<RobotConfig> RobotConfigPtr;
typedef boost::shared_ptr<Grasp> GraspPtr;
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index c25e1a0c6..cb06b4ad4 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -15,7 +15,7 @@
#include "../../GraspSet.h"
#include "../../SceneObject.h"
#include "../TriMeshModel.h"
-#include "../../ReachabilitySpace.h"
+#include "../../Workspace/Reachability.h"
#include <Inventor/SoDB.h>
#include <Inventor/nodes/SoNode.h>
#include <Inventor/nodes/SoUnits.h>
@@ -1141,7 +1141,7 @@ VirtualRobot::VisualizationNodePtr CoinVisualizationFactory::createArrow( const
}
-SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilitySpacePtr reachSpace, int a, int b, int c, /*const Eigen::Vector3f &positionGlobal,*/ int nrBestEntries, SoSeparator* arrow, const VirtualRobot::ColorMap &cm, bool transformToGlobalPose, unsigned char minValue)
+SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilityPtr reachSpace, int a, int b, int c, /*const Eigen::Vector3f &positionGlobal,*/ int nrBestEntries, SoSeparator* arrow, const VirtualRobot::ColorMap &cm, bool transformToGlobalPose, unsigned char minValue)
{
SoSeparator *res = new SoSeparator;
res->ref();
@@ -1266,7 +1266,7 @@ SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilitySpacePtr reac
return res;
}
-SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilitySpacePtr reachSpace, const Eigen::Vector3f &fixedEEFOrientationGlobalRPY, VirtualRobot::ColorMap::type cmType, bool transformToGlobalPose, const Eigen::Vector3f &axis, unsigned char minValue, float arrowSize)
+SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilityPtr reachSpace, const Eigen::Vector3f &fixedEEFOrientationGlobalRPY, VirtualRobot::ColorMap::type cmType, bool transformToGlobalPose, const Eigen::Vector3f &axis, unsigned char minValue, float arrowSize)
{
SoSeparator *res = new SoSeparator;
res->ref();
@@ -1359,7 +1359,7 @@ SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilitySpacePtr reac
return res;
}
/*
-SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilitySpacePtr reachSpace, VirtualRobot::ColorMap::type cmType, const Eigen::Vector3f &axis, bool transformToGlobalPose, unsigned char minValue)
+SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilityPtr reachSpace, VirtualRobot::ColorMap::type cmType, const Eigen::Vector3f &axis, bool transformToGlobalPose, unsigned char minValue)
{
SoSeparator *res = new SoSeparator;
res->ref();
@@ -1404,7 +1404,7 @@ SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilitySpacePtr reac
}
*/
-SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilitySpacePtr reachSpace, VirtualRobot::ColorMap::type cmType, const Eigen::Vector3f &axis, bool transformToGlobalPose, unsigned char minValue, float arrowSize)
+SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilityPtr reachSpace, VirtualRobot::ColorMap::type cmType, const Eigen::Vector3f &axis, bool transformToGlobalPose, unsigned char minValue, float arrowSize)
{
SoSeparator *res = new SoSeparator;
res->ref();
@@ -1450,7 +1450,7 @@ SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilitySpacePtr reac
}
-SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilitySpacePtr reachSpace, const VirtualRobot::ColorMap::type cmType, bool transformToGlobalPose)
+SoNode* CoinVisualizationFactory::getCoinVisualization(ReachabilityPtr reachSpace, const VirtualRobot::ColorMap::type cmType, bool transformToGlobalPose)
{
SoSeparator *res = new SoSeparator;
res->ref();
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index 3cc8dfd9b..efb78687f 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -130,17 +130,17 @@ public:
static SoNode *getCoinVisualization(TriMeshModelPtr model, bool shownormals, VisualizationFactory::Color color = VisualizationFactory::Color::Gray());
/*!
- Create a visualization of the reachability space.
+ Create a visualization of the reachability data.
*/
- static SoNode* getCoinVisualization(ReachabilitySpacePtr reachSpace, const VirtualRobot::ColorMap::type cmType, bool transformToGlobalPose = true);
+ static SoNode* getCoinVisualization(ReachabilityPtr reachSpace, const VirtualRobot::ColorMap::type cmType, bool transformToGlobalPose = true);
/*!
- Creates a visualization of the reachability space. For each 3D point, the orientation with maximum entry is determined and visualized as an arrow. The direction of this arrow is aligned to the param axis.
+ Creates a visualization of the reachability data. For each 3D point, the orientation with maximum entry is determined and visualized as an arrow. The direction of this arrow is aligned to the param axis.
*/
- static SoNode* getCoinVisualization(ReachabilitySpacePtr reachSpace, VirtualRobot::ColorMap::type cmType, const Eigen::Vector3f &axis, bool transformToGlobalPose = true, unsigned char minValue = 0, float arrowSize = 0);
+ static SoNode* getCoinVisualization(ReachabilityPtr reachSpace, VirtualRobot::ColorMap::type cmType, const Eigen::Vector3f &axis, bool transformToGlobalPose = true, unsigned char minValue = 0, float arrowSize = 0);
//! Helper method: Create reach space visualization of a fixed orientation
- static SoNode* getCoinVisualization(ReachabilitySpacePtr reachSpace, const Eigen::Vector3f &fixedEEFOrientationGlobalRPY, VirtualRobot::ColorMap::type cmType = VirtualRobot::ColorMap::eHot, bool transformToGlobalPose = true, const Eigen::Vector3f &axis = Eigen::Vector3f(0,0,1.0f), unsigned char minValue = 0, float arrowSize = 0);
+ static SoNode* getCoinVisualization(ReachabilityPtr reachSpace, const Eigen::Vector3f &fixedEEFOrientationGlobalRPY, VirtualRobot::ColorMap::type cmType = VirtualRobot::ColorMap::eHot, bool transformToGlobalPose = true, const Eigen::Vector3f &axis = Eigen::Vector3f(0,0,1.0f), unsigned char minValue = 0, float arrowSize = 0);
//! helper method: create nrBestEntries arrows in direction of maximum orientation for voxelPosition (a,b,c)
- static SoNode* getCoinVisualization(ReachabilitySpacePtr reachSpace, int a, int b, int c, int nrBestEntries, SoSeparator* arrow, const VirtualRobot::ColorMap &cm, bool transformToGlobalPose, unsigned char minValue);
+ static SoNode* getCoinVisualization(ReachabilityPtr reachSpace, int a, int b, int c, int nrBestEntries, SoSeparator* arrow, const VirtualRobot::ColorMap &cm, bool transformToGlobalPose, unsigned char minValue);
static SoMatrixTransform* getMatrixTransform(Eigen::Matrix4f &m);
static SoNode* createCoinLine(const Eigen::Matrix4f &from, const Eigen::Matrix4f &to, float width, float colorR, float colorG, float colorB);
diff --git a/VirtualRobot/Workspace/Reachability.cpp b/VirtualRobot/Workspace/Reachability.cpp
new file mode 100644
index 000000000..3b2fb668e
--- /dev/null
+++ b/VirtualRobot/Workspace/Reachability.cpp
@@ -0,0 +1,98 @@
+#include "Reachability.h"
+#include "../VirtualRobotException.h"
+#include "../Robot.h"
+#include "../ManipulationObject.h"
+#include "../Grasp.h"
+#include "../GraspSet.h"
+#include <fstream>
+#include <cmath>
+#include <float.h>
+#include <limits.h>
+
+namespace VirtualRobot
+{
+
+
+
+Reachability::Reachability(RobotPtr robot) : WorkspaceRepresentation (robot)
+{
+ type = "Reachbaility";
+}
+
+void Reachability::addCurrentTCPPose()
+{
+ THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "No reachability data loaded");
+
+ Eigen::Matrix4f p = tcpNode->getGlobalPose();
+ if (baseNode)
+ p = baseNode->toLocalCoordinateSystem(p);
+
+ float x[6];
+ MathTools::eigen4f2rpy(p,x);
+
+ // check for achieved values
+ for (int i=0;i<6;i++)
+ {
+ if (x[i] < achievedMinValues[i])
+ achievedMinValues[i] = x[i];
+ if (x[i] > achievedMaxValues[i])
+ achievedMaxValues[i] = x[i];
+ }
+
+ // get voxels
+ unsigned int v[6];
+ if (getVoxelFromPose(x,v))
+ {
+ data->increaseDatum(v);
+ }
+
+ buildUpLoops++;
+}
+
+void Reachability::addRandomTCPPoses( unsigned int loops, bool checkForSelfCollisions )
+{
+ THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "Reachability data not initialized");
+
+ std::vector<float> c;
+ nodeSet->getJointValues(c);
+ bool visuSate = robot->getUpdateVisualizationStatus();
+ robot->setUpdateVisualization(false);
+
+ updateBaseTransformation();
+
+ for (unsigned int i=0;i<loops;i++)
+ {
+ if (setRobotNodesToRandomConfig(checkForSelfCollisions))
+ addCurrentTCPPose();
+ else
+ VR_WARNING << "Could not find collision-free configuration...";
+ }
+ robot->setUpdateVisualization(visuSate);
+ nodeSet->setJointValues(c);
+}
+bool Reachability::isReachable( const Eigen::Matrix4f &globalPose )
+{
+ return isCovered(globalPose);
+}
+
+VirtualRobot::GraspSetPtr Reachability::getReachableGrasps( GraspSetPtr grasps, ManipulationObjectPtr object )
+{
+ THROW_VR_EXCEPTION_IF(!object,"no object");
+ THROW_VR_EXCEPTION_IF(!grasps,"no grasps");
+
+ GraspSetPtr result(new GraspSet(grasps->getName(),grasps->getRobotType(),grasps->getEndEffector()));
+ for (unsigned int i=0; i<grasps->getSize(); i++)
+ {
+ Eigen::Matrix4f m = grasps->getGrasp(i)->getTcpPoseGlobal(object->getGlobalPose());
+ if (isReachable(m))
+ result->addGrasp(grasps->getGrasp(i));
+ }
+ return result;
+}
+
+Eigen::Matrix4f Reachability::sampleReachablePose()
+{
+ return sampleCoveredPose();
+}
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/Workspace/Reachability.h b/VirtualRobot/Workspace/Reachability.h
new file mode 100644
index 000000000..884af4b25
--- /dev/null
+++ b/VirtualRobot/Workspace/Reachability.h
@@ -0,0 +1,88 @@
+/**
+* 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 VirtualRobot
+* @author Peter Kaiser, Nikolaus Vahrenkamp
+* @copyright 2011 Peter Kaiser, Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#ifndef _VirtualRobot_Reachability_h_
+#define _VirtualRobot_Reachability_h_
+
+#include "../VirtualRobotImportExport.h"
+#include "WorkspaceRepresentation.h"
+
+
+namespace VirtualRobot
+{
+
+/*!
+ This class represents an approximation of the reachability distribution of a kinematic chain (e.g. an arm).
+ Consists of voxels covering the 6D space for position (XYZ) and orientation (RPY).
+ Each voxels holds a counter with the number of successful IK solver calls,
+ representing the approximated probability that an IK solver call can be successfully answered.
+ The discretized reachability data can be written to and loaded from binary files.
+
+ The reachability is linked to a base coordinate system which is defined by a robot joint.
+ This base system is used to align the data when the robot is moving.
+ I.E. think of an arm of a humanoid where the reachability is linked to the shoulder.
+ When the torso moves, the reachability also changes it's position according to the position of the shoulder.
+*/
+class VIRTUAL_ROBOT_IMPORT_EXPORT Reachability : public WorkspaceRepresentation, public boost::enable_shared_from_this<Reachability>
+{
+public:
+ friend class CoinVisualizationFactory;
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ Reachability(RobotPtr robot);
+
+ /*!
+ Returns true, if the corresponding reachability entry is nun zero.
+ */
+ bool isReachable(const Eigen::Matrix4f &globalPose);
+
+ /*!
+ Returns all reachable grasps that can be applied at the current position of object.
+ */
+ GraspSetPtr getReachableGrasps(GraspSetPtr grasps, ManipulationObjectPtr object);
+
+
+ /*!
+ Append current TCP pose of the ocrresponding robot to reachability data.
+ This means that the entry of the corresponding WorkspaceData voxel is increased by 1.
+ */
+ void addCurrentTCPPose();
+
+ /*!
+ Append a number of random TCP poses to Reachability Data
+ \param loops Number of poses that should be appended
+ \param checkForSelfCollisions Build a collision-free configuration. If true, random configs are generated until one is collision-free.
+ */
+ void addRandomTCPPoses(unsigned int loops, bool checkForSelfCollisions = true);
+
+ //! returns a random pose that is covered by the workspace data
+ Eigen::Matrix4f sampleReachablePose();
+
+protected:
+
+};
+
+
+} // namespace VirtualRobot
+
+#endif // _Reachability_h_
diff --git a/VirtualRobot/Workspace/VoxelTree6D.hpp b/VirtualRobot/Workspace/VoxelTree6D.hpp
new file mode 100644
index 000000000..eeb651a7f
--- /dev/null
+++ b/VirtualRobot/Workspace/VoxelTree6D.hpp
@@ -0,0 +1,122 @@
+/**
+* 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 VirtualRobot
+* @author Nikolaus Vahrenkamp
+* @copyright 2012 Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#ifndef _VirtualRobot_VoxelTree6D_h_
+#define _VirtualRobot_VoxelTree6D_h_
+
+#include "../VirtualRobotImportExport.h"
+#include "../VirtualRobotException.h"
+#include "VoxelTree6DElement.hpp"
+
+#include <string>
+#include <vector>
+
+namespace VirtualRobot
+{
+
+template <typename T>
+class VoxelTree6D
+{
+public:
+
+ /*!
+ */
+ VoxelTree6D(float minExtend[6], float maxExtend[6], float discretizationTransl, float discretizationRot)
+ {
+ memcpy (&(this->minExtend[0]),&(minExtend[0]),sizeof(float)*6);
+ memcpy (&(this->maxExtend[0]),&(maxExtend[0]),sizeof(float)*6);
+ if (discretizationTransl<=0.0f || discretizationRot<=0.0f)
+ {
+ THROW_VR_EXCEPTION ("INVALID parameters");
+ }
+ float size[6];
+ for (int i=0;i<6;i++)
+ {
+ size[i] = maxExtend[i] - minExtend[i];
+ }
+
+ float maxSize = 0;
+ for (int i=0;i<3;i++)
+ {
+ if (size[i] > maxSize)
+ maxSize = size[i];
+ }
+ int steps = (int)(maxSize / discretizationTransl + 0.5f);
+ float maxSize2 = 0;
+ for (int i=3;i<6;i++)
+ {
+ if (size[i] > maxSize2)
+ maxSize2 = size[i];
+ }
+ int steps2 = (int)(maxSize2 / discretizationRot + 0.5f);
+ if (steps2 > steps)
+ steps = steps2;
+ maxLevels = steps;
+ THROW_VR_EXCEPTION_IF (steps<=0,"Invalid parameters...");
+ root = new VoxelTree6DElement<T>(minExtend,size,0,maxLevels);
+
+ }
+
+ virtual ~VoxelTree6D()
+ {
+ delete root;
+ }
+
+ /*!
+ Store entry to this voxel grid.
+ Creates a leaf if necessary. Existing entries are silently overwritten.
+ A copy of e is created.
+ */
+ bool setEntry(float pos[6], const T &e)
+ {
+ return root->setEntry(pos,e);
+ }
+
+ /*!
+ Returns entry at pos. If pos is outside the workspace representation or no data stored at pos, NULL is returned.
+ */
+ T* getEntry(float pos[6])
+ {
+ return root->getEntry(pos);
+ }
+
+
+ /*!
+ Check if the corresponding leaf is present in data structure.
+ \Return False if p is outside the extends or if no data is stored for pos
+ */
+ //bool isCovered(float pos[6]);
+
+protected:
+ float minExtend[6];
+ float maxExtend[6];
+ int maxLevels;
+
+ VoxelTree6DElement<T> *root;
+
+};
+
+
+
+} // namespace
+
+#endif // _VirtualRobot_VoxelTree6D_h_
diff --git a/VirtualRobot/Workspace/WorkspaceData.cpp b/VirtualRobot/Workspace/WorkspaceData.cpp
new file mode 100644
index 000000000..9ca94331c
--- /dev/null
+++ b/VirtualRobot/Workspace/WorkspaceData.cpp
@@ -0,0 +1,127 @@
+#include "WorkspaceData.h"
+
+#include <fstream>
+#include <cmath>
+#include <float.h>
+#include <limits.h>
+
+namespace VirtualRobot
+{
+
+WorkspaceData::WorkspaceData(unsigned int size1, unsigned int size2, unsigned int size3,
+ unsigned int size4, unsigned int size5, unsigned int size6, bool adjustOnOverflow)
+{
+ unsigned long long size = (unsigned long long)size1 * (unsigned long long)size2 * (unsigned long long)size3 * (unsigned long long)size4 * (unsigned long long)size5 * (unsigned long long)size6;
+ if (size>UINT_MAX)
+ {
+ VR_ERROR << "Could not assign " << size << " bytes of memory (>UINT_MAX). Reduce size of reachability space..." << endl;
+ }
+ try
+ {
+ data = new unsigned char[(unsigned int)size];
+ } catch (const std::exception &e)
+ {
+ VR_ERROR << "Exception: " << e.what() << endl << "Could not assign " << size << " bytes of memory. Reduce size of reachability space..." << endl;
+ throw;
+ } catch (...)
+ {
+ VR_ERROR << "Could not assign " << size << " bytes of memory. Reduce size of reachability space..." << endl;
+ throw;
+ }
+
+
+ sizes[0] = size1;
+ sizes[1] = size2;
+ sizes[2] = size3;
+ sizes[3] = size4;
+ sizes[4] = size5;
+ sizes[5] = size6;
+ sizeX0 = sizes[1]*sizes[2]*sizes[3]*sizes[4]*sizes[5];
+ sizeX1 = sizes[2]*sizes[3]*sizes[4]*sizes[5];
+ sizeX2 = sizes[3]*sizes[4]*sizes[5];
+ sizeX3 = sizes[4]*sizes[5];
+ sizeX4 = sizes[5];
+ maxEntry = 0;
+ voxelFilledCount = 0;
+ this->adjustOnOverflow = adjustOnOverflow;
+
+ memset(data,0,getSize()*sizeof(unsigned char));
+}
+
+WorkspaceData::~WorkspaceData()
+{
+ delete[] data;
+}
+
+unsigned int WorkspaceData::getSize() const
+{
+ return sizes[0]*sizes[1]*sizes[2]*sizes[3]*sizes[4]*sizes[5];
+}
+
+void WorkspaceData::setData(unsigned char *data)
+{
+ memcpy(this->data, data, getSize()*sizeof(unsigned char));
+}
+
+const unsigned char *WorkspaceData::getData() const
+{
+ return data;
+}
+
+unsigned char WorkspaceData::getMaxEntry() const
+{
+ return maxEntry;
+}
+
+unsigned int WorkspaceData::getVoxelFilledCount() const
+{
+ return voxelFilledCount;
+}
+
+void WorkspaceData::binarize()
+{
+
+ for (unsigned int a=0;a<sizes[0];a++)
+ {
+ for (unsigned int b=0;b<sizes[1];b++)
+ {
+ for (unsigned int c=0;c<sizes[2];c++)
+ {
+ for (unsigned int d=0;d<sizes[3];d++)
+ {
+ for (unsigned int e=0;e<sizes[4];e++)
+ {
+ for (unsigned int f=0;f<sizes[5];f++)
+ {
+ unsigned int pos = getPos(a,b,c,d,e,f);
+ if (data[pos]>1)
+ data[pos] = 1;
+ }
+ }
+ }
+ }
+ }
+ }
+ maxEntry = 1;
+}
+
+void WorkspaceData::bisectData()
+{
+ for (unsigned int x0=0;x0<sizes[0];x0++)
+ for (unsigned int x1=0;x1<sizes[1];x1++)
+ for (unsigned int x2=0;x2<sizes[2];x2++)
+ for (unsigned int x3=0;x3<sizes[3];x3++)
+ for (unsigned int x4=0;x4<sizes[4];x4++)
+ for (unsigned int x5=0;x5<sizes[5];x5++)
+ {
+ unsigned char c = data[x0 * sizeX0 + x1 * sizeX1 + x2 * sizeX2 + x3 * sizeX3 + x4 * sizeX4 + x5];
+ if (c>1)
+ data[x0 * sizeX0 + x1 * sizeX1 + x2 * sizeX2 + x3 * sizeX3 + x4 * sizeX4 + x5] = c/2;
+ }
+ if (maxEntry>1)
+ maxEntry = maxEntry / 2;
+}
+
+
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/Workspace/WorkspaceData.h b/VirtualRobot/Workspace/WorkspaceData.h
new file mode 100644
index 000000000..0eb4fff86
--- /dev/null
+++ b/VirtualRobot/Workspace/WorkspaceData.h
@@ -0,0 +1,153 @@
+/**
+* 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 VirtualRobot
+* @author Peter Kaiser, Nikolaus Vahrenkamp
+* @copyright 2011 Peter Kaiser, Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#ifndef _VirtualRobot_WorkspaceData_h_
+#define _VirtualRobot_WorkspaceData_h_
+
+#include "../VirtualRobotImportExport.h"
+
+#include <boost/enable_shared_from_this.hpp>
+#include <boost/type_traits/is_base_of.hpp>
+#include <boost/mpl/assert.hpp>
+
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+
+namespace VirtualRobot
+{
+/*!
+ Stores a 6-dimensional array for the vertex data of a workspace representation.
+ Internally unsigned char data types are used (0...255)
+*/
+class VIRTUAL_ROBOT_IMPORT_EXPORT WorkspaceData : public boost::enable_shared_from_this<WorkspaceData>
+{
+public:
+ /*!
+ Constructor, fills the data with 0
+ */
+ WorkspaceData(unsigned int size1, unsigned int size2, unsigned int size3,
+ unsigned int size4, unsigned int size5, unsigned int size6, bool adjustOnOverflow);
+ ~WorkspaceData();
+
+ //! Return the amount of data in bytes
+ unsigned int getSize() const;
+
+ inline unsigned int getPos(unsigned int x0, unsigned int x1, unsigned int x2,
+ unsigned int x3, unsigned int x4, unsigned int x5) const
+ {
+ return x0 * sizeX0 + x1 * sizeX1 + x2 * sizeX2 + x3 * sizeX3 + x4 * sizeX4 + x5;
+ }
+
+ inline unsigned int getPos( unsigned int x[6] ) const
+ {
+ return x[0] * sizeX0 + x[1] * sizeX1 + x[2] * sizeX2 + x[3] * sizeX3 + x[4] * sizeX4 + x[5];
+ }
+
+ inline void setDatum(unsigned int x0, unsigned int x1, unsigned int x2,
+ unsigned int x3, unsigned int x4, unsigned int x5, unsigned char value)
+ {
+ unsigned int pos = getPos(x0,x1,x2,x3,x4,x5);
+ if (data[pos]==0)
+ voxelFilledCount++;
+ data[pos] = value;
+ if (value >= maxEntry)
+ maxEntry = value;
+ }
+
+ inline void setDatum(unsigned int x[6], unsigned char value)
+ {
+ unsigned int pos = getPos(x);
+ if (data[pos]==0)
+ voxelFilledCount++;
+ data[pos] = value;
+ if (value >= maxEntry)
+ maxEntry = value;
+ }
+
+ inline void increaseDatum( unsigned int x0, unsigned int x1, unsigned int x2,
+ unsigned int x3, unsigned int x4, unsigned int x5)
+ {
+ unsigned int pos = getPos(x0,x1,x2,x3,x4,x5);
+ unsigned char e = data[pos];
+ if (e==0)
+ voxelFilledCount++;
+ if (e<UCHAR_MAX)
+ {
+ data[pos]++;
+ if (e >= maxEntry)
+ maxEntry = e+1;
+ } else if (adjustOnOverflow)
+ bisectData();
+ }
+ inline void increaseDatum( unsigned int x[6] )
+ {
+ unsigned int pos = getPos(x);
+ unsigned char e = data[pos];
+ if (e==0)
+ voxelFilledCount++;
+ if (e<UCHAR_MAX)
+ {
+ data[pos]++;
+ if (e >= maxEntry)
+ maxEntry = e+1;
+ } else if (adjustOnOverflow)
+ bisectData();
+ }
+
+ void setData(unsigned char *data);
+ const unsigned char *getData() const;
+
+ //! Simulates a multi-dimensional array access
+ inline unsigned char get(unsigned int x0, unsigned int x1, unsigned int x2,
+ unsigned int x3, unsigned int x4, unsigned int x5) const
+ {
+ return data[x0 * sizeX0 + x1 * sizeX1 + x2 * sizeX2 + x3 * sizeX3 + x4 * sizeX4 + x5];
+ }
+
+ //! Simulates a multi-dimensional array access
+ inline unsigned char get( unsigned int x[6] ) const
+ {
+ return data[x[0] * sizeX0 + x[1] * sizeX1 + x[2] * sizeX2 + x[3] * sizeX3 + x[4] * sizeX4 + x[5]];
+ }
+
+ unsigned char getMaxEntry() const;
+ unsigned int getVoxelFilledCount() const;
+ void binarize();
+
+ void bisectData();
+
+ unsigned int sizes[6];
+ unsigned int sizeX0,sizeX1,sizeX2,sizeX3,sizeX4;
+ unsigned char *data;
+ unsigned char maxEntry;
+ unsigned int voxelFilledCount;
+ bool adjustOnOverflow;
+};
+
+
+
+} // namespace VirtualRobot
+
+#endif // _WorkspaceData_h_
diff --git a/VirtualRobot/ReachabilitySpace.cpp b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
similarity index 68%
rename from VirtualRobot/ReachabilitySpace.cpp
rename to VirtualRobot/Workspace/WorkspaceRepresentation.cpp
index c2c92df51..741a053f0 100644
--- a/VirtualRobot/ReachabilitySpace.cpp
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
@@ -1,16 +1,16 @@
-#include "ReachabilitySpace.h"
-#include "VirtualRobotException.h"
-#include "Robot.h"
-#include "CompressionRLE.h"
-#include "SceneObjectSet.h"
-#include "Nodes/RobotNode.h"
-#include "Visualization/Visualization.h"
-#include "Visualization/VisualizationFactory.h"
-#include "CollisionDetection/CollisionChecker.h"
-#include "Visualization/ColorMap.h"
-#include "ManipulationObject.h"
-#include "Grasp.h"
-#include "GraspSet.h"
+#include "WorkspaceRepresentation.h"
+#include "../VirtualRobotException.h"
+#include "../Robot.h"
+#include "../CompressionRLE.h"
+#include "../SceneObjectSet.h"
+#include "../Nodes/RobotNode.h"
+#include "../Visualization/Visualization.h"
+#include "../Visualization/VisualizationFactory.h"
+#include "../CollisionDetection/CollisionChecker.h"
+#include "../Visualization/ColorMap.h"
+#include "../ManipulationObject.h"
+#include "../Grasp.h"
+#include "../GraspSet.h"
#include <fstream>
#include <cmath>
#include <float.h>
@@ -19,134 +19,22 @@
namespace VirtualRobot
{
-ReachabilitySpaceData::ReachabilitySpaceData(unsigned int size1, unsigned int size2, unsigned int size3,
- unsigned int size4, unsigned int size5, unsigned int size6, bool adjustOnOverflow)
-{
- unsigned long long size = (unsigned long long)size1 * (unsigned long long)size2 * (unsigned long long)size3 * (unsigned long long)size4 * (unsigned long long)size5 * (unsigned long long)size6;
- if (size>UINT_MAX)
- {
- VR_ERROR << "Could not assign " << size << " bytes of memory (>UINT_MAX). Reduce size of reachability space..." << endl;
- }
- try
- {
- data = new unsigned char[(unsigned int)size];
- } catch (const std::exception &e)
- {
- VR_ERROR << "Exception: " << e.what() << endl << "Could not assign " << size << " bytes of memory. Reduce size of reachability space..." << endl;
- throw;
- } catch (...)
- {
- VR_ERROR << "Could not assign " << size << " bytes of memory. Reduce size of reachability space..." << endl;
- throw;
- }
-
-
- sizes[0] = size1;
- sizes[1] = size2;
- sizes[2] = size3;
- sizes[3] = size4;
- sizes[4] = size5;
- sizes[5] = size6;
- sizeX0 = sizes[1]*sizes[2]*sizes[3]*sizes[4]*sizes[5];
- sizeX1 = sizes[2]*sizes[3]*sizes[4]*sizes[5];
- sizeX2 = sizes[3]*sizes[4]*sizes[5];
- sizeX3 = sizes[4]*sizes[5];
- sizeX4 = sizes[5];
- maxEntry = 0;
- voxelFilledCount = 0;
- this->adjustOnOverflow = adjustOnOverflow;
-
- memset(data,0,getSize()*sizeof(unsigned char));
-}
-
-ReachabilitySpaceData::~ReachabilitySpaceData()
-{
- delete[] data;
-}
-
-int ReachabilitySpaceData::getSize()
-{
- return sizes[0]*sizes[1]*sizes[2]*sizes[3]*sizes[4]*sizes[5];
-}
-
-void ReachabilitySpaceData::setData(unsigned char *data)
-{
- memcpy(this->data, data, getSize()*sizeof(unsigned char));
-}
-
-const unsigned char *ReachabilitySpaceData::getData() const
-{
- return data;
-}
-
-unsigned char ReachabilitySpaceData::getMaxEntry()
-{
- return maxEntry;
-}
-unsigned int ReachabilitySpaceData::getVoxelFilledCount()
-{
- return voxelFilledCount;
-}
-void ReachabilitySpaceData::binarize()
-{
-
- for (unsigned int a=0;a<sizes[0];a++)
- {
- for (unsigned int b=0;b<sizes[1];b++)
- {
- for (unsigned int c=0;c<sizes[2];c++)
- {
- for (unsigned int d=0;d<sizes[3];d++)
- {
- for (unsigned int e=0;e<sizes[4];e++)
- {
- for (unsigned int f=0;f<sizes[5];f++)
- {
- unsigned int pos = getPos(a,b,c,d,e,f);
- if (data[pos]>1)
- data[pos] = 1;
- }
- }
- }
- }
- }
- }
- maxEntry = 1;
-}
-
-void ReachabilitySpaceData::bisectData()
-{
- for (unsigned int x0=0;x0<sizes[0];x0++)
- for (unsigned int x1=0;x1<sizes[1];x1++)
- for (unsigned int x2=0;x2<sizes[2];x2++)
- for (unsigned int x3=0;x3<sizes[3];x3++)
- for (unsigned int x4=0;x4<sizes[4];x4++)
- for (unsigned int x5=0;x5<sizes[5];x5++)
- {
- unsigned char c = data[x0 * sizeX0 + x1 * sizeX1 + x2 * sizeX2 + x3 * sizeX3 + x4 * sizeX4 + x5];
- if (c>1)
- data[x0 * sizeX0 + x1 * sizeX1 + x2 * sizeX2 + x3 * sizeX3 + x4 * sizeX4 + x5] = c/2;
- }
- maxEntry = maxEntry / 2;
-}
-
-
-
-ReachabilitySpace::ReachabilitySpace(RobotPtr robot)
+WorkspaceRepresentation::WorkspaceRepresentation(RobotPtr robot)
{
THROW_VR_EXCEPTION_IF(!robot,"Need a robot ptr here");
this->robot = robot;
+ type = "WorkspaceRepresentation";
reset();
}
-void ReachabilitySpace::updateBaseTransformation()
+void WorkspaceRepresentation::updateBaseTransformation()
{
baseTransformation = baseNode->getGlobalPose().inverse();
}
-int ReachabilitySpace::sumAngleReachabilities(int x0, int x1, int x2)
+int WorkspaceRepresentation::sumAngleReachabilities(int x0, int x1, int x2) const
{
int res = 0;
for(int d = 0; d < numVoxels[3]; d++)
@@ -161,7 +49,7 @@ int ReachabilitySpace::sumAngleReachabilities(int x0, int x1, int x2)
}
-bool ReachabilitySpace::readString(std::string &res, std::ifstream &file)
+bool WorkspaceRepresentation::readString(std::string &res, std::ifstream &file)
{
int length = read<int>(file);
if(length <= 0)
@@ -178,37 +66,37 @@ bool ReachabilitySpace::readString(std::string &res, std::ifstream &file)
return true;
}
-void ReachabilitySpace::writeString(std::ofstream &file, const std::string &value)
+void WorkspaceRepresentation::writeString(std::ofstream &file, const std::string &value)
{
int len = value.length();
file.write((char *)&len, sizeof(int));
file.write(value.c_str(), len);
}
-template<typename T> T ReachabilitySpace::read(std::ifstream &file)
+template<typename T> T WorkspaceRepresentation::read(std::ifstream &file)
{
T t;
file.read((char *)&t, sizeof(T));
return t;
}
-template<typename T> void ReachabilitySpace::readArray(T *res, int num, std::ifstream &file)
+template<typename T> void WorkspaceRepresentation::readArray(T *res, int num, std::ifstream &file)
{
file.read((char *)res, num * sizeof(T));
}
-template<typename T> void ReachabilitySpace::write(std::ofstream &file, T value)
+template<typename T> void WorkspaceRepresentation::write(std::ofstream &file, T value)
{
file.write((char *)&value, sizeof(T));
}
-template<typename T> void ReachabilitySpace::writeArray(std::ofstream &file, const T *value, int num)
+template<typename T> void WorkspaceRepresentation::writeArray(std::ofstream &file, const T *value, int num)
{
file.write((char *)value, num * sizeof(T));
}
-void ReachabilitySpace::uncompressData(const unsigned char *source, int size, unsigned char *dest)
+void WorkspaceRepresentation::uncompressData(const unsigned char *source, int size, unsigned char *dest)
{
unsigned char count;
unsigned char value;
@@ -224,7 +112,7 @@ void ReachabilitySpace::uncompressData(const unsigned char *source, int size, un
}
}
-unsigned char *ReachabilitySpace::compressData(const unsigned char *source, int size, int &compressedSize)
+unsigned char *WorkspaceRepresentation::compressData(const unsigned char *source, int size, int &compressedSize)
{
// on large arrays sometimes an out-of-memory exception is thrown, so in order to reduce the size of the array, we assume we can compress it
// hence, we have to check if the compressed size does not exceed the original size on every pos increase
@@ -235,11 +123,11 @@ unsigned char *ReachabilitySpace::compressData(const unsigned char *source, int
}
catch (std::exception e)
{
- VR_ERROR << "Error:" << e.what() << endl << "Could not assign " << size << " bytes of memory. Reduce size of reachability space..." << endl;
+ VR_ERROR << "Error:" << e.what() << endl << "Could not assign " << size << " bytes of memory. Reduce size of WorkspaceRepresentation data..." << endl;
throw;
} catch (...)
{
- VR_ERROR << "Could not assign " << size << " bytes of memory. Reduce size of reachability space..." << endl;
+ VR_ERROR << "Could not assign " << size << " bytes of memory. Reduce size of WorkspaceRepresentation data..." << endl;
throw;
}
int pos = 0;
@@ -256,7 +144,7 @@ unsigned char *ReachabilitySpace::compressData(const unsigned char *source, int
dest[pos] = 255;
dest[pos+1] = curValue;
pos += 2;
- THROW_VR_EXCEPTION_IF(pos>=size, "Could not perform run-length compression. Data is too clutered!!!");
+ THROW_VR_EXCEPTION_IF(pos>=size, "Could not perform run-length compression. Data is too cluttered!!!");
count = 1;
}
@@ -268,7 +156,7 @@ unsigned char *ReachabilitySpace::compressData(const unsigned char *source, int
dest[pos] = count;
dest[pos+1] = curValue;
pos += 2;
- THROW_VR_EXCEPTION_IF(pos>=size, "Could not perform run-length compression. Data is too clutered!!!");
+ THROW_VR_EXCEPTION_IF(pos>=size, "Could not perform run-length compression. Data is too cluttered!!!");
curValue = source[i];
count = 1;
@@ -279,13 +167,13 @@ unsigned char *ReachabilitySpace::compressData(const unsigned char *source, int
dest[pos] = count;
dest[pos+1] = curValue;
pos += 2;
- THROW_VR_EXCEPTION_IF(pos>=size, "Could not perform run-length compression. Data is too clutered!!!");
+ THROW_VR_EXCEPTION_IF(pos>=size, "Could not perform run-length compression. Data is too cluttered!!!");
}
compressedSize = pos;
return dest;
}
-void ReachabilitySpace::load(const std::string &filename)
+void WorkspaceRepresentation::load(const std::string &filename)
{
std::ifstream file(filename.c_str(), std::ios::in | std::ios::binary);
THROW_VR_EXCEPTION_IF(!file, "File could not be read.");
@@ -293,10 +181,22 @@ void ReachabilitySpace::load(const std::string &filename)
try
{
std::string tmpString;
+
+ std::string tmpStr2 = type;
+ tmpStr2 += " Binary File";
// Check file type
readString(tmpString, file);
- THROW_VR_EXCEPTION_IF(tmpString != "ReachabilitySpace Binary File", "Wrong file format.");
+ bool fileTypeOK = false;
+ if (tmpString == "WorkspaceRepresentation Binary File" ||
+ tmpString == "Reachability Binary File" ||
+ tmpString == "Manipulability Binary File" ||
+ tmpString == "ReachabilitySpace Binary File" ||
+ tmpString == tmpStr2)
+ fileTypeOK = true;
+
+
+ THROW_VR_EXCEPTION_IF(!fileTypeOK, "Wrong file format.");
// Check version
int version[2];
@@ -382,7 +282,7 @@ void ReachabilitySpace::load(const std::string &filename)
THROW_VR_EXCEPTION_IF(tmpString != "DATA_START", "Bad file format, expecting DATA_START.");
int size = numVoxels[0]*numVoxels[1]*numVoxels[2]*numVoxels[3]*numVoxels[4]*numVoxels[5];
- data.reset(new ReachabilitySpaceData(numVoxels[0], numVoxels[1], numVoxels[2], numVoxels[3], numVoxels[4], numVoxels[5],true));
+ data.reset(new WorkspaceData(numVoxels[0], numVoxels[1], numVoxels[2], numVoxels[3], numVoxels[4], numVoxels[5],true));
unsigned char *d = new unsigned char[size];
if(version[0] == 1 && version[1] <= 2)
@@ -421,9 +321,9 @@ void ReachabilitySpace::load(const std::string &filename)
file.close();
}
-void ReachabilitySpace::save(const std::string &filename)
+void WorkspaceRepresentation::save(const std::string &filename)
{
- THROW_VR_EXCEPTION_IF(!data || !nodeSet, "No reachability space loaded");
+ THROW_VR_EXCEPTION_IF(!data || !nodeSet, "No WorkspaceRepresentation data loaded");
std::ofstream file;
file.open(filename.c_str(), std::ios::out | std::ios::binary);
@@ -432,7 +332,9 @@ void ReachabilitySpace::save(const std::string &filename)
try
{
// File type
- writeString(file, "ReachabilitySpace Binary File");
+ std::string tmpStr = type;
+ tmpStr += " Binary File";
+ writeString(file, tmpStr);
// Version
write<int>(file, 2);
@@ -520,14 +422,14 @@ void ReachabilitySpace::save(const std::string &filename)
file.close();
}
-int ReachabilitySpace::getMaxEntry()
+int WorkspaceRepresentation::getMaxEntry() const
{
if (!data)
return 0;
return data->getMaxEntry();
}
-int ReachabilitySpace::getMaxEntry( const Eigen::Vector3f &position_global )
+int WorkspaceRepresentation::getMaxEntry( const Eigen::Vector3f &position_global ) const
{
Eigen::Matrix4f gp;
gp.setIdentity();
@@ -541,7 +443,7 @@ int ReachabilitySpace::getMaxEntry( const Eigen::Vector3f &position_global )
return getMaxEntry(v[0],v[1],v[2]);
}
-int ReachabilitySpace::getMaxEntry( int x0, int x1, int x2 )
+int WorkspaceRepresentation::getMaxEntry( int x0, int x1, int x2 ) const
{
int maxValue = 0;
for(int a = 0; a < getNumVoxels(3); a+=1)
@@ -560,7 +462,7 @@ int ReachabilitySpace::getMaxEntry( int x0, int x1, int x2 )
}
-float ReachabilitySpace::getVoxelSize(int dim)
+float WorkspaceRepresentation::getVoxelSize(int dim) const
{
if(dim < 0 || dim > 6)
return 0.0f;
@@ -569,23 +471,23 @@ float ReachabilitySpace::getVoxelSize(int dim)
return spaceSize[dim] / numVoxels[dim];
}
-RobotNodePtr ReachabilitySpace::getBaseNode()
+RobotNodePtr WorkspaceRepresentation::getBaseNode()
{
return baseNode;
}
-RobotNodePtr ReachabilitySpace::getTCP()
+RobotNodePtr WorkspaceRepresentation::getTCP()
{
return tcpNode;
}
-RobotNodeSetPtr ReachabilitySpace::getNodeSet()
+RobotNodeSetPtr WorkspaceRepresentation::getNodeSet()
{
return nodeSet;
}
-void ReachabilitySpace::setCurrentTCPPoseEntryIfLower(unsigned char e)
+void WorkspaceRepresentation::setCurrentTCPPoseEntryIfLower(unsigned char e)
{
- THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "No reachability space loaded");
+ THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "No WorkspaceRepresentation data loaded");
Eigen::Matrix4f p = tcpNode->getGlobalPose();
if (baseNode)
@@ -616,37 +518,7 @@ void ReachabilitySpace::setCurrentTCPPoseEntryIfLower(unsigned char e)
buildUpLoops++;
}
-void ReachabilitySpace::addCurrentTCPPose()
-{
- THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "No reachability space loaded");
-
- Eigen::Matrix4f p = tcpNode->getGlobalPose();
- if (baseNode)
- p = baseNode->toLocalCoordinateSystem(p);
-
- float x[6];
- MathTools::eigen4f2rpy(p,x);
-
- // check for achieved values
- for (int i=0;i<6;i++)
- {
- if (x[i] < achievedMinValues[i])
- achievedMinValues[i] = x[i];
- if (x[i] > achievedMaxValues[i])
- achievedMaxValues[i] = x[i];
- }
-
- // get voxels
- unsigned int v[6];
- if (getVoxelFromPose(x,v))
- {
- data->increaseDatum(v);
- }
-
- buildUpLoops++;
-}
-
-bool ReachabilitySpace::getVoxelFromPose(float x[6], unsigned int v[6])
+bool WorkspaceRepresentation::getVoxelFromPose(float x[6], unsigned int v[6]) const
{
int a;
for (int i=0;i<6;i++)
@@ -661,7 +533,7 @@ bool ReachabilitySpace::getVoxelFromPose(float x[6], unsigned int v[6])
return true;
}
-bool ReachabilitySpace::getVoxelFromPose( const Eigen::Matrix4f &globalPose, unsigned int v[6] )
+bool WorkspaceRepresentation::getVoxelFromPose( const Eigen::Matrix4f &globalPose, unsigned int v[6] ) const
{
float x[6];
@@ -673,29 +545,7 @@ bool ReachabilitySpace::getVoxelFromPose( const Eigen::Matrix4f &globalPose, uns
return getVoxelFromPose(x,v);
}
-void ReachabilitySpace::addRandomTCPPoses( unsigned int loops, bool checkForSelfCollisions )
-{
- THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "Reachability space not initialized");
-
- std::vector<float> c;
- nodeSet->getJointValues(c);
- bool visuSate = robot->getUpdateVisualizationStatus();
- robot->setUpdateVisualization(false);
-
- updateBaseTransformation();
-
- for (unsigned int i=0;i<loops;i++)
- {
- if (setRobotNodesToRandomConfig(checkForSelfCollisions))
- addCurrentTCPPose();
- else
- VR_WARNING << "Could not find collision-free configuration...";
- }
- robot->setUpdateVisualization(visuSate);
- nodeSet->setJointValues(c);
-}
-
-bool ReachabilitySpace::setRobotNodesToRandomConfig( bool checkForSelfCollisions /*= true*/ )
+bool WorkspaceRepresentation::setRobotNodesToRandomConfig( bool checkForSelfCollisions /*= true*/ )
{
static const float randMult = (float)(1.0/(double)(RAND_MAX));
float rndValue;
@@ -725,10 +575,11 @@ bool ReachabilitySpace::setRobotNodesToRandomConfig( bool checkForSelfCollisions
return false;
}
-void ReachabilitySpace::print()
+void WorkspaceRepresentation::print()
{
cout << "-----------------------------------------------------------" << endl;
- cout << "ReachabilitySpace - Status:" << endl;
+ cout << type << " - Status:" << endl;
+
if (data)
{
if (nodeSet)
@@ -756,11 +607,11 @@ void ReachabilitySpace::print()
cout << "Used " << buildUpLoops << " loops for building the random configs " << endl;
cout << "Discretization step sizes: Translation: " << discretizeStepTranslation << " - Rotation: " << discretizeStepRotation << endl;
- cout << "ReachabilitySpace space extends: " << numVoxels[0] << "x" << numVoxels[1] << "x" << numVoxels[2] << "x" << numVoxels[3] << "x" << numVoxels[4] << "x" << numVoxels[5] << endl;
+ cout << type << " data extends: " << numVoxels[0] << "x" << numVoxels[1] << "x" << numVoxels[2] << "x" << numVoxels[3] << "x" << numVoxels[4] << "x" << numVoxels[5] << endl;
cout << "Filled " << data->getVoxelFilledCount() << " of " << (numVoxels[0]*numVoxels[1]*numVoxels[2]*numVoxels[3]*numVoxels[4]*numVoxels[5]) << " voxels" << endl;
cout << "Collisions: " << collisionConfigs << endl;
cout << "Maximum entry in a voxel: " << (int)data->getMaxEntry() << endl;
- cout << "Reachability workspace extend (as defined on construction):" << endl;
+ cout << type << " workspace extend (as defined on construction):" << endl;
cout << "Min boundary: ";
for (int i=0;i<6;i++)
cout << minBounds[i] << ",";
@@ -780,13 +631,13 @@ void ReachabilitySpace::print()
cout << endl;
} else
{
- cout << "ReachabilitySpace not created yet..." << endl;
+ cout << type << " not created yet..." << endl;
}
cout << "-----------------------------------------------------------" << endl;
cout << endl;
}
-void ReachabilitySpace::reset()
+void WorkspaceRepresentation::reset()
{
data.reset();
nodeSet.reset();
@@ -798,8 +649,6 @@ void ReachabilitySpace::reset()
collisionConfigs = 0;
discretizeStepTranslation = 0;
discretizeStepRotation = 0;
- //voxelFilledCount = 0;
- //maxVoxelEntry = 0;
for (int i=0;i<6;i++)
{
minBounds[i] = FLT_MAX;
@@ -812,7 +661,7 @@ void ReachabilitySpace::reset()
baseTransformation.setIdentity();
}
-void ReachabilitySpace::initialize( RobotNodeSetPtr nodeSet, float discretizeStepTranslation, float discretizeStepRotation,
+void WorkspaceRepresentation::initialize( RobotNodeSetPtr nodeSet, float discretizeStepTranslation, float discretizeStepRotation,
float minBounds[6], float maxBounds[6],
SceneObjectSetPtr staticCollisionModel,
SceneObjectSetPtr dynamicCollisionModel,
@@ -838,7 +687,7 @@ void ReachabilitySpace::initialize( RobotNodeSetPtr nodeSet, float discretizeSte
{
THROW_VR_EXCEPTION("Robot does not know basenode:" << baseNode->getName());
}
- THROW_VR_EXCEPTION_IF (nodeSet->hasRobotNode(baseNode)," baseNode is part of RobotNodeSet! This is not a good idea, since the globalPose of the baseNode will change during buildup of reachability data...");
+ THROW_VR_EXCEPTION_IF (nodeSet->hasRobotNode(baseNode)," baseNode is part of RobotNodeSet! This is not a good idea, since the globalPose of the baseNode will change during buildup of WorkspaceRepresentation data...");
updateBaseTransformation();
this->staticCollisionModel = staticCollisionModel;
this->dynamicCollisionModel = dynamicCollisionModel;
@@ -866,17 +715,17 @@ void ReachabilitySpace::initialize( RobotNodeSetPtr nodeSet, float discretizeSte
numVoxels[i] = (int)(spaceSize[i] / discretizeStepRotation) + 1;
THROW_VR_EXCEPTION_IF( (numVoxels[i]<=0), " numVoxels <= 0 in dimension " << i);
}
- data.reset(new ReachabilitySpaceData(numVoxels[0],numVoxels[1],numVoxels[2],numVoxels[3],numVoxels[4],numVoxels[5],adjustOnOverflow));
+ data.reset(new WorkspaceData(numVoxels[0],numVoxels[1],numVoxels[2],numVoxels[3],numVoxels[4],numVoxels[5],adjustOnOverflow));
}
-void ReachabilitySpace::binarize()
+void WorkspaceRepresentation::binarize()
{
if (data)
data->binarize();
}
-unsigned char ReachabilitySpace::getEntry( const Eigen::Matrix4f &globalPose )
+unsigned char WorkspaceRepresentation::getEntry( const Eigen::Matrix4f &globalPose )
{
if (!data)
{
@@ -891,34 +740,15 @@ unsigned char ReachabilitySpace::getEntry( const Eigen::Matrix4f &globalPose )
return data->get(v);
} else
{
- // position is outside reachability data
+ // position is outside WorkspaceRepresentation data
return 0;
}
}
-bool ReachabilitySpace::isReachable( const Eigen::Matrix4f &globalPose )
-{
- return (getEntry(globalPose) > 0);
-}
-VirtualRobot::GraspSetPtr ReachabilitySpace::getReachableGrasps( GraspSetPtr grasps, ManipulationObjectPtr object )
-{
- THROW_VR_EXCEPTION_IF(!object,"no object");
- THROW_VR_EXCEPTION_IF(!grasps,"no grasps");
-
- GraspSetPtr result(new GraspSet(grasps->getName(),grasps->getRobotType(),grasps->getEndEffector()));
- for (unsigned int i=0; i<grasps->getSize(); i++)
- {
- Eigen::Matrix4f m = grasps->getGrasp(i)->getTcpPoseGlobal(object->getGlobalPose());
- if (isReachable(m))
- result->addGrasp(grasps->getGrasp(i));
- }
- return result;
-}
-
-Eigen::Matrix4f ReachabilitySpace::sampleReachablePose()
+Eigen::Matrix4f WorkspaceRepresentation::sampleCoveredPose()
{
int maxLoops = 10000;
int i = 0;
@@ -953,7 +783,7 @@ Eigen::Matrix4f ReachabilitySpace::sampleReachablePose()
return m;
}
-int ReachabilitySpace::fillHoles()
+int WorkspaceRepresentation::fillHoles()
{
unsigned int x[6];
int res = 0;
@@ -998,33 +828,33 @@ int ReachabilitySpace::fillHoles()
return res;
}
-int ReachabilitySpace::getNumVoxels( int dim )
+int WorkspaceRepresentation::getNumVoxels( int dim ) const
{
VR_ASSERT((dim>=0 && dim<6));
return numVoxels[dim];
}
-float ReachabilitySpace::getMinBound( int dim )
+float WorkspaceRepresentation::getMinBound( int dim ) const
{
VR_ASSERT((dim>=0 && dim<6));
return minBounds[dim];
}
-float ReachabilitySpace::getMaxBound( int dim )
+float WorkspaceRepresentation::getMaxBound( int dim ) const
{
VR_ASSERT((dim>=0 && dim<6));
return maxBounds[dim];
}
-unsigned char ReachabilitySpace::getVoxelEntry(unsigned int a, unsigned int b, unsigned int c, unsigned int d, unsigned int e, unsigned int f)
+unsigned char WorkspaceRepresentation::getVoxelEntry(unsigned int a, unsigned int b, unsigned int c, unsigned int d, unsigned int e, unsigned int f) const
{
return data->get(a,b,c,d,e,f);
}
-int ReachabilitySpace::getMaxSummedAngleReachablity()
+int WorkspaceRepresentation::getMaxSummedAngleReachablity()
{
int maxValue = 0;
for(int a = 0; a < getNumVoxels(0); a+=1)
@@ -1042,5 +872,39 @@ int ReachabilitySpace::getMaxSummedAngleReachablity()
return maxValue;
}
+bool WorkspaceRepresentation::isCovered( const Eigen::Matrix4f &globalPose )
+{
+ return (getEntry(globalPose) > 0);
+}
+
+void WorkspaceRepresentation::setCurrentTCPPoseEntry( unsigned char e )
+{
+ THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "No WorkspaceRepresentation data loaded");
+
+ Eigen::Matrix4f p = tcpNode->getGlobalPose();
+ if (baseNode)
+ p = baseNode->toLocalCoordinateSystem(p);
+
+ float x[6];
+ MathTools::eigen4f2rpy(p,x);
+
+ // check for achieved values
+ for (int i=0;i<6;i++)
+ {
+ if (x[i] < achievedMinValues[i])
+ achievedMinValues[i] = x[i];
+ if (x[i] > achievedMaxValues[i])
+ achievedMaxValues[i] = x[i];
+ }
+
+ // get voxels
+ unsigned int v[6];
+ if (getVoxelFromPose(x,v))
+ {
+ data->setDatum(v,e);
+ }
+
+ buildUpLoops++;
+}
} // namespace VirtualRobot
diff --git a/VirtualRobot/ReachabilitySpace.h b/VirtualRobot/Workspace/WorkspaceRepresentation.h
similarity index 51%
rename from VirtualRobot/ReachabilitySpace.h
rename to VirtualRobot/Workspace/WorkspaceRepresentation.h
index bfdf7ad44..c96799170 100644
--- a/VirtualRobot/ReachabilitySpace.h
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.h
@@ -20,11 +20,12 @@
* GNU Lesser General Public License
*
*/
-#ifndef _VirtualRobot_ReachabilitySpace_h_
-#define _VirtualRobot_ReachabilitySpace_h_
+#ifndef _VirtualRobot_WorkspaceRepresentation_h_
+#define _VirtualRobot_WorkspaceRepresentation_h_
+
+#include "../VirtualRobotImportExport.h"
+#include "WorkspaceData.h"
-#include "VirtualRobotImportExport.h"
-#include "Visualization/VisualizationFactory.h"
#include <boost/enable_shared_from_this.hpp>
#include <boost/type_traits/is_base_of.hpp>
@@ -38,166 +39,63 @@
namespace VirtualRobot
{
-class ReachabilitySpace;
-/*!
- Stores a 6-dimensional array for the vertex data of a reachability space.
-*/
-class VIRTUAL_ROBOT_IMPORT_EXPORT ReachabilitySpaceData : public boost::enable_shared_from_this<ReachabilitySpaceData>
-{
-public:
- friend class ReachabilitySpace;
- /*!
- Constructor, fills the data with 0
- */
- ReachabilitySpaceData(unsigned int size1, unsigned int size2, unsigned int size3,
- unsigned int size4, unsigned int size5, unsigned int size6, bool adjustOnOverflow);
- ~ReachabilitySpaceData();
-
- //! Return the amount of data in bytes
- int getSize();
-
- inline unsigned int getPos(unsigned int x0, unsigned int x1, unsigned int x2,
- unsigned int x3, unsigned int x4, unsigned int x5)
- {
- return x0 * sizeX0 + x1 * sizeX1 + x2 * sizeX2 + x3 * sizeX3 + x4 * sizeX4 + x5;
- }
-
- inline unsigned int getPos( unsigned int x[6] )
- {
- return x[0] * sizeX0 + x[1] * sizeX1 + x[2] * sizeX2 + x[3] * sizeX3 + x[4] * sizeX4 + x[5];
- }
-
- inline void setDatum( unsigned int x0, unsigned int x1, unsigned int x2,
- unsigned int x3, unsigned int x4, unsigned int x5, unsigned char value)
- {
- unsigned int pos = getPos(x0,x1,x2,x3,x4,x5);
- if (data[pos]==0)
- voxelFilledCount++;
- data[pos] = value;
- if (value >= maxEntry)
- maxEntry = value;
- }
-
- inline void setDatum(unsigned int x[6], unsigned char value)
- {
- unsigned int pos = getPos(x);
- if (data[pos]==0)
- voxelFilledCount++;
- data[pos] = value;
- if (value >= maxEntry)
- maxEntry = value;
- }
-
- inline void increaseDatum( unsigned int x0, unsigned int x1, unsigned int x2,
- unsigned int x3, unsigned int x4, unsigned int x5)
- {
- unsigned int pos = getPos(x0,x1,x2,x3,x4,x5);
- unsigned char e = data[pos];
- if (e==0)
- voxelFilledCount++;
- if (e<UCHAR_MAX)
- {
- data[pos]++;
- if (e >= maxEntry)
- maxEntry = e+1;
- } else if (adjustOnOverflow)
- bisectData();
- }
- inline void increaseDatum( unsigned int x[6] )
- {
- unsigned int pos = getPos(x);
- unsigned char e = data[pos];
- if (e==0)
- voxelFilledCount++;
- if (e<UCHAR_MAX)
- {
- data[pos]++;
- if (e >= maxEntry)
- maxEntry = e+1;
- } else if (adjustOnOverflow)
- bisectData();
- }
-
- void setData(unsigned char *data);
- const unsigned char *getData() const;
-
- //! Simulates a multi-dimensional array access
- inline unsigned char get(unsigned int x0, unsigned int x1, unsigned int x2,
- unsigned int x3, unsigned int x4, unsigned int x5) const
- {
- return data[x0 * sizeX0 + x1 * sizeX1 + x2 * sizeX2 + x3 * sizeX3 + x4 * sizeX4 + x5];
- }
-
- //! Simulates a multi-dimensional array access
- inline unsigned char get( unsigned int x[6] ) const
- {
- return data[x[0] * sizeX0 + x[1] * sizeX1 + x[2] * sizeX2 + x[3] * sizeX3 + x[4] * sizeX4 + x[5]];
- }
-
- unsigned char getMaxEntry();
- unsigned int getVoxelFilledCount();
- void binarize();
-
- void bisectData();
-private:
- unsigned int sizes[6];
- unsigned int sizeX0,sizeX1,sizeX2,sizeX3,sizeX4;
- unsigned char *data;
- unsigned char maxEntry;
- unsigned int voxelFilledCount;
- bool adjustOnOverflow;
-};
/*!
- This class represents an approximation of the reachability distribution of a kinematic chain (e.g. an arm).
- Consists of voxels covering the 6D space for position (XYZ) and orientation (RPY).
- Each voxels holds a counter with the number of successful IK solver calls.
- The discretized reachability space can be written to and loaded from binary files.
-
- The reachability space is linked to a base coordinate system which is defined by a robot joint.
- This base system is used in order to use the reachability space when the robot is moving.
- I.E. think of an arm of a humanoid where the reachability space is linked to the shoulder.
- When the torso moves, the reachability also changes it's position according to the position of the shoulder.
+ This class represents a voxelized approximation of the workspace that is covered by a kinematic chain of a robot.
+ The voxel grid covers the 6d Cartesian space: xyz translations (mm) and rpy orientations.
+ Each voxels holds a counter (uchar) that holds information, e.g. about reachbaility.
+ The discretized data can be written to and loaded from binary files.
+
+ The data is linked to a base coordinate system which is defined by a robot joint.
+ This base system is used to align the data when the robot is moving.
+ I.E. think of an arm of a humanoid where the workspace representation is linked to the shoulder.
+ When the torso moves, the data representation also changes it's position according to the position of the shoulder.
*/
-class VIRTUAL_ROBOT_IMPORT_EXPORT ReachabilitySpace : public boost::enable_shared_from_this<ReachabilitySpace>
+
+class VIRTUAL_ROBOT_IMPORT_EXPORT WorkspaceRepresentation : public boost::enable_shared_from_this<WorkspaceRepresentation>
{
public:
friend class CoinVisualizationFactory;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- ReachabilitySpace(RobotPtr robot);
+ WorkspaceRepresentation(RobotPtr robot);
/*!
Reset all data.
*/
- void reset();
+ virtual void reset();
/*!
Load the reachability data from a binary file.
Exceptions are thrown on case errors are detected.
*/
- void load(const std::string &filename);
+ virtual void load(const std::string &filename);
/*!
Store the reachability data to a binary file.
Exceptions are thrown on case errors are detected.
*/
- void save(const std::string &filename);
+ virtual void save(const std::string &filename);
/*!
Return corresponding entry of reachability data
*/
unsigned char getEntry(const Eigen::Matrix4f &globalPose);
- bool isReachable(const Eigen::Matrix4f &globalPose);
-
- GraspSetPtr getReachableGrasps(GraspSetPtr grasps, ManipulationObjectPtr object);
-
- int getMaxEntry();
- float getVoxelSize(int dim);
+ //! Returns the maximum entry of a voxel.
+ int getMaxEntry() const;
+
+ //! returns the extends of a voxel at corresponding dimension.
+ float getVoxelSize(int dim) const;
+
+ //! The base node of this reachability data
RobotNodePtr getBaseNode();
+
+ //! The corresponding TCP
RobotNodePtr getTCP();
+
+ //! The kinematic chain that is covered by this reachability data
RobotNodeSetPtr getNodeSet();
/*!
@@ -213,7 +111,7 @@ public:
\param tcpNode If given, the pose of this node is used for reachability calculations. If not given, the TCP node of the nodeSet is used.
\param adjustOnOverflow If set the 8bit data is divided by 2 when one voxel entry exceeds 255. Otherwise the entries remain at 255.
*/
- void initialize(RobotNodeSetPtr nodeSet,
+ virtual void initialize(RobotNodeSetPtr nodeSet,
float discretizeStepTranslation,
float discretizeStepRotation,
float minBounds[6],
@@ -224,80 +122,82 @@ public:
RobotNodePtr tcpNode = RobotNodePtr(),
bool adjustOnOverflow = true);
- /*!
- Append current TCP pose to Reachability Data
- */
- void addCurrentTCPPose();
-
/*!
Sets entry that corresponds to TCP pose to e, if current entry is lower than e.
+ Therefore the corresponding voxel of the current TCP pose is determined and its entry is adjusted.
*/
- void setCurrentTCPPoseEntryIfLower(unsigned char e);
-
+ virtual void setCurrentTCPPoseEntryIfLower(unsigned char e);
+
/*!
- Append a number of random TCP poses to Reachability Data
- \param loops Number of poses that should be appended
- \param checkForSelfCollisions Build a collision-free configuration. If true, random configs are generated until one is collision-free.
+ Sets entry that corresponds to TCP pose to e.
+ Therefore the corresponding voxel of the current TCP pose is determined and its entry is set.
*/
- void addRandomTCPPoses(unsigned int loops, bool checkForSelfCollisions = true);
+ virtual void setCurrentTCPPoseEntry(unsigned char e);
+
/*!
Generate a random configuration for the robot node set. This configuration is within the joint limits of the current robot node set.
\param checkForSelfCollisions Build a collision-free configuration. If true, random configs are generated until one is collision-free.
*/
- bool setRobotNodesToRandomConfig(bool checkForSelfCollisions = true);
+ virtual bool setRobotNodesToRandomConfig(bool checkForSelfCollisions = true);
/*!
Cut all data >1 to 1. This reduces the file size when saving compressed data.
*/
- void binarize();
+ virtual void binarize();
/*!
- Checks for all voxels with entiry==0 if there are neighbors with entries>0.0 If so the entry is set to the averaged value of the neighbors
+ Checks for all voxels with entiry==0 if there are neighbors with entries>0.
+ If so the entry is set to the averaged value of the neighbors
\return The number of changed voxels.
*/
- int fillHoles();
+ virtual int fillHoles();
/*!
Print status information
*/
- void print();
+ virtual void print();
- //! returns a random pose that is reachable
- Eigen::Matrix4f sampleReachablePose();
+ //! returns a random pose that is covered by the workspace data
+ Eigen::Matrix4f sampleCoveredPose();
+
+ /*!
+ Returns true, if the corresponding voxel entry is nun zero.
+ */
+ bool isCovered(const Eigen::Matrix4f &globalPose);
- int getNumVoxels(int dim);
- float getMinBound(int dim);
- float getMaxBound(int dim);
+ virtual int getNumVoxels(int dim) const;
+ virtual float getMinBound(int dim) const;
+ virtual float getMaxBound(int dim) const;
- unsigned char getVoxelEntry(unsigned int a, unsigned int b, unsigned int c, unsigned int d, unsigned int e, unsigned int f);
+ virtual unsigned char getVoxelEntry(unsigned int a, unsigned int b, unsigned int c, unsigned int d, unsigned int e, unsigned int f) const;
/*!
Sums all angle (x3,x4,x5) entries for the given position.
*/
- int sumAngleReachabilities(int x0, int x1, int x2);
+ virtual int sumAngleReachabilities(int x0, int x1, int x2) const;
/*!
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);
+ virtual int getMaxEntry(int x0, int x1, int x2) const;
/*!
Returns the maximum reachability entry that can be achieved by an arbitrary orientation at the given position.
*/
- int getMaxEntry( const Eigen::Vector3f &position_global );
+ virtual int getMaxEntry( const Eigen::Vector3f &position_global ) const;
/*!
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]);
+ virtual bool getVoxelFromPose(const Eigen::Matrix4f &globalPose, unsigned int v[6]) const;
/*!
Returns the maximum that can be achieved by calling sumAngleReachabilities()
*/
- int getMaxSummedAngleReachablity();
+ virtual int getMaxSummedAngleReachablity();
protected:
@@ -311,14 +211,15 @@ protected:
template<typename T> void write(std::ofstream &file, T value);
template<typename T> void writeArray(std::ofstream &file, const T *value, int num);
- //! (Un-)compress the data
+ //! Uncompress the data
void uncompressData(const unsigned char *source, int size, unsigned char *dest);
+ //! Compress the data
unsigned char *compressData(const unsigned char *source, int size, int &compressedSize);
//! Refetch the base joint's transformation, in case this joint has moved
void updateBaseTransformation();
- bool getVoxelFromPose(float x[6], unsigned int v[6]);
+ virtual bool getVoxelFromPose(float x[6], unsigned int v[6]) const;
RobotPtr robot;
@@ -335,7 +236,7 @@ protected:
// Number of reported collisions
int collisionConfigs;
- // Tells how to discretize the reachability space
+ // Tells how to discretize the reachability data
float discretizeStepTranslation;
float discretizeStepRotation;
float minBounds[6];
@@ -351,13 +252,15 @@ protected:
// workspace extend in each dimension
float spaceSize[6];
- ReachabilitySpaceDataPtr data;
+ WorkspaceDataPtr data;
bool adjustOnOverflow;
+
+ std::string type;
};
} // namespace VirtualRobot
-#endif // _ReachabilitySpace_h_
+#endif // _VirtualRobot_WorkspaceRepresentation_h_
diff --git a/VirtualRobot/Workspace/tests/CMakeLists.txt b/VirtualRobot/Workspace/tests/CMakeLists.txt
new file mode 100644
index 000000000..b17f4bc74
--- /dev/null
+++ b/VirtualRobot/Workspace/tests/CMakeLists.txt
@@ -0,0 +1 @@
+ADD_VR_TEST(VoxelTreeTest)
diff --git a/VirtualRobot/Workspace/tests/TransformationTest.cpp b/VirtualRobot/Workspace/tests/TransformationTest.cpp
new file mode 100644
index 000000000..6f29a6131
--- /dev/null
+++ b/VirtualRobot/Workspace/tests/TransformationTest.cpp
@@ -0,0 +1,55 @@
+/**
+* @package VirtualRobot
+* @author Manfred Kroehnert <mkroehnert _at_ users dot sourceforge dot net>
+* @copyright 2010 Manfred Kroehnert
+*/
+
+#define BOOST_TEST_MODULE VirtualRobot_TransformationTest
+
+#include <VirtualRobot/VirtualRobotTest.h>
+#include <VirtualRobot/Transformation/Transformation.h>
+#include <Eigen/Core>
+
+BOOST_AUTO_TEST_SUITE(Transformation)
+
+void checkMatrix4fIdentity(const Eigen::Matrix4f& matrix)
+{
+ // first row
+ BOOST_CHECK_EQUAL(1, matrix(0, 0));
+ BOOST_CHECK_EQUAL(0, matrix(0, 1));
+ BOOST_CHECK_EQUAL(0, matrix(0, 2));
+ BOOST_CHECK_EQUAL(0, matrix(0, 3));
+ // second row
+ BOOST_CHECK_EQUAL(0, matrix(1, 0));
+ BOOST_CHECK_EQUAL(1, matrix(1, 1));
+ BOOST_CHECK_EQUAL(0, matrix(1, 2));
+ BOOST_CHECK_EQUAL(0, matrix(1, 3));
+ // third row
+ BOOST_CHECK_EQUAL(0, matrix(2, 0));
+ BOOST_CHECK_EQUAL(0, matrix(2, 1));
+ BOOST_CHECK_EQUAL(1, matrix(2, 2));
+ BOOST_CHECK_EQUAL(0, matrix(2, 3));
+ // fourth row
+ BOOST_CHECK_EQUAL(0, matrix(3, 0));
+ BOOST_CHECK_EQUAL(0, matrix(3, 1));
+ BOOST_CHECK_EQUAL(0, matrix(3, 2));
+ BOOST_CHECK_EQUAL(1, matrix(3, 3));
+}
+
+void checkVector3fIdentity(const Eigen::Vector3f& vector)
+{
+ BOOST_CHECK_EQUAL(1, vector.x());
+ BOOST_CHECK_EQUAL(0, vector.y());
+ BOOST_CHECK_EQUAL(0, vector.z());
+}
+
+BOOST_AUTO_TEST_CASE(TransformationConstructorTest)
+{
+ VirtualRobot::Transformation transform;
+ checkVector3fIdentity(transform.getJointRotationAxis());
+ checkVector3fIdentity(transform.getJointTranslationDirection());
+ checkMatrix4fIdentity(transform.getPreJointTransformation());
+ checkMatrix4fIdentity(transform.getPostJointTransformation());
+}
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/VirtualRobot/Workspace/tests/VoxelTreeTest.cpp b/VirtualRobot/Workspace/tests/VoxelTreeTest.cpp
new file mode 100644
index 000000000..c9be96d15
--- /dev/null
+++ b/VirtualRobot/Workspace/tests/VoxelTreeTest.cpp
@@ -0,0 +1,58 @@
+/**
+* @package VirtualRobot
+* @author Nikolaus Vahrenkamp
+* @copyright 2012 nv
+*/
+
+#define BOOST_TEST_MODULE VirtualRobot_VoxelTreeTest
+
+#include <VirtualRobot/VirtualRobotTest.h>
+#include <VirtualRobot/Workspace/VoxelTree6D.hpp>
+
+BOOST_AUTO_TEST_SUITE(VoxelTree)
+
+BOOST_AUTO_TEST_CASE(VoxelTreeConstructionTest)
+{
+ float minB[6];
+ float maxB[6];
+ for (int i=0;i<3;i++)
+ {
+ minB[i] = -100.0f;
+ maxB[i] = 100.0f;
+ minB[3+i] = (float)-M_PI;
+ maxB[3+i] = (float)M_PI;
+ }
+ BOOST_REQUIRE_NO_THROW(VirtualRobot::VoxelTree6D<unsigned char> v(minB,maxB,20.0f,1.0f));
+}
+
+
+BOOST_AUTO_TEST_CASE(VoxelTreeEntriesTest)
+{
+ float minB[6];
+ float maxB[6];
+ for (int i=0;i<3;i++)
+ {
+ minB[i] = -100.0f;
+ maxB[i] = 100.0f;
+ minB[3+i] = (float)-M_PI;
+ maxB[3+i] = (float)M_PI;
+ }
+ VirtualRobot::VoxelTree6D<unsigned char> v(minB,maxB,20.0f,1.0f);
+ float pos[6];
+ for (int i=0;i<6;i++)
+ pos[i] = 0;
+
+ unsigned char* c = v.getEntry(pos);
+ bool isNull = (c==NULL);
+ BOOST_REQUIRE(isNull);
+
+ v.setEntry(pos,10);
+ c = v.getEntry(pos);
+ isNull = (c==NULL);
+ BOOST_REQUIRE(!isNull);
+
+ BOOST_REQUIRE_EQUAL(*c,10);
+
+}
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/VirtualRobot/examples/GraspEditor/GraspEditorWindow.cpp b/VirtualRobot/examples/GraspEditor/GraspEditorWindow.cpp
index 96de6ed5b..dbdceec63 100644
--- a/VirtualRobot/examples/GraspEditor/GraspEditorWindow.cpp
+++ b/VirtualRobot/examples/GraspEditor/GraspEditorWindow.cpp
@@ -1,7 +1,7 @@
#include "GraspEditorWindow.h"
#include "VirtualRobot/EndEffector/EndEffector.h"
-#include "VirtualRobot/ReachabilitySpace.h"
+#include "VirtualRobot/Workspace/Reachability.h"
#include "VirtualRobot/ManipulationObject.h"
#include "VirtualRobot/Grasp.h"
#include "VirtualRobot/GraspSet.h"
diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
index 2efba51d0..b66bc7e26 100644
--- a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
+++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
@@ -1,7 +1,7 @@
#include "showRobotWindow.h"
#include "VirtualRobot/EndEffector/EndEffector.h"
-#include "VirtualRobot/ReachabilitySpace.h"
+#include "VirtualRobot/Workspace/Reachability.h"
#include <VirtualRobot/RuntimeEnvironment.h>
#include <QFileDialog>
diff --git a/VirtualRobot/examples/SceneViewer/showSceneWindow.cpp b/VirtualRobot/examples/SceneViewer/showSceneWindow.cpp
index 9d3fdb519..85d8dff13 100644
--- a/VirtualRobot/examples/SceneViewer/showSceneWindow.cpp
+++ b/VirtualRobot/examples/SceneViewer/showSceneWindow.cpp
@@ -1,7 +1,7 @@
#include "showSceneWindow.h"
#include "VirtualRobot/EndEffector/EndEffector.h"
-#include "VirtualRobot/ReachabilitySpace.h"
+#include "VirtualRobot/Workspace/Reachability.h"
#include "VirtualRobot/ManipulationObject.h"
#include "VirtualRobot/XML/ObjectIO.h"
diff --git a/VirtualRobot/examples/reachability/reachabilityScene.cpp b/VirtualRobot/examples/reachability/reachabilityScene.cpp
index 99ede5e4a..28167b423 100644
--- a/VirtualRobot/examples/reachability/reachabilityScene.cpp
+++ b/VirtualRobot/examples/reachability/reachabilityScene.cpp
@@ -20,6 +20,8 @@
#define ICUB
//#define AXIS_X
+// --robot robots/iCub/iCub_LeftHand_Extended.xml
+
using std::cout;
using std::endl;
using namespace VirtualRobot;
@@ -33,7 +35,7 @@ int main(int argc, char *argv[])
#ifdef ICUB
std::string filenameRob(VR_BASE_DIR "/data/robots/iCub/iCub.xml");
Eigen::Vector3f axisTCP(1.0f,0,0);
- filenameReach = "reachability/iCub_HipLeftArm.bin";
+ filenameReach = "reachability/iCub_HipRightArm.bin";
#else
std::string filenameRob(VR_BASE_DIR "/data/robots/ArmarIII/ArmarIII.xml");
Eigen::Vector3f axisTCP(0,0,1.0f);
diff --git a/VirtualRobot/examples/reachability/reachabilityWindow.cpp b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
index b8d30feae..d5d8916f5 100644
--- a/VirtualRobot/examples/reachability/reachabilityWindow.cpp
+++ b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
@@ -1,7 +1,7 @@
#include "reachabilityWindow.h"
#include "VirtualRobot/EndEffector/EndEffector.h"
-#include "VirtualRobot/ReachabilitySpace.h"
+#include "VirtualRobot/Workspace/Reachability.h"
#include "VirtualRobot/XML/RobotIO.h"
#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h"
#include <VirtualRobot/RuntimeEnvironment.h>
@@ -416,7 +416,7 @@ void reachabilityWindow::createReach()
}
if (diag.exec())
{
- reachSpace.reset(new ReachabilitySpace(robot));
+ reachSpace.reset(new Reachability(robot));
float minB[6];// = {-1000.0f,-1000.0f,-1000.0f,(float)-M_PI,(float)-M_PI,(float)-M_PI};
float maxB[6];// ={1000.0f,1000.0f,1000.0f,(float)M_PI,(float)M_PI,(float)M_PI};
minB[0] = UICreate.doubleSpinBoxMinX->value();
@@ -487,7 +487,7 @@ void reachabilityWindow::loadReachFile(std::string filename)
if (!robot)
return;
reachFile = filename;
- reachSpace.reset(new ReachabilitySpace(robot));
+ reachSpace.reset(new Reachability(robot));
reachSpace->load(reachFile);
reachSpace->print();
if (reachSpace->getNodeSet())
diff --git a/VirtualRobot/examples/reachability/reachabilityWindow.h b/VirtualRobot/examples/reachability/reachabilityWindow.h
index 363ca0f91..abd956f2f 100644
--- a/VirtualRobot/examples/reachability/reachabilityWindow.h
+++ b/VirtualRobot/examples/reachability/reachabilityWindow.h
@@ -14,7 +14,7 @@
#include <QtCore/QtGlobal>
#include <QtGui/QtGui>
#include <QtCore/QtCore>
-#include <VirtualRobot/ReachabilitySpace.h>
+#include <VirtualRobot/Workspace/Reachability.h>
#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/nodes/SoEventCallback.h>
@@ -100,7 +100,7 @@ protected:
std::vector < VirtualRobot::RobotNodePtr > currentRobotNodes;
std::vector < VirtualRobot::RobotNodeSetPtr > robotNodeSets;
- VirtualRobot::ReachabilitySpacePtr reachSpace;
+ VirtualRobot::ReachabilityPtr reachSpace;
VirtualRobot::RobotNodePtr currentRobotNode;
/*
diff --git a/VirtualRobot/examples/stability/stabilityWindow.cpp b/VirtualRobot/examples/stability/stabilityWindow.cpp
index 9c2964030..0eeb08bbd 100644
--- a/VirtualRobot/examples/stability/stabilityWindow.cpp
+++ b/VirtualRobot/examples/stability/stabilityWindow.cpp
@@ -1,7 +1,7 @@
#include "stabilityWindow.h"
#include "VirtualRobot/EndEffector/EndEffector.h"
-#include "VirtualRobot/ReachabilitySpace.h"
+#include "VirtualRobot/Workspace/Reachability.h"
#include "VirtualRobot/XML/RobotIO.h"
#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h"
#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h"
--
GitLab