From da8d8061fb39ef779d21fc27e76914c6360328d1 Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Thu, 29 Mar 2012 08:07:24 +0000
Subject: [PATCH] Adding GraspRrt planner and several helper classes. Starting
with a GraspRrt demo. Improving RrtVisualization.
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@226 042f3d55-54a8-47e9-b7fb-15903f145c44
---
GraspPlanning/CMakeLists.txt | 4 +-
.../GraspPlanner/GenericGraspPlanner.cpp | 2 +-
GraspPlanning/GraspPlanner/GraspPlanner.h | 2 +-
.../GraspQuality/GraspQualityMeasure.cpp | 106 +-
.../GraspQuality/GraspQualityMeasure.h | 41 +-
.../GraspQualityMeasureWrenchSpace.cpp | 6 +-
.../GraspQualityMeasureWrenchSpace.h | 4 +-
.../GraspPlanner/GraspPlannerWindow.cpp | 4 +-
.../GraspQuality/GraspQualityWindow.cpp | 4 +-
MotionPlanning/ApproachDiscretization.cpp | 219 ++++
MotionPlanning/ApproachDiscretization.h | 116 ++
MotionPlanning/CMakeLists.txt | 4 +
MotionPlanning/CSpace/CSpaceNode.h | 1 +
MotionPlanning/Planner/GraspIkRrt.cpp | 2 +-
MotionPlanning/Planner/GraspRrt.cpp | 1009 +++++++++++++++++
MotionPlanning/Planner/GraspRrt.h | 267 +++++
MotionPlanning/Planner/MotionPlanner.cpp | 3 +-
MotionPlanning/Planner/Rrt.cpp | 1 -
MotionPlanning/Planner/Rrt.h | 6 +-
MotionPlanning/Saba.h | 4 +
.../CoinRrtWorkspaceVisualization.cpp | 88 +-
.../CoinRrtWorkspaceVisualization.h | 35 +-
.../RrtWorkspaceVisualization.cpp | 81 +-
.../Visualization/RrtWorkspaceVisualization.h | 50 +-
MotionPlanning/examples/CMakeLists.txt | 4 +
.../examples/GraspRRT/CMakeLists.txt | 26 +
MotionPlanning/examples/GraspRRT/GraspRrt.ui | 608 ++++++++++
.../examples/GraspRRT/GraspRrtDemo.cpp | 119 ++
.../examples/GraspRRT/GraspRrtWindow.cpp | 749 ++++++++++++
.../examples/GraspRRT/GraspRrtWindow.h | 152 +++
.../examples/GraspRRT/scenes/planning.xml | 145 +++
MotionPlanning/examples/IKRRT/IKRRTWindow.cpp | 4 +-
.../examples/RrtGui/RrtGuiWindow.cpp | 4 +-
33 files changed, 3598 insertions(+), 272 deletions(-)
create mode 100644 MotionPlanning/ApproachDiscretization.cpp
create mode 100644 MotionPlanning/ApproachDiscretization.h
create mode 100644 MotionPlanning/Planner/GraspRrt.cpp
create mode 100644 MotionPlanning/Planner/GraspRrt.h
create mode 100644 MotionPlanning/examples/GraspRRT/CMakeLists.txt
create mode 100644 MotionPlanning/examples/GraspRRT/GraspRrt.ui
create mode 100644 MotionPlanning/examples/GraspRRT/GraspRrtDemo.cpp
create mode 100644 MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
create mode 100644 MotionPlanning/examples/GraspRRT/GraspRrtWindow.h
create mode 100644 MotionPlanning/examples/GraspRRT/scenes/planning.xml
diff --git a/GraspPlanning/CMakeLists.txt b/GraspPlanning/CMakeLists.txt
index 67c88b427..c796d72f8 100644
--- a/GraspPlanning/CMakeLists.txt
+++ b/GraspPlanning/CMakeLists.txt
@@ -39,7 +39,7 @@ Visualization/ConvexHullVisualization.h
${GRASPSTUDIO_SimoxDir}/VirtualRobot/definesVR.h
)
-if (SIMOX_USE_COIN_VISUALIZATION)
+if (SIMOX_VISUALIZATION AND SIMOX_USE_COIN_VISUALIZATION)
SET(SOURCES
${SOURCES}
Visualization/CoinVisualization/CoinConvexHullVisualization.cpp
@@ -49,7 +49,7 @@ if (SIMOX_USE_COIN_VISUALIZATION)
${INCLUDES}
Visualization/CoinVisualization/CoinConvexHullVisualization.h
)
-endif (SIMOX_USE_COIN_VISUALIZATION)
+endif ()
INCLUDE_DIRECTORIES(${GRASPSTUDIO_VirtualRobotDir}/..)
diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
index 82a93c3c3..6e1191936 100644
--- a/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
+++ b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
@@ -1,5 +1,5 @@
#include "GenericGraspPlanner.h"
-#include <VirtualRobot/Grasp.h>
+#include <VirtualRobot/Grasping/Grasp.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <iostream>
diff --git a/GraspPlanning/GraspPlanner/GraspPlanner.h b/GraspPlanning/GraspPlanner/GraspPlanner.h
index 91fe8bf10..526db3f94 100644
--- a/GraspPlanning/GraspPlanner/GraspPlanner.h
+++ b/GraspPlanning/GraspPlanner/GraspPlanner.h
@@ -26,7 +26,7 @@
#include "GraspStudio.h"
#include <VirtualRobot/EndEffector/EndEffector.h>
#include <VirtualRobot/SceneObject.h>
-#include <VirtualRobot/GraspSet.h>
+#include <VirtualRobot/Grasping/GraspSet.h>
#include <vector>
namespace GraspStudio
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp b/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp
index d45881f87..9366bf98a 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp
@@ -24,85 +24,15 @@ namespace GraspStudio
GraspQualityMeasure::GraspQualityMeasure(VirtualRobot::SceneObjectPtr object, float unitForce, float frictionConeCoeff, int frictionConeSamples )
- : object(object), unitForce(unitForce),frictionCoeff(frictionConeCoeff),frictionConeSamples(frictionConeSamples)
+ : VirtualRobot::BasicGraspQualityMeasure(object), unitForce(unitForce),frictionCoeff(frictionConeCoeff),frictionConeSamples(frictionConeSamples)
{
- THROW_VR_EXCEPTION_IF(!object,"Need an object");
- THROW_VR_EXCEPTION_IF(!object->getCollisionModel(),"Need an object with collision model");
- THROW_VR_EXCEPTION_IF(!object->getCollisionModel()->getTriMeshModel(),"Need an object with trimeshmodel");
-
coneGenerator.reset(new ContactConeGenerator(frictionConeSamples, frictionCoeff, unitForce));
-
- //Member variable representing Grasp Quality ranging from 1 to 0
- graspQuality = 0.0;
- verbose = false;
- objectLength = 0.0f;
-
- centerOfModel = object->getCollisionModel()->getTriMeshModel()->getCOM();
-
- Eigen::Vector3f minS,maxS;
- object->getCollisionModel()->getTriMeshModel()->getSize(minS,maxS);
- maxS = maxS - minS;
- objectLength = maxS(0);
- if (maxS(1)>objectLength)
- objectLength = maxS(1);
- if (maxS(2)>objectLength)
- objectLength = maxS(2);
}
GraspQualityMeasure::~GraspQualityMeasure()
{
}
-void GraspQualityMeasure::setContactPoints( const std::vector<VirtualRobot::MathTools::ContactPoint> &contactPoints6d )
-{
- //if (contactPoints.size() < 2)
- // return;
- this->contactPoints.clear();
- this->contactPointsM.clear();
- std::vector<MathTools::ContactPoint>::const_iterator objPointsIter;
- for (objPointsIter = contactPoints.begin(); objPointsIter != contactPoints.end(); objPointsIter++)
- {
- MathTools::ContactPoint point = (*objPointsIter);
- point.p -= centerOfModel;
- point.n.normalize();
-
- this->contactPoints.push_back(point);
- }
- MathTools::convertMM2M(this->contactPoints,this->contactPointsM);
- if (verbose)
- {
- GRASPSTUDIO_INFO << ": Nr of contact points:" << this->contactPoints.size() << endl;
- }
-}
-
-void GraspQualityMeasure::setContactPoints( const std::vector<EndEffector::ContactInfo> &contactPoints )
-{
- //if (contactPoints.size() < 2)
- // return;
- this->contactPoints.clear();
- this->contactPointsM.clear();
- std::vector<EndEffector::ContactInfo>::const_iterator objPointsIter;
- for (objPointsIter = contactPoints.begin(); objPointsIter != contactPoints.end(); objPointsIter++)
- {
- MathTools::ContactPoint point;
-
- point.p = objPointsIter->contactPointObstacleLocal;
- point.p -= centerOfModel;
-
- point.n = objPointsIter->contactPointFingerLocal - objPointsIter->contactPointObstacleLocal;
- point.n.normalize();
-
- this->contactPoints.push_back(point);
- }
- VirtualRobot::MathTools::convertMM2M(this->contactPoints,this->contactPointsM);
-
- if (verbose)
- {
- GRASPSTUDIO_INFO << ": Nr of contact points:" << this->contactPoints.size() << endl;
- }
-}
-
-
bool GraspQualityMeasure::sampleObjectPoints(int nMaxFaces)
{
sampledObjectPoints.clear();
@@ -152,25 +82,6 @@ bool GraspQualityMeasure::sampleObjectPoints(int nMaxFaces)
}
-MathTools::ContactPoint GraspQualityMeasure::getContactPointsCenter()
-{
- MathTools::ContactPoint p;
- p.p.setZero();
- p.n.setZero();
- if (contactPoints.size()==0)
- return p;
- for (int i=0;i<(int)contactPoints.size();i++)
- {
- p.p += contactPoints[i].p;
- p.n += contactPoints[i].n;
-
- }
- p.p /= (float)contactPoints.size();
- p.n /= (float)contactPoints.size();
-
- return p;
-}
-
MathTools::ContactPoint GraspQualityMeasure::getSampledObjectPointsCenter()
{
MathTools::ContactPoint p;
@@ -188,26 +99,15 @@ MathTools::ContactPoint GraspQualityMeasure::getSampledObjectPointsCenter()
return p;
}
-void GraspQualityMeasure::setVerbose( bool enable )
-{
- verbose = enable;
-}
-
std::string GraspQualityMeasure::getName()
{
std::string sName("GraspQualityMeasure");
return sName;
}
-Eigen::Vector3f GraspQualityMeasure::getCoM()
+bool GraspQualityMeasure::isValid()
{
- return centerOfModel;
+ return isGraspForceClosure();
}
-VirtualRobot::SceneObjectPtr GraspQualityMeasure::getObject()
-{
- return object;
-}
-
-
} // namespace
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasure.h b/GraspPlanning/GraspQuality/GraspQualityMeasure.h
index 40be65fac..51ee95be0 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasure.h
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasure.h
@@ -28,17 +28,19 @@
#include "../ContactConeGenerator.h"
#include <VirtualRobot/EndEffector/EndEffector.h>
#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Grasping/BasicGraspQualityMeasure.h>
#include <vector>
#include <Eigen/Core>
namespace GraspStudio
{
/*!
- \brief An interface class for grasp quality algorithms.
+ \brief An interface class for grasp quality algorithms that offer a force closure test.
@see GraspQualityMeasureWrenchSpace
+ @see VirtualRobot::BasicGraspQualityMeasure
*/
-class GRASPSTUDIO_IMPORT_EXPORT GraspQualityMeasure
+class GRASPSTUDIO_IMPORT_EXPORT GraspQualityMeasure : public VirtualRobot::BasicGraspQualityMeasure
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -46,20 +48,8 @@ public:
GraspQualityMeasure(VirtualRobot::SceneObjectPtr object, float unitForce = 1.0f, float frictionConeCoeff = 0.35f, int frictionConeSamples = 8);
// destructor
- ~GraspQualityMeasure();
+ virtual ~GraspQualityMeasure();
- /*!
- setup contact information
- the contact points are normalized by subtracting the COM
- the contact normals are normalize to unit length
- */
- virtual void setContactPoints(const std::vector<VirtualRobot::EndEffector::ContactInfo> &contactPoints);
- virtual void setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint> &contactPoints6d);
-
- /*!
- Returns calculated grasp quality
- */
- virtual float getGraspQuality() = 0;
/*
Checks if grasp is force closure
@@ -70,46 +60,27 @@ public:
//! This method is used to compute a reference value that describes a perfect grasp
virtual bool calculateObjectProperties() = 0;
- //! Compute the grasp quality for the given contact points
- virtual bool calculateGraspQuality() = 0;
-
- virtual Eigen::Vector3f getCoM();
-
- virtual VirtualRobot::MathTools::ContactPoint getContactPointsCenter();
virtual VirtualRobot::MathTools::ContactPoint getSampledObjectPointsCenter();
- virtual void setVerbose(bool enable);
-
- //! Returns description of this object
virtual std::string getName();
- VirtualRobot::SceneObjectPtr getObject();
+ virtual bool isValid();
protected:
//Methods
bool sampleObjectPoints(int nMaxFaces = 400);
- //Object relevant parameters
- Eigen::Vector3f centerOfModel;
- float objectLength;
- float graspQuality;
-
//Friction cone relevant parameters
float unitForce;
float frictionCoeff;
int frictionConeSamples;
ContactConeGeneratorPtr coneGenerator;
- VirtualRobot::SceneObjectPtr object;
-
//For Object and Grasp Wrench Space Calculation
- std::vector<VirtualRobot::MathTools::ContactPoint> contactPoints; // in MM
- std::vector<VirtualRobot::MathTools::ContactPoint> contactPointsM; // converted to M
std::vector<VirtualRobot::MathTools::ContactPoint> sampledObjectPoints; // in MM
std::vector<VirtualRobot::MathTools::ContactPoint> sampledObjectPointsM; // converted to M
- bool verbose;
};
} // namespace
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp
index 1590f0d8d..a83257101 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp
@@ -65,13 +65,15 @@ float GraspQualityMeasureWrenchSpace::getGraspQuality()
bool GraspQualityMeasureWrenchSpace::isGraspForceClosure()
{
+ if (!GWSCalculated)
+ calculateGWS();
return isOriginInGWSHull();
}
-void GraspQualityMeasureWrenchSpace::calculateOWS()
+void GraspQualityMeasureWrenchSpace::calculateOWS(int samplePoints)
{
bool printAll = false;
- bool bRes = sampleObjectPoints();
+ bool bRes = sampleObjectPoints(samplePoints);
if (!bRes)
return;
//Rotate generic friction cone to align with object normals
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h
index c4d085fab..6667e34d1 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h
@@ -23,7 +23,7 @@
#ifndef __GRASP_QUALTIY_MEASURE_WRENCH_H__
#define __GRASP_QUALTIY_MEASURE_WRENCH_H__
-#include "GraspStudio.h"
+#include "../GraspStudio.h"
#include "GraspQualityMeasure.h"
#include <Eigen/Core>
@@ -87,7 +87,7 @@ public:
*/
virtual VirtualRobot::MathTools::ConvexHull6DPtr getConvexHullGWS(){return convexHullGWS;}
- void calculateOWS();
+ void calculateOWS(int samplePoints = 400);
void calculateGWS();
bool OWSExists(){return OWSCalculated;}
bool GWSExists(){return GWSCalculated;}
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
index db728532f..ba8847cdc 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
@@ -4,9 +4,9 @@
#include "VirtualRobot/EndEffector/EndEffector.h"
#include "VirtualRobot/Workspace/Reachability.h"
#include "VirtualRobot/ManipulationObject.h"
-#include "VirtualRobot/Grasp.h"
+#include "VirtualRobot/Grasping/Grasp.h"
#include "VirtualRobot/IK/GenericIKSolver.h"
-#include "VirtualRobot/GraspSet.h"
+#include "VirtualRobot/Grasping/GraspSet.h"
#include "VirtualRobot/CollisionDetection/CDManager.h"
#include "VirtualRobot/XML/ObjectIO.h"
#include "VirtualRobot/XML/RobotIO.h"
diff --git a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
index bef9149d4..5e0b957c8 100644
--- a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
+++ b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
@@ -4,9 +4,9 @@
#include "VirtualRobot/EndEffector/EndEffector.h"
#include "VirtualRobot/Workspace/Reachability.h"
#include "VirtualRobot/ManipulationObject.h"
-#include "VirtualRobot/Grasp.h"
+#include "VirtualRobot/Grasping/Grasp.h"
#include "VirtualRobot/IK/GenericIKSolver.h"
-#include "VirtualRobot/GraspSet.h"
+#include "VirtualRobot/Grasping/GraspSet.h"
#include "VirtualRobot/CollisionDetection/CDManager.h"
#include "VirtualRobot/XML/ObjectIO.h"
#include "VirtualRobot/XML/RobotIO.h"
diff --git a/MotionPlanning/ApproachDiscretization.cpp b/MotionPlanning/ApproachDiscretization.cpp
new file mode 100644
index 000000000..04bef0607
--- /dev/null
+++ b/MotionPlanning/ApproachDiscretization.cpp
@@ -0,0 +1,219 @@
+#include "ApproachDiscretization.h"
+#include <iostream>
+#include <time.h>
+#include <algorithm>
+#include <limits.h>
+
+using namespace std;
+
+namespace Saba
+{
+
+
+ApproachDiscretization::ApproachDiscretization(float radius, int steps)
+{
+ globalPose.setIdentity();
+
+ //clock_t tStart = clock();
+ sphereGenerator.reset(new VirtualRobot::SphereApproximator());
+ sphereGenerator->generateGraph(sphere, VirtualRobot::SphereApproximator::eOctahedron, steps, radius);
+ //buildIVModel();
+ //clock_t tEnd = clock();
+ //long diffClock = (long)(((float)(tEnd - tStart) / (float)CLOCKS_PER_SEC) * 1000.0);
+ //cout << __FUNCTION__ << " Created Sphere in " << diffClock << "ms with " << sphere.m_Vertices.size() << " vertices and " << sphere.faces.size() << " faces "<< endl;
+}
+
+ApproachDiscretization::~ApproachDiscretization()
+{
+}
+
+/*
+void ApproachDiscretization::buildIVModel()
+{
+ if (m_pIVModel)
+ m_pIVModel->unref();
+ m_pIVModel = new SoSeparator();
+ m_pIVModel->ref();
+
+ SoMatrixTransform *pTrans = new SoMatrixTransform();
+ pTrans->matrix.setValue(globalPose);
+ m_pIVModel->addChild(pTrans);
+ SoSeparator* pSphereIV = m_pSphereGenerator->generateIVModel(sphere,0.0f);
+ m_pIVModel->addChild(pSphereIV);
+}
+*/
+
+Eigen::Matrix4f ApproachDiscretization::getGlobalPose() const
+{
+ return globalPose;
+}
+
+void ApproachDiscretization::setGlobalPose(const Eigen::Matrix4f &pose)
+{
+ globalPose.setIdentity();
+ globalPose.block(0,3,3,1) = pose.block(0,3,3,1);
+}
+
+int ApproachDiscretization::getNearestVertexId(const Eigen::Matrix4f &pose)
+{
+ return getNearestVertexIdVec(pose.block(0,3,3,1));
+}
+
+int ApproachDiscretization::getNearestVertexIdVec(const Eigen::Vector3f &pos)
+{
+ Eigen::Vector3f dir = pos-globalPose.block(0,3,3,1);
+ if (dir.norm()<1e-8)
+ return 0;
+ int nIndex = sphereGenerator->findVertex(dir,10000.0f,sphere.vertices);
+
+ return nIndex;
+}
+
+int ApproachDiscretization::getNearestFaceId(const Eigen::Matrix4f &pose)
+{
+ return getNearestFaceIdVec(pose.block(0,3,3,1));
+}
+
+int ApproachDiscretization::getNearestFaceIdVec(const Eigen::Vector3f &pos)
+{
+ Eigen::Vector3f dir = pos-globalPose.block(0,3,3,1);
+ if (dir.norm()<1e-8)
+ return 0;
+ int nIndex = sphereGenerator->findVertex(dir,10000.0f,sphere.vertices);
+
+ Eigen::Vector3f zeroPt,storeIntPoint;
+ zeroPt.setZero();
+ VirtualRobot::SphereApproximator::FaceIndex faceIndx = sphere.mapVerticeIndxToFaceIndx[nIndex];
+ for (int i=0; i<(int)faceIndx.faceIds.size();i++)
+ {
+ VirtualRobot::MathTools::TriangleFace f = sphere.faces[faceIndx.faceIds[i]];
+ //if (sphereGenerator->check_intersect_tri(sphere.vertices[f.n1],sphere.m_Vertices[f.m_n2],sphere.m_Vertices[f.m_n3],zeroPt,Pose1,storeIntPoint))
+ if (sphereGenerator->check_intersect_tri(sphere.vertices[f.id1],sphere.vertices[f.id2],sphere.vertices[f.id3],zeroPt,dir,storeIntPoint))
+ {
+ //MarkFace(FaceIndx.m_faceIds[i],true);
+ return faceIndx.faceIds[i];
+ }
+ }
+ cout << __FUNCTION__ << " No face found ?!" << endl;
+ return -1;
+}
+
+void ApproachDiscretization::removeCSpaceNode(int faceId, CSpaceNodePtr node)
+{
+ if (faceId<0 || faceId>(int)sphere.faces.size())
+ {
+ cout << __FUNCTION__ << "wrong face ID :" << faceId << endl;
+ return;
+ }
+ if (faceIdToCSpaceNodesMapping.find(faceId) != faceIdToCSpaceNodesMapping.end())
+ {
+ std::vector<CSpaceNodePtr>::iterator iter = find(faceIdToCSpaceNodesMapping[faceId].cspaceNodes.begin(),faceIdToCSpaceNodesMapping[faceId].cspaceNodes.end(),node);
+ if (iter == faceIdToCSpaceNodesMapping[faceId].cspaceNodes.end())
+ {
+ cout << __FUNCTION__ << " Warning: Node " << node->ID << " is not mapped to face with id " << faceId << endl;
+ } else
+ {
+ faceIdToCSpaceNodesMapping[faceId].cspaceNodes.erase(iter);
+ }
+ } else
+ {
+ cout << __FUNCTION__ << " Warning: Node " << node->ID << " is not mapped to face with id " << faceId << endl;
+ }
+}
+
+void ApproachDiscretization::removeCSpaceNode(const Eigen::Vector3f &cartPos, CSpaceNodePtr node)
+{
+ if (!node)
+ {
+ cout << __FUNCTION__ << "NULL data..." << endl;
+ return;
+ }
+ int faceId = getNearestFaceIdVec(cartPos);
+ removeCSpaceNode(faceId,node);
+}
+
+void ApproachDiscretization::addCSpaceNode(int faceId, CSpaceNodePtr node)
+{
+ if (faceId<0 || faceId>(int)sphere.faces.size())
+ {
+ cout << __FUNCTION__ << "wrong face ID :" << faceId << endl;
+ return;
+ }
+ if (faceIdToCSpaceNodesMapping.find(faceId) != faceIdToCSpaceNodesMapping.end())
+ {
+ std::vector<CSpaceNodePtr>::iterator iter = find(faceIdToCSpaceNodesMapping[faceId].cspaceNodes.begin(),faceIdToCSpaceNodesMapping[faceId].cspaceNodes.end(),node);
+ if (iter != faceIdToCSpaceNodesMapping[faceId].cspaceNodes.end())
+ {
+ cout << __FUNCTION__ << " Warning: Node " << node->ID << " is already mapped to face with id " << faceId << endl;
+ } else
+ {
+ faceIdToCSpaceNodesMapping[faceId].cspaceNodes.push_back(node);
+ }
+ } else
+ {
+ faceIdToCSpaceNodesMapping[faceId].count = 0;
+ faceIdToCSpaceNodesMapping[faceId].cspaceNodes.push_back(node);
+ activeFaces.push_back(faceId);
+ }
+}
+
+void ApproachDiscretization::addCSpaceNode(const Eigen::Vector3f &pos, CSpaceNodePtr node)
+{
+ if (!node)
+ {
+ cout << __FUNCTION__ << "NULL data..." << endl;
+ return;
+ }
+ int faceId = getNearestFaceIdVec(pos);
+ addCSpaceNode(faceId,node);
+}
+
+void ApproachDiscretization::clearCSpaceNodeMapping()
+{
+ faceIdToCSpaceNodesMapping.clear();
+ activeFaces.clear();
+}
+
+CSpaceNodePtr ApproachDiscretization::getGoodRatedNode(int loops)
+{
+ int nSize = (int)activeFaces.size();
+ if (nSize==0)
+ return CSpaceNodePtr();
+
+ int nBestRanking = INT_MAX;
+ int nBestFace = -1;
+ int nRandFaceId =0;
+ int nLoopCount = 0;
+ std::map<int,CSpaceNodeMapping>::iterator iter;
+ while (nLoopCount<loops)
+ {
+ nRandFaceId = activeFaces[(rand()%nSize)];
+ if (faceIdToCSpaceNodesMapping[nRandFaceId].count < nBestRanking && faceIdToCSpaceNodesMapping[nRandFaceId].cspaceNodes.size() > 0)
+ {
+ nBestRanking = faceIdToCSpaceNodesMapping[nRandFaceId].count;
+ nBestFace = nRandFaceId;
+ if (nBestRanking==0)
+ break;
+ }
+ nLoopCount++;
+ }
+ if (nBestFace<0)
+ return CSpaceNodePtr();
+
+ // increase ranking
+ faceIdToCSpaceNodesMapping[nBestFace].count++;
+
+ // get random cspace node
+ int nNodes = (int)faceIdToCSpaceNodesMapping[nBestFace].cspaceNodes.size();
+ if (nNodes<=0)
+ return CSpaceNodePtr();
+ int nRandNode = rand()%nNodes;
+ CSpaceNodePtr node = faceIdToCSpaceNodesMapping[nBestFace].cspaceNodes[nRandNode];
+
+ // remove node from list
+ removeCSpaceNode(nBestFace,node);
+
+ return node;
+}
+
+}
diff --git a/MotionPlanning/ApproachDiscretization.h b/MotionPlanning/ApproachDiscretization.h
new file mode 100644
index 000000000..c907231dd
--- /dev/null
+++ b/MotionPlanning/ApproachDiscretization.h
@@ -0,0 +1,116 @@
+/**
+* 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 Saba
+* @author Nikolaus Vahrenkamp
+* @copyright 2012 Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+
+#ifndef _SABA_APPROACH_DISCRETIZATION_H_
+#define _SABA_APPROACH_DISCRETIZATION_H_
+
+#include "Saba.h"
+
+#include <vector>
+#include <map>
+#include <float.h>
+#include <VirtualRobot/SphereApproximator.h>
+
+#include "CSpace/CSpaceNode.h"
+
+namespace Saba
+{
+
+/*!
+* This discretization class can be used to encode approach directions.
+* Internally all approach directions are mapped onto the surface of a discretized sphere.
+* Each face of the PoseRelationSphere holds a list with corresponding RRT nodes,
+* lying in the direction that is covered by the triangle.
+* The method getGoodRatedNode can be used to retrieve RRT-nodes for which the approach directions are uniformly sampled.
+* (further details can be found in "Integrated Grasp and Motion Planning", ICRA 2010, Vahrenkamp et al.)
+*
+*/
+
+class SABA_IMPORT_EXPORT ApproachDiscretization
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ ApproachDiscretization(float radius = 100.0f, int steps = 3);
+ virtual ~ApproachDiscretization();
+
+ Eigen::Matrix4f getGlobalPose() const;
+
+ /*!
+ Set the location (only position is used)
+ */
+ void setGlobalPose(const Eigen::Matrix4f &pose);
+
+ /*!
+ Returns Id of the surface vertex which is nearest to line through center of sphere and pose
+ \param pose in global coordinate system, (center of sphere is at globalPose)
+ */
+ int getNearestVertexId(const Eigen::Matrix4f &pose);
+ int getNearestVertexIdVec(const Eigen::Vector3f &pos);
+
+ /*!
+ Returns Id of the surface face which is nearest to line through center of sphere and pose
+ \param pose in global coordinate system, (center of sphere is at globalPose)
+ */
+ int getNearestFaceId(const Eigen::Matrix4f &pose);
+ int getNearestFaceIdVec(const Eigen::Vector3f &pos);
+
+ void removeCSpaceNode(const Eigen::Vector3f &cartPose, CSpaceNodePtr node);
+ void removeCSpaceNode(int faceId, CSpaceNodePtr node);
+
+ /*!
+ Add a node of the RRT with corresponding Cartesian pose. The pose should be the resulting EEF's pose.
+ */
+ void addCSpaceNode(const Eigen::Vector3f &cartPos, CSpaceNodePtr node);
+ void addCSpaceNode(int faceId, CSpaceNodePtr node);
+
+ //! Reset
+ void clearCSpaceNodeMapping();
+
+ /*!
+ Return a RRT node, so that the corresponding Cartesian poses are uniformly sampled over the sphere.
+ Removes the selected RRT node from the internal data.
+ This method just checks loops faces of the sphere, so that the result is approximated.
+ */
+ CSpaceNodePtr getGoodRatedNode(int loops = 30);
+
+
+private:
+
+ struct CSpaceNodeMapping
+ {
+ int count;
+ std::vector<CSpaceNodePtr> cspaceNodes;
+ };
+
+ Eigen::Matrix4f globalPose;
+ VirtualRobot::SphereApproximator::SphereApproximation sphere;
+ VirtualRobot::SphereApproximatorPtr sphereGenerator;
+
+ std::map<int,CSpaceNodeMapping> faceIdToCSpaceNodesMapping;
+ std::vector<int> activeFaces;
+};
+
+}
+
+#endif /* _SABA_APPROACH_DISCRETIZATION_H_ */
diff --git a/MotionPlanning/CMakeLists.txt b/MotionPlanning/CMakeLists.txt
index a3a1aba2f..77498960b 100644
--- a/MotionPlanning/CMakeLists.txt
+++ b/MotionPlanning/CMakeLists.txt
@@ -22,9 +22,11 @@ Planner/MotionPlanner.cpp
Planner/Rrt.cpp
Planner/BiRrt.cpp
Planner/GraspIkRrt.cpp
+Planner/GraspRrt.cpp
Visualization/RrtWorkspaceVisualization.cpp
PostProcessing/PathProcessor.cpp
PostProcessing/ShortcutProcessor.cpp
+ApproachDiscretization.cpp
)
SET(INCLUDES
@@ -40,9 +42,11 @@ Planner/MotionPlanner.h
Planner/Rrt.h
Planner/BiRrt.h
Planner/GraspIkRrt.h
+Planner/GraspRrt.h
Visualization/RrtWorkspaceVisualization.h
PostProcessing/PathProcessor.h
PostProcessing/ShortcutProcessor.h
+ApproachDiscretization.h
${SABA_SimoxDir}/VirtualRobot/definesVR.h
)
diff --git a/MotionPlanning/CSpace/CSpaceNode.h b/MotionPlanning/CSpace/CSpaceNode.h
index 67788769c..f1a58d4d3 100644
--- a/MotionPlanning/CSpace/CSpaceNode.h
+++ b/MotionPlanning/CSpace/CSpaceNode.h
@@ -53,6 +53,7 @@ public:
bool allocated;
// optional
+ int status;
float obstacleDistance; //!< work space distance to obstacles (-1 if the dist was not calculated)
float dynDomRadius; //!< radius for this node (used by dynamic domain RRTs)
std::vector<CSpaceNodePtr> children; //!< children of this node
diff --git a/MotionPlanning/Planner/GraspIkRrt.cpp b/MotionPlanning/Planner/GraspIkRrt.cpp
index b0f042325..4ae7841c0 100644
--- a/MotionPlanning/Planner/GraspIkRrt.cpp
+++ b/MotionPlanning/Planner/GraspIkRrt.cpp
@@ -6,7 +6,7 @@
#include "../CSpace/CSpacePath.h"
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/GraspSet.h>
+#include <VirtualRobot/Grasping/GraspSet.h>
#include <time.h>
#include <boost/pointer_cast.hpp>
diff --git a/MotionPlanning/Planner/GraspRrt.cpp b/MotionPlanning/Planner/GraspRrt.cpp
new file mode 100644
index 000000000..3fbe25e9e
--- /dev/null
+++ b/MotionPlanning/Planner/GraspRrt.cpp
@@ -0,0 +1,1009 @@
+#include "GraspRrt.h"
+
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/EndEffector/EndEffector.h>
+#include <VirtualRobot/EndEffector/EndEffectorActor.h>
+#include <VirtualRobot/Grasping/Grasp.h>
+#include <VirtualRobot/Grasping/GraspSet.h>
+#include <VirtualRobot/Grasping/BasicGraspQualityMeasure.h>
+#include <algorithm>
+#include <float.h>
+#include <time.h>
+
+using namespace std;
+
+namespace Saba
+{
+
+
+GraspRrt::GraspRrt( CSpaceSampledPtr cspace,
+ VirtualRobot::EndEffectorPtr eef,
+ VirtualRobot::ObstaclePtr object,
+ VirtualRobot::BasicGraspQualityMeasurePtr measure,
+ VirtualRobot::SceneObjectSetPtr graspCollisionObjects,
+ float probabGraspHypothesis,
+ float graspQualityMinScore
+ )
+: Rrt(cspace), probabGraspHypothesis(probabGraspHypothesis), graspQualityMinScore(graspQualityMinScore)
+{
+ plannerInitialized = false;
+ name ="GraspRrt";
+ THROW_VR_EXCEPTION_IF(!cspace || !object || !eef || !cspace->getRobotNodeSet() || !measure, "NULL data");
+ rns = cspace->getRobotNodeSet();
+ targetObject = object;
+ this->eef = eef;
+ this->graspQualityMeasure = measure;
+ if (graspCollisionObjects)
+ {
+ this->graspCollisionObjects = graspCollisionObjects->clone();
+ } else
+ {
+ this->graspCollisionObjects.reset (new VirtualRobot::SceneObjectSet("GraspRrtSceneObject",object->getCollisionChecker()));
+ }
+ if (!this->graspCollisionObjects->hasSceneObject(targetObject))
+ this->graspCollisionObjects->addSceneObject(targetObject);
+
+
+
+ cartSamplingPosStepSize = 10.0f;
+ cartSamplingOriStepSize = (float)M_PI / 18.0f; // == 10 degree
+
+ THROW_VR_EXCEPTION_IF (dimension<=0,"Zero dim?");
+ THROW_VR_EXCEPTION_IF(!eef->getGCP(),"No gcp..");
+
+ performanceMeasure.setupTimeMS = 0.0f;
+ performanceMeasure.planningTimeMS = 0.0f;
+ performanceMeasure.moveTowardGraspPosesTimeMS = 0.0f;
+ performanceMeasure.numberOfMovementsTowardGraspPose = 0;
+ performanceMeasure.scoreGraspTimeMS = 0.0f;
+ performanceMeasure.numberOfGraspScorings = 0;
+ performanceMeasure.rrtTimeMS = 0.0f;
+ foundSolution = false;
+ verbose = false;
+#ifdef _DEBUG
+ verbose = true;
+#endif
+ targetObjectPosition = targetObject->getGlobalPose().block(0,3,3,1);
+ // todo: is this value a good guess?
+ tryGraspsDistance2 = 600.0f * 600.0f;
+
+ gcpOject = VirtualRobot::Obstacle::createBox(1.0f,1.0f,1.0f,VirtualRobot::VisualizationFactory::Color::Red(),"",eef->getCollisionChecker());
+}
+
+GraspRrt::~GraspRrt()
+{
+ reset();
+ gcpOject.reset();
+}
+
+
+void GraspRrt::reset()
+{
+ Rrt::reset();
+ if (poseSphere)
+ poseSphere.reset();
+ performanceMeasure.setupTimeMS = 0.0f;
+ performanceMeasure.planningTimeMS = 0.0f;
+ performanceMeasure.moveTowardGraspPosesTimeMS = 0.0f;
+ performanceMeasure.numberOfMovementsTowardGraspPose = 0;
+ performanceMeasure.scoreGraspTimeMS = 0.0f;
+ performanceMeasure.numberOfGraspScorings = 0;
+ performanceMeasure.rrtTimeMS = 0.0f;
+ plannerInitialized = false;
+ foundSolution = false;
+ colChecksOverall = 0;
+
+ mapConfigTcp.clear();
+}
+
+bool GraspRrt::init()
+{
+ performanceMeasure.setupTimeMS = 0.0f;
+ performanceMeasure.planningTimeMS = 0.0f;
+ performanceMeasure.moveTowardGraspPosesTimeMS = 0.0f;
+ performanceMeasure.numberOfMovementsTowardGraspPose = 0;
+ performanceMeasure.scoreGraspTimeMS = 0.0f;
+ performanceMeasure.numberOfGraspScorings = 0;
+ performanceMeasure.rrtTimeMS = 0.0f;
+ foundSolution = false;
+ colChecksOverall = 0;
+
+ clock_t timeStart = clock();
+ solution.reset();
+
+ if (!startNode)
+ {
+ VR_ERROR << ": not initialized correctly..." << endl;
+ return false;
+ }
+
+ cycles = 0;
+
+ stopSearch = false;
+
+ workSpaceSamplingCount = 0;
+
+ grasps.clear();
+
+ poseSphere.reset(new ApproachDiscretization());
+
+ poseSphere->setGlobalPose(targetObject->getGlobalPose());
+
+ targetObjectPosition = targetObject->getGlobalPose().block(0,3,3,1);
+
+ // init jacobian solver
+
+ diffIK.reset(new VirtualRobot::DifferentialIK(rns));
+
+ plannerInitialized = true;
+ clock_t timeEnd = clock();
+ performanceMeasure.setupTimeMS = ((float)(timeEnd - timeStart) / (float)CLOCKS_PER_SEC) * 1000.0f;
+ return true;
+}
+
+bool GraspRrt::doPlanningCycle()
+{
+ static const float randMult = (float)(1.0/(double)(RAND_MAX));
+ if (!plannerInitialized)
+ return false;
+
+ int colChecksStart = cspace->performaceVars_collisionCheck;
+
+ // CHOOSE A RANDOM CONFIGURATION
+ // check if we want to go to the goal directly or extend randomly
+ bool bDoNormalConnect = true;
+ float r = (float)rand() * randMult;
+ if (r <= probabGraspHypothesis)
+ {
+ bDoNormalConnect = false;
+
+ extendGraspStatus = connectRandomGraspPositionJacobian();
+ switch (extendGraspStatus)
+ {
+ case eFatalError:
+ stopSearch = true;
+ break;
+ case eGraspablePoseReached:
+ cout << endl << __FUNCTION__ << " -- FOUND A GRASPABLE POSITION (but score is not high enough) -- " << endl;
+ if (grasps.size()<=0)
+ {
+ cout << __FUNCTION__ << ": Error, no grasp results stored..." << endl;
+ } else
+ {
+ GraspInfo graspInfo = grasps[grasps.size()-1];
+ printGraspInfo(graspInfo);
+ }
+ break;
+ case eGoalReached:
+ cout << endl << __FUNCTION__ << " -- FOUND GOAL PATH (CONNECTION TO GRASPING POSITION VIA INV JACOBIAN) -- " << endl;
+ if (grasps.size()<=0)
+ {
+ cout << __FUNCTION__ << ": Error, no grasp results stored..." << endl;
+ stopSearch = true;
+
+ } else
+ {
+ GraspInfo graspInfo = grasps[grasps.size()-1];
+ printGraspInfo(graspInfo);
+ goalNode = tree->getNode(graspInfo.rrtNodeId);
+ foundSolution = true;
+ }
+ break;
+ default:
+ // nothing to to
+ break;
+ }
+ }
+
+ if (bDoNormalConnect)
+ {
+ cspace->getRandomConfig(tmpConfig);
+ ExtensionResult extendStatus = connectComplete(tmpConfig,tree,lastAddedID);
+
+ if (extendStatus == eError)
+ stopSearch = true;
+ }
+ cycles++;
+
+ colChecksOverall += cspace->performaceVars_collisionCheck - colChecksStart;
+
+ return true;
+}
+
+Rrt::ExtensionResult GraspRrt::connectComplete(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID)
+{
+ // NEAREST NEIGHBOR OF RANDOM CONFIGURATION
+ CSpaceNodePtr nn = tree->getNearestNeighbor(c);
+
+ SABA_ASSERT (nn);
+
+ // CHECK PATH FOR COLLISIONS AND VALID NODES
+ if (!cspace->isPathValid(nn->configuration,c))
+ {
+ return eFailed; // CONNECT FAILS
+ }
+
+ // ADD IT TO RRT TREE
+ if (!tree->appendPath(nn,c,&storeLastAddedID))
+ return Rrt::eError;
+
+ // add nodes to pose sphere and store TCP poses
+ processNodes(nn->ID,storeLastAddedID);
+
+ return Rrt::eSuccess; // REACHED
+}
+
+
+bool GraspRrt::plan(bool bQuiet)
+{
+ if (!plannerInitialized)
+ if (!init())
+ return false;
+
+ cspace->getRobot()->setUpdateVisualization(false);
+
+ clock_t startClock = clock();
+ time_t startTime = time(NULL);
+
+ bool bStopLoop = false;
+ // the planning loop
+ do
+ {
+ doPlanningCycle();
+ if (stopSearch)
+ bStopLoop = true;
+ //if (!m_bEndlessMode)
+ //{
+ if (foundSolution)
+ bStopLoop = true;
+ //}
+ if (cycles>maxCycles)
+ bStopLoop = true;
+ } while (!bStopLoop);
+
+ time_t endTime = time(NULL);
+ clock_t endClock = clock();
+
+ //long diffClock = (long)(((float)(endClock - startClock) / (float)CLOCKS_PER_SEC) * 1000.0);
+ //long diffTime = (long)((float)(endTime - startTime) * 1000.0);
+ performanceMeasure.planningTimeMS = ((float)(endClock - startClock) / (float)CLOCKS_PER_SEC) * 1000.0f;
+ planningTime = performanceMeasure.planningTimeMS;
+
+
+ // calculate the RRT buildup time (the time for moving toward the goals includes the scoring time)
+ performanceMeasure.rrtTimeMS = performanceMeasure.planningTimeMS - performanceMeasure.moveTowardGraspPosesTimeMS;
+ // until now the time for moving toward goal includes the grasp scoring time, so subtract it
+ performanceMeasure.moveTowardGraspPosesTimeMS -= performanceMeasure.scoreGraspTimeMS;
+
+ if (!bQuiet)
+ printPerformanceResults();
+
+ cspace->getRobot()->setUpdateVisualization(true);
+ if (foundSolution)
+ {
+ if (!bQuiet)
+ SABA_INFO << "Found path in rrt with " << cycles << " cycles."<< std::endl;
+ return createSolution();
+ }
+
+ // something went wrong...
+ if (cycles>=maxCycles)
+ {
+ if (!bQuiet)
+ SABA_ERROR << " maxCycles exceeded..." << std::endl;
+ }
+ if (stopSearch)
+ {
+ if (!bQuiet)
+ SABA_ERROR << " search was stopped..." << std::endl;
+ }
+
+ return false;
+}
+
+void GraspRrt::printPerformanceResults()
+{
+ cout << endl << "=================== RESULTS ===================" << endl;
+ std::cout << "Needed " << performanceMeasure.setupTimeMS << " ms for setup." << std::endl;
+ std::cout << "Needed " << performanceMeasure.planningTimeMS << " ms for the complete planning process." << std::endl;
+
+ std::cout << "Needed " << performanceMeasure.rrtTimeMS << " ms for building up the RRT." << std::endl;
+ std::cout << "Needed " << performanceMeasure.moveTowardGraspPosesTimeMS << " ms for moving toward the grasp goals." << std::endl;
+ std::cout << "Needed " << performanceMeasure.scoreGraspTimeMS << " ms for scoring the grasps." << std::endl;
+ cout << " Nr of grasp tries: " << performanceMeasure.numberOfMovementsTowardGraspPose << endl;
+ if (performanceMeasure.numberOfMovementsTowardGraspPose>0)
+ cout << " Avg Time for moving to grasping test position (without checking the grasp score):" << performanceMeasure.moveTowardGraspPosesTimeMS/(float)performanceMeasure.numberOfMovementsTowardGraspPose << " ms " << endl;
+ cout << " Nr of grasp scorings: " << performanceMeasure.numberOfGraspScorings << endl;
+ if (performanceMeasure.numberOfGraspScorings>0)
+ cout << " Avg Time for scoring a grasp:" << performanceMeasure.scoreGraspTimeMS/(float)performanceMeasure.numberOfGraspScorings << " ms " << endl;
+
+ std::cout << "Created " << tree->getNrOfNodes() << " nodes." << std::endl;
+ std::cout << "Collision Checks: " << colChecksOverall << std::endl;
+ cout << "=================== RESULTS ===================" << endl << endl;
+}
+
+bool GraspRrt::setStart(const Eigen::VectorXf &startVec)
+{
+ if (!tree)
+ return false;
+ if (!Rrt::setStart(startVec) || !startNode)
+ {
+ SABA_ERROR << " error initializing start node..." << endl;
+ return false;
+ }
+ startNode->status = 0;
+ processNode(startNode);
+
+ if (verbose)
+ {
+ cout << "GraspRrt::setStart" << endl;
+ cout << "startVec:";
+ for (unsigned int i = 0; i < dimension; i++)
+ {
+ cout << startNode->configuration[i] << ",";
+ }
+ cout << endl;
+ }
+ return true;
+}
+
+
+bool GraspRrt::setGoal(const Eigen::VectorXf &c)
+{
+ THROW_VR_EXCEPTION ("Not allowed here, goal configurations are sampled during planning..");
+ return false;
+}
+
+
+bool GraspRrt::calculateGlobalGraspPose(const Eigen::VectorXf &c, Eigen::Matrix4f &storeGoal)
+{
+ Eigen::Vector3f P1;
+ Eigen::Vector3f P2;
+ int nId1,nId2;
+ Eigen::Matrix4f mMat,mRotMat;
+
+ // set gcp object
+ rns->setJointValues(c);
+ gcpOject->setGlobalPose(eef->getGCP()->getGlobalPose());
+
+ // get target position (position on grasp object with shortest distance to hand)
+ double dist = targetObject->getCollisionChecker()->calculateDistance(targetObject->getCollisionModel(), gcpOject->getCollisionModel(), P1, P2, &nId1, &nId2);
+
+ // now target position in global coord system is stored in P1
+
+ // calculate target orientation
+
+ // a) get P1 in local grasping position coordinate system
+ mMat.setIdentity();
+ mMat.block(0,3,3,1) = P1;
+ mMat = eef->getGCP()->toLocalCoordinateSystem(mMat);
+
+ Eigen::Vector3f vecTarget_local;
+ Eigen::Vector3f vecZ_local;
+ vecTarget_local = mMat.block(0,3,3,1);
+ vecZ_local[0] = 0.0f;
+ vecZ_local[1] = 0.0f;
+ vecZ_local[2] = 1.0f;
+
+ //b) get angle between z axis of grasping position (vecZ_local) and target distance vector (vecTarget_local)
+ float l = vecTarget_local.norm();
+ if (l<1e-8)
+ {
+ cout << __FUNCTION__ << ":WARNING: length to target is small ... aborting " << endl;
+ return false;
+ }
+ vecTarget_local.normalize();
+ float fAngle = VirtualRobot::MathTools::getAngle(vecTarget_local,vecZ_local);
+ //cout << "Angle : " << fAngle << endl;
+
+ // c) rotation axis (is orthogonal on vecZ_local and vecTarget_local)
+ Eigen::Vector3f rotAxis_local;
+ rotAxis_local = vecZ_local.cross(vecTarget_local);
+
+ mRotMat = VirtualRobot::MathTools::axisangle2eigen4f(rotAxis_local,fAngle);
+
+ /*
+ MathHelpers::crossProduct(vecZ_local,vecTarget_local,rotAxis_local);
+ SbVec3f rotVec3f(rotAxis_local[0],rotAxis_local[1],rotAxis_local[2]);
+ SbRotation sbRot(rotVec3f,fAngle);
+ mRotMat.makeIdentity();
+ mRotMat.setRotate(sbRot);
+ */
+
+ // construct global grasp pose
+ mMat = eef->getGCP()->toGlobalCoordinateSystem(mRotMat);
+ mMat.block(0,3,3,1) = P1;
+ //mMat[3][0] = P1[0];
+ //mMat[3][1] = P1[1];
+ //mMat[3][2] = P1[2];
+
+ storeGoal = mMat;
+ return true;
+}
+
+GraspRrt::MoveArmResult GraspRrt::connectRandomGraspPositionJacobian()
+{
+ clock_t timeStart = clock();
+ // choose one node to extend
+ /*Eigen::Matrix4f mObjPose = *(m_pManipulationObject->GetGlobalPose());
+ MathTools::Eigen::Matrix4f2PosQuat(mObjPose, m_pTmpPose);
+ CSpaceNode *nnNode = findNearestNeighborCart(m_pTmpPose);
+
+ if (nnNode == NULL)
+ return eError;
+
+ // mark nn node, so it will never be selected again in findNearestNeighborCart()
+ nnNode->failurecount++;*/
+
+ CSpaceNodePtr nnNode = poseSphere->getGoodRatedNode();
+ if (!nnNode)
+ {
+ cout << __FUNCTION__ << ": No good ranked node found..." << endl;
+ return eTrapped;
+ }
+
+ // set config and compute target pos on object's surface
+ Eigen::Matrix4f goalPose;
+ if (!calculateGlobalGraspPose(nnNode->configuration,goalPose))
+ return eError;
+
+ //MathTools::Eigen::Matrix4f2PosQuat(goalPose,m_pTmpPose);
+
+ //cout << "GoalGrasp:" << grasp->GetName() << endl;
+ //cout << "pos:" << m_pTmpPose[0] << "," << m_pTmpPose[1] << "," << m_pTmpPose[2] << endl;
+ GraspRrt::MoveArmResult res = moveTowardsGoal(nnNode,goalPose,300);
+ clock_t timeEnd = clock();
+ float fMoveTime = ((float)(timeEnd - timeStart) / (float)CLOCKS_PER_SEC) * 1000.0f;
+ performanceMeasure.moveTowardGraspPosesTimeMS += fMoveTime;
+ performanceMeasure.numberOfMovementsTowardGraspPose++;
+ if (verbose)
+ cout << __FUNCTION__ << " - Time for Connect To Grasp Position:" << fMoveTime << endl;
+
+ return res;
+
+}
+
+// not needed any more
+/*CSpaceNode* GraspRrt::findNearestNeighborCart(float *pCartConf)
+{
+ if (!pCartConf || !m_pCSpace)
+ return NULL;
+
+ float minDist = FLT_MAX;
+ float actDist;
+ CSpaceNode* bestNode = NULL;
+ std::vector<CSpaceNode*> *nodes = m_pRrtCart->getNodes();
+ if (nodes->size()==0)
+ return NULL;
+ CSpaceNode *testNode;
+ unsigned int loops = 400; // TODO: this is just a random number, better things should be possible?!
+ for (unsigned int i=0; i<loops; i++)
+ {
+ int nr = rand()%((int)nodes->size());
+ testNode = (*nodes)[nr];
+ if (testNode->status==0)
+ {
+ actDist = MathHelpers::calcCartesianPoseDiff(testNode->cartPosTCP1,pCartConf);
+ if (actDist < minDist)
+ {
+ minDist = actDist;
+ bestNode = testNode;
+ return bestNode;
+ }
+ }
+ }
+ if (minDist==FLT_MAX)
+ {
+ minDist = MathHelpers::calcCartesianPoseDiff((*nodes)[0]->cartPosTCP1,pCartConf);
+ bestNode = (*nodes)[0];
+ }
+ //cout << " , minDist: " << minDist ;
+ return bestNode;
+}*/
+
+GraspRrt::MoveArmResult GraspRrt::moveTowardsGoal(CSpaceNodePtr startNode, const Eigen::Matrix4f &targetPose, int nMaxLoops)
+{
+ if (!startNode)
+ return eError;
+ CSpaceNodePtr lastExtendNode = startNode;
+ float dist;
+ bool r;
+ dist = VirtualRobot::MathTools::getCartesianPoseDiff(mapConfigTcp[lastExtendNode],targetPose);
+ int loopCount = 0;
+ float lastDist = 0;
+ float fMaxDist_GraspGoalReached = 1.0f; // 1 mm or 3 degrees
+ while (loopCount<nMaxLoops)
+ {
+ MoveArmResult res = createWorkSpaceSamplingStep(targetPose, lastExtendNode, tmpConfig);
+
+ // check result
+ switch (res)
+ {
+ // ERROR -> abort
+ case eError:
+ case eFatalError:
+ return res;
+ break;
+
+ // COLLISION -> don't apply step, but try to close hand
+ // todo: here just the eCollision_Environment is used,
+ // maybe it's better to check only the TCP collisions
+ case eCollision_Environment:
+ {
+ // check grasp score of last valid config
+ float fGraspScore = calculateGraspScore(lastExtendNode->configuration, lastExtendNode->ID, true);
+ if (fGraspScore>=graspQualityMinScore)
+ {
+ cout << __FUNCTION__ << ": found valid grasp (score:" << fGraspScore << ")" << endl;
+ return eGoalReached;
+ } else if (fGraspScore>0.0f)
+ {
+ cout << __FUNCTION__ << ": found valid grasp (score:" << fGraspScore << ")" << endl;
+ return eGraspablePoseReached;
+ } else
+ {
+ if (verbose)
+ cout << __FUNCTION__ << ": aborting moveArmToGraspPos, Collision but no graspable config " << endl;
+ return eCollision_Environment;
+ }
+ break;
+ }
+
+ // ONE STEP MOVED
+ case eMovedOneStep:
+ // check path (todo: maybe this step could be skipped, depending on the workspace step sizes)
+ if (!cspace->isPathValid(lastExtendNode->configuration,tmpConfig))
+ {
+ if (verbose)
+ cout << __FUNCTION__ << ": aborting moveArmToGraspPos, checkPath fails, todo: check GRASP of last valid config.. " << endl;
+ return eCollision_Environment;
+ }
+ // ADD IT TO RRT TREE
+ r = tree->appendPath(lastExtendNode,tmpConfig,&lastAddedID);
+
+ if (!r)
+ {
+ cout << __FUNCTION__ << ": aborting moveArmToGraspPos, appendPath failed?! " << endl;
+ return eError;
+ }
+ processNodes(lastExtendNode->ID,lastAddedID);
+
+ // update "last" data
+ lastExtendNode = tree->getNode((unsigned int)lastAddedID);
+
+ // mark last extended node (shouldn't be selected in any future nearest neighbor searches)
+ // 2 -> this node was created during a moveArmToGraspPosition action
+ lastExtendNode->status = 2;
+
+ lastDist = dist;
+ dist = VirtualRobot::MathTools::getCartesianPoseDiff(mapConfigTcp[lastExtendNode],targetPose);
+
+ if ((dist-lastDist)>2.0f*cartSamplingPosStepSize)
+ {
+ if (verbose)
+ cout << __FUNCTION__ << ": Stop no dist improvement: last: " << lastDist << ", dist: " << dist << endl;
+ return eTrapped;
+ }
+ loopCount++;
+ break;
+ case eJointBoundaryViolation:
+ if (verbose)
+ cout << __FUNCTION__ << " Stopping: Joint limits reached " << endl;
+ return eTrapped;
+ break;
+ default:
+ cout << __FUNCTION__ << ": result nyi " << res << endl;
+ break;
+ }
+
+ if (dist <= fMaxDist_GraspGoalReached)
+ {
+ if (verbose)
+ cout << __FUNCTION__ << " grasp goal position reached, dist: " << dist << endl;
+ // check grasp score of last valid config
+ float fGraspScore = calculateGraspScore(lastExtendNode->configuration, lastExtendNode->ID, true);
+ if (fGraspScore>=graspQualityMinScore)
+ {
+ cout << __FUNCTION__ << ": found valid grasp (score:" << fGraspScore << ")" << endl;
+ return eGoalReached;
+ } else if (fGraspScore>0.0f)
+ {
+ cout << __FUNCTION__ << ": found valid grasp (score:" << fGraspScore << ")" << endl;
+ return eGraspablePoseReached;
+ } else
+ {
+ if (verbose)
+ cout << __FUNCTION__ << ": aborting moveArmToGraspPos, goal pos reached but zero grasp score " << endl;
+ return eTrapped;
+ }
+ }
+ }
+ if (verbose)
+ cout << __FUNCTION__ << " max lops reached, dist: " << dist << endl;
+ return eTrapped;
+}
+
+void GraspRrt::limitWorkspaceStep(Eigen::Matrix4f &p)
+{
+ float distPos = p.block(0,3,3,1).norm();
+ Eigen::Vector3f axis;
+ float angle;
+ //MathHelpers::quat2AxisAngle(&(pPosQuat[3]),axis,&angle);
+ VirtualRobot::MathTools::eigen4f2axisangle(p,axis,angle);
+ //cout << "Delta in Workspace: pos:" << deltaX << "," << deltaY << "," << deltaZ << " Dist: " << distPos << endl;
+ //cout << "Delta in Workspace: ori:" << angle << endl;
+ float distOri = fabs(angle);
+
+ float factorPos = 1.0f;
+ float factorOri = 1.0f;
+ if (distPos>cartSamplingPosStepSize || distOri>cartSamplingOriStepSize)
+ {
+ if (distPos>cartSamplingPosStepSize)
+ factorPos = cartSamplingPosStepSize / distPos;
+ if (distOri>cartSamplingOriStepSize)
+ factorOri = cartSamplingOriStepSize / distOri;
+
+ if (factorPos<factorOri)
+ {
+ factorOri = factorPos;
+ }
+ if (factorOri<factorPos)
+ {
+ factorPos = factorOri;
+ }
+ }
+
+ /*pPosQuat[0] *= factorPos;
+ pPosQuat[1] *= factorPos;
+ pPosQuat[2] *= factorPos;
+ MathHelpers::axisAngle2Quat(axis,angle*factorOri,&(pPosQuat[3]));*/
+
+ Eigen::Matrix4f m = VirtualRobot::MathTools::axisangle2eigen4f(axis,angle*factorOri);
+ m.block(0,3,3,1) = p.block(0,3,3,1)*factorPos;
+ p = m;
+}
+
+
+GraspRrt::MoveArmResult GraspRrt::createWorkSpaceSamplingStep(const Eigen::Matrix4f ¤tPose, const Eigen::Matrix4f &goalPose, Eigen::VectorXf &storeCSpaceConf)
+{
+ Eigen::Matrix4f deltaPose = goalPose * currentPose.inverse();
+
+ // we need the translational error in global coords
+ deltaPose.block(0,3,3,1) = goalPose.block(0,3,3,1) - currentPose.block(0,3,3,1);
+
+ /*
+ cout << "Current pose:" << endl;
+ cout << currentPose << endl;;
+ cout << "goal pose:" << endl;
+ cout << goalPose << endl;;
+ cout << "delta pose:" << endl;
+ cout << deltaPose << endl;;
+ */
+
+ limitWorkspaceStep(deltaPose);
+ //MathTools::PosQuat2PosRPY(pDeltaPosQuat_Global,pDeltaPosRPY_Global);
+
+ /*
+ cout << "delta pose (limit):" << endl;
+ cout << deltaPose << endl;;
+ cout << "---------------" << endl;
+ */
+
+ return moveArmDiffKin( deltaPose, storeCSpaceConf);
+}
+GraspRrt::MoveArmResult GraspRrt::createWorkSpaceSamplingStep(const Eigen::Matrix4f &goalPose, CSpaceNodePtr extendNode, Eigen::VectorXf &storeCSpaceConf)
+{
+ // calculate delta
+ /*float pDeltaPosRPY_Global[6];
+ float pDeltaPosQuat_Global[7];
+ for (int i=0;i<3;i++)
+ {
+ pDeltaPosQuat_Global[i] = pCartGoalPose[i] - pExtendNode->cartPosTCP1[i];
+ }
+ MathHelpers::deltaQuat(&(pExtendNode->cartPosTCP1[3]),&(pCartGoalPose[3]),&(pDeltaPosQuat_Global[3]));
+ */
+ rns->setJointValues(extendNode->configuration);
+ Eigen::Matrix4f currentPose = mapConfigTcp[extendNode];
+ return createWorkSpaceSamplingStep (currentPose,goalPose,storeCSpaceConf);
+}
+
+GraspRrt::MoveArmResult GraspRrt::moveArmDiffKin(const Eigen::Matrix4f &deltaPose, Eigen::VectorXf &storeCSpaceConf)
+{
+ Eigen::VectorXf startConfig(rns->getSize());
+ rns->getJointValues(startConfig);
+
+ // get InvJac
+ Eigen::VectorXf e(6);
+
+ // The translational error is just the vector between the actual and the target position
+ e.segment(0,3) = deltaPose.block(0,3,3,1);
+
+ Eigen::AngleAxis<float> aa(deltaPose.block<3,3>(0,0));
+ e.segment(3,3) = aa.axis()*aa.angle();
+
+ // Calculate the IK
+ Eigen::VectorXf dTheta = diffIK->getPseudoInverseJacobianMatrix(eef->getGCP()) * e;
+
+
+
+ /*std::string sKinChain = m_pCSpace->getNameOfKinematicChain();
+ CKinematicChain* pKinChain = m_pRobot->GetKinematicChain(sKinChain);
+ if (!pKinChain)
+ {
+ std::cout << "GraspRrt::plan:Error: planner: no kinematic chain with name " << sKinChain << std::endl;
+ return eFatalError;
+ }
+ int nrOfJoints = pKinChain->GetNoOfNodes();
+
+ Matrix invJac;
+ //invJac = m_pRobot->getInverseJacobian(pKinChain);
+ invJac = m_pEfficientJacobain->getInverseJacobain();
+
+ ColumnVector delatGlobal(6);
+ delatGlobal << pDeltaPosRPY_Global[0] << pDeltaPosRPY_Global[1] << pDeltaPosRPY_Global[2] << pDeltaPosRPY_Global[3] << pDeltaPosRPY_Global[4] << pDeltaPosRPY_Global[5];
+ std::vector<CRobotNode*> nodes;
+ m_pRobot->CollectKinematicChainNodes(nodes, sKinChain);
+ int nNrOfNodes = (int)nodes.size();
+
+ ColumnVector storeJointDelta(nNrOfNodes);
+ storeJointDelta = invJac*delatGlobal;
+
+ */
+
+ storeCSpaceConf.resize(cspace->getDimension());
+ // apply values
+ for (int i=0;i<(int)cspace->getDimension();i++)
+ {
+ storeCSpaceConf[i] = startConfig[i] + dTheta[i];
+ }
+ bool r1 = cspace->isInBoundary(storeCSpaceConf);
+ if (!r1)
+ return eJointBoundaryViolation;
+
+ // TODO: maybe the planner could speed up by testing TCP / arm collisions
+ // this collision check does not distinguish between TCP or arm and grasp object or environment collisions
+ bool r2 = cspace->isCollisionFree(storeCSpaceConf);
+ if (!r2)
+ return eCollision_Environment;
+
+ // apply values
+ /*float pV = new float[nNrOfNodes];
+
+ for (int i = 0; i<nNrOfNodes; i++)
+ pV[i] = nodes[i]->GetJointValue()+storeJointDelta(i+1);
+ m_pRobot->SetJointValues(pV,sKinChain);
+
+ delete []pV;*/
+
+ return eMovedOneStep;
+
+}
+
+
+float GraspRrt::calculateGraspScore(const Eigen::VectorXf &c, int nId, bool bStoreGraspInfoOnSuccess)
+{
+ SABA_ASSERT(graspQualityMeasure);
+ clock_t timeStart = clock();
+ performanceMeasure.numberOfGraspScorings++;
+
+ rns->setJointValues(c);
+ std::vector< VirtualRobot::EndEffector::ContactInfo > contactsAll = eef->closeActors(graspCollisionObjects);
+ std::vector< VirtualRobot::EndEffector::ContactInfo > contacts;
+ // we only need the targetObject contacts
+ for (size_t i=0;i<contactsAll.size();i++)
+ {
+ if (contactsAll[i].obstacle == targetObject)
+ {
+ contacts.push_back(contactsAll[i]);
+ }
+ }
+ eef->openActors();
+ if (contacts.size()<=2)
+ {
+ if (verbose)
+ {
+ cout << __FUNCTION__ << ": Low number of contacts -> Zero Grasp Score " << endl;
+ cout << "Fingers: " ;
+ for (int i=0;i<(int)contacts.size();i++)
+ {
+ cout << contacts[i].actor->getName() << ", ";
+ }
+ cout << endl;
+ }
+
+ clock_t timeEnd = clock();
+ performanceMeasure.scoreGraspTimeMS += ((float)(timeEnd - timeStart) / (float)CLOCKS_PER_SEC) * 1000.0f;
+ return 0.0f;
+ }
+
+
+ float fScore = 0.0f;
+ float fScoreContacts = 0.0f;
+ // get distance to target
+ Eigen::Vector3f P1,P2;
+ int nId1,nId2;
+ float fDistTarget = targetObject->getCollisionChecker()->calculateDistance(targetObject->getCollisionModel(), gcpOject->getCollisionModel(), P1, P2, &nId1, &nId2);
+
+
+ graspQualityMeasure->setContactPoints(contacts);
+ graspQualityMeasure->calculateGraspQuality();
+ fScore = graspQualityMeasure->getGraspQuality();
+
+ bool isFC = graspQualityMeasure->isValid();
+ cout << __FUNCTION__ << ": Grasp Measure Score:" << fScore << endl;
+ cout << __FUNCTION__ << ": IsGraspForceClosure/isValid:" << isFC << endl;
+ cout << __FUNCTION__ << ": Nr of Contacts Score:" << fScoreContacts << endl;
+
+ if (!isFC)
+ fScore = 0.0f;
+ //else
+ // fScore = 1.0f;
+
+ if (bStoreGraspInfoOnSuccess && fScore>0.0f)
+ {
+ GraspInfo info;
+ info.distanceToObject = (float)fDistTarget;
+ info.graspScore = fScore;
+ info.rrtNodeId = nId;
+ //info.contacts = (int)contacts.size();
+ Eigen::Matrix4f mMat;
+
+ mMat = eef->getGCP()->toLocalCoordinateSystem(targetObject->getGlobalPose());
+ //CFeasibleGrasp::ComputeObjectPoseInHandFrame(m_pManipulationObject,m_pEndEffector,mMat);
+ info.handToObjectTransform = mMat;
+ info.contacts = contacts;
+ graspInfoMutex.lock();
+ grasps.push_back(info);
+ graspInfoMutex.unlock();
+ }
+ clock_t timeEnd = clock();
+ performanceMeasure.scoreGraspTimeMS += ((float)(timeEnd - timeStart) / (float)CLOCKS_PER_SEC) * 1000.0f;
+
+ return fScore;
+}
+
+/*
+// todo: this should go to the demo
+void GraspRrt::initGraspMeasurement()
+{
+ if (!m_pManipulationObject || !m_pGraspQualityMeasure)
+ return;
+ SbVec3f VecCOM;
+ SoSeparator* pObjSep = m_pManipulationObject->GetIVModel();
+ m_pManipulationObject->getCenterOfMass(VecCOM);
+ cout << "Center of Mass " << VecCOM[0] << "," << VecCOM[1] << "," << VecCOM[2] << endl;
+ SoGetBoundingBoxAction *objectBBAction = new SoGetBoundingBoxAction(SbViewportRegion(0, 0));
+ SbBox3f objectBB;
+ if (pObjSep==NULL)
+ {
+ printf ("pObject: NULL Data..\n");
+ return;
+ }
+ pObjSep->getBoundingBox(objectBBAction);
+ objectBB = objectBBAction->getBoundingBox();
+ SbVec3f objectBBSize;
+ objectBB.getSize(objectBBSize[0],objectBBSize[1],objectBBSize[2]);
+ double objectLength = max(max(objectBBSize[0],objectBBSize[1]),objectBBSize[2]);
+ GraspStudio::Vec3D objectCoM;
+ objectCoM.x = VecCOM[0];
+ objectCoM.y = VecCOM[1];
+ objectCoM.z = VecCOM[2];
+ m_pGraspQualityMeasure->SetObjectProperties(objectCoM, objectLength, (SoNode*)pObjSep);
+ m_pGraspQualityMeasure->CalculateObjectProperties();
+}*/
+
+void GraspRrt::printGraspInfo(GraspInfo &GrInfo)
+{
+ cout << "Grasp Info:" << endl;
+ cout << " Distance to object: " << GrInfo.distanceToObject << endl;
+ cout << " Grasp score: " << GrInfo.graspScore << endl;
+ cout << " RRT Node ID: " << GrInfo.rrtNodeId << endl;
+ cout << " Contacts stored: " << GrInfo.contacts.size() << endl;
+}
+
+
+void GraspRrt::printConfig(bool printOnlyParams)
+{
+ if (!printOnlyParams)
+ {
+ std::cout << "-- GraspRrt config --" << std::endl;
+ std::cout << "------------------------------" << std::endl;
+ }
+ if (cspace && cspace->getRobot())
+ std::cout << " Robot: " << cspace->getRobot()->getName() << std::endl;
+ if (eef)
+ {
+ std::cout << " EndEffector: " << eef->getName() << std::endl;
+ if (eef->getGCP())
+ std::cout << " GCP: " << eef->getGCP()->getName() << std::endl;
+ }
+ if (rns)
+ std::cout << " RNS: " << rns->getName() << std::endl;
+ cout << " Cart Step Size: " << cartSamplingPosStepSize << ", Orientational step size: " << cartSamplingPosStepSize << endl;
+ Rrt::printConfig (true);
+
+ if (!printOnlyParams)
+ {
+ std::cout << "------------------------------" << std::endl;
+ }
+}
+
+ApproachDiscretizationPtr GraspRrt::getPoseRelationSphere()
+{
+ return poseSphere;
+}
+
+void GraspRrt::getGraspInfoResult(std::vector<GraspInfo, Eigen::aligned_allocator<GraspInfo> > &storeGraspInfo)
+{
+ graspInfoMutex.lock();
+ storeGraspInfo = grasps;
+ graspInfoMutex.unlock();
+}
+
+bool GraspRrt::getGraspInfoResult(int nIndex, GraspInfo &storeGraspInfo)
+{
+ graspInfoMutex.lock();
+ if (nIndex<0 || nIndex>=(int)grasps.size())
+ {
+ cout << __FUNCTION__ << ": index out of range: " << nIndex << endl;
+ graspInfoMutex.unlock();
+ return false;
+ }
+ storeGraspInfo = grasps[nIndex];
+ graspInfoMutex.unlock();
+ return true;
+}
+
+int GraspRrt::getNrOfGraspInfoResults()
+{
+ int nRes;
+ graspInfoMutex.lock();
+ nRes = (int)grasps.size();
+ graspInfoMutex.unlock();
+ return nRes;
+}
+
+GraspRrt::PlanningPerformance GraspRrt::getPerformanceMeasurement()
+{
+ return performanceMeasure;
+}
+
+
+bool GraspRrt::processNodes( unsigned int startId, unsigned int endId )
+{
+ if (!tree)
+ return false;
+ int nActId = endId;
+ CSpaceNodePtr currentNode;
+ while (nActId>=0 && nActId!=startId)
+ {
+ currentNode = tree->getNode(nActId);
+ processNode(currentNode);
+
+ nActId = currentNode->parentID;
+ }
+ return true;
+}
+
+bool GraspRrt::processNode( CSpaceNodePtr n )
+{
+ if (!n)
+ return false;
+ // get tcp pose
+ rns->setJointValues(n->configuration);
+
+ // using gcp!
+ Eigen::Matrix4f p = eef->getGCP()->getGlobalPose();
+ mapConfigTcp[n] = p;
+ Eigen::Vector3f dist = p.block(0,3,3,1) - targetObjectPosition;
+
+ if (poseSphere && dist.squaredNorm() < tryGraspsDistance2)
+ {
+ poseSphere->addCSpaceNode(p.block(0,3,3,1),n);
+ }
+ return true;
+
+}
+
+}
diff --git a/MotionPlanning/Planner/GraspRrt.h b/MotionPlanning/Planner/GraspRrt.h
new file mode 100644
index 000000000..b9efa7dc1
--- /dev/null
+++ b/MotionPlanning/Planner/GraspRrt.h
@@ -0,0 +1,267 @@
+/**
+* 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 Saba
+* @author Nikolaus Vahrenkamp
+* @copyright 2012 Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#ifndef _Saba_GraspRrt_h
+#define _Saba_GraspRrt_h
+
+#include "../Saba.h"
+#include "../CSpace/CSpaceSampled.h"
+#include "../CSpace/CSpacePath.h"
+#include "../CSpace/CSpaceNode.h"
+#include "../ApproachDiscretization.h"
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/EndEffector/EndEffector.h>
+#include <VirtualRobot/Obstacle.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/IK/IKSolver.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Obstacle.h>
+#include "Rrt.h"
+
+namespace Saba {
+
+/*!
+ *
+ *
+* A planner that combines the search for a feasible grasping pose with RRT-based motion planning.
+* No predefined grasps are needed, a feasible grasp is searched during the planning process.
+* Makes use of the DifferntialIK methods to generate potential approach movements.
+* A GraspQualityMeasure object is needed in order to evaluate generated grasping hypotheses.
+*
+* Related publication: "Integrated Grasp and Motion Planning", N. Vahrenkamp, M. Do, T. Asfour, R. Dillmann, ICRA 2010
+*
+*/
+class SABA_IMPORT_EXPORT GraspRrt : public Rrt
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /*!
+ Constructor
+ \param cspace The C-Space that should be used for collision detection.
+ The RobotNodeSet, that was used to define the cspace, is internally used to generate approach movements.
+ \param eef The EndEffector that should be used for grasping.
+ \param object The object to be grasped
+ \param measure A measure object that should be used for evaluating the quality of generated grasping hypotheses.
+ \param graspCollisionObjects These (optional) objects are considered as obstacles when closing the hand. The targetObject is handled explicitly and need not be part of these object set.
+ \param probabGraspHypothesis The probability that a grasping hypothesis is generated during a loop.
+ \param graspQualityMinScore The minimum score that must be achieved for a valid grasp.
+
+ @see GraspStudio::GraspQualityMeasureWrenchSpace
+ */
+ GraspRrt( CSpaceSampledPtr cspace,
+ VirtualRobot::EndEffectorPtr eef,
+ VirtualRobot::ObstaclePtr object,
+ VirtualRobot::BasicGraspQualityMeasurePtr measure,
+ VirtualRobot::SceneObjectSetPtr graspCollisionObjects = VirtualRobot::SceneObjectSetPtr(),
+ float probabGraspHypothesis = 0.1f,
+ float graspQualityMinScore = 0.01f);
+
+ virtual ~GraspRrt();
+
+ /*!
+ do the planning (blocking method)
+ \return true if solution was found, otherwise false
+ */
+ virtual bool plan(bool bQuiet = false);
+
+
+ virtual void printConfig(bool printOnlyParams = false);
+ virtual bool setStart(const Eigen::VectorXf &c);
+
+ //! This is not allowed here, since we sample goal configurations during planning: If called an exception is thrown
+ virtual bool setGoal(const Eigen::VectorXf &c);
+
+ //! reset the planner
+ virtual void reset();
+
+ /*!
+ Returns the internal representation of the pose sphere.
+ The pose sphere is used to encode approach directions.
+ */
+ ApproachDiscretizationPtr getPoseRelationSphere();
+
+ struct GraspInfo
+ {
+ Eigen::Matrix4f handToObjectTransform;
+ int rrtNodeId;
+ float graspScore;
+ float distanceToObject;
+ std::vector<VirtualRobot::EndEffector::ContactInfo> contacts;
+ };
+
+ //! Stores all found grasps to given vector (thread safe)
+ void getGraspInfoResult(std::vector<GraspInfo, Eigen::aligned_allocator<GraspInfo> > &vStoreGraspInfo);
+
+ //! Returns a specific grasp result (thread safe)
+ bool getGraspInfoResult(int index, GraspInfo &storeGraspInfo);
+
+ //! Returns number of (currently) found grasps (thread safe)
+ int getNrOfGraspInfoResults();
+
+ /*!
+ Creates a solution with last entry cspaceNodeIndex.
+ You are responsible for deleting.
+ */
+ //solution* buildSolution(int cspaceNodeIndex);
+
+ struct PlanningPerformance
+ {
+ float setupTimeMS;
+ float planningTimeMS; // == RRTTime + moveTime + graspScoreTime
+ float moveTowardGraspPosesTimeMS; // accumulated time
+ int numberOfMovementsTowardGraspPose;
+ float scoreGraspTimeMS; // accumulated time
+ int numberOfGraspScorings;
+ float rrtTimeMS;
+ };
+
+ PlanningPerformance getPerformanceMeasurement();
+ void printPerformanceResults();
+
+ /*!
+ This method generates a grasping pose on the object's surface.
+ The z-axis of the GCP's coordinate system is aligned with the normal.
+ We assume that the z axis specifies the favorite approach movement of the EEF.
+ */
+ bool calculateGlobalGraspPose(const Eigen::VectorXf &c, Eigen::Matrix4f &storeGoal);
+
+ enum MoveArmResult
+ {
+ eMovedOneStep, // arm movement was successful, but goal not reached yet
+ eGoalReached, // goal reached
+ eCollision_Environment, // arm or hand collided with environment
+ eGraspablePoseReached, // a position which allows applying a feasible grasp was reached
+ eJointBoundaryViolation, // joint limits were violated
+ eTrapped, // trapped in local minima
+ eError, // unexpected error
+ eFatalError // unexpected fatal error
+ };
+
+ /*!
+ Computes a step towards goalPose.
+ */
+ MoveArmResult createWorkSpaceSamplingStep(const Eigen::Matrix4f ¤tPose, const Eigen::Matrix4f &goalPose, Eigen::VectorXf &storeCSpaceConf);
+
+ /*!
+ Can be used for custom initialization. Usually this method is automatically called at the beginning of a planning query.
+ */
+ virtual bool init();
+protected:
+
+ bool doPlanningCycle();
+
+ /*!
+ For all nodes in tree, that form the path from startId to endId processNode is called.
+ startId is not processed.
+ */
+ bool processNodes( unsigned int startId, unsigned int endId );
+
+ //! Compute tcp (gcp) pose and update internal map and pose relation sphere
+ bool processNode(CSpaceNodePtr n);
+
+ VirtualRobot::ObstaclePtr targetObject;
+ VirtualRobot::RobotNodeSetPtr rns;
+ VirtualRobot::EndEffectorPtr eef;
+
+ std::map < VirtualRobot::GraspPtr, Saba::CSpaceNodePtr > graspNodeMapping;
+ std::vector< Eigen::VectorXf > ikSolutions;
+
+
+
+ PlanningPerformance performanceMeasure;
+
+
+
+
+ MoveArmResult connectRandomGraspPositionJacobian();
+
+ Rrt::ExtensionResult connectComplete();
+
+ float workSpaceSamplingCount;
+
+ VirtualRobot::ObstaclePtr gcpOject; //<! This object is used to determine the distances to the targetObject
+
+ MoveArmResult moveTowardsGoal(CSpaceNodePtr startNode, const Eigen::Matrix4f &targetPose, int nMaxLoops);
+ MoveArmResult createWorkSpaceSamplingStep(const Eigen::Matrix4f &goalPose, CSpaceNodePtr extendNode, Eigen::VectorXf &storeCSpaceConf);
+
+ /*!
+ Limit the step that is defined with p, so that the translational and rotational stepsize is limited by cartSamplingOriStepSize and cartSamplingPosStepSize
+ */
+ void limitWorkspaceStep(Eigen::Matrix4f &p);
+
+ /*!
+ Current pose of RNS is moved.
+ \param deltaPose Consists of the 3x3 rotation delta R and the 3 dim translational delta T in homogeneous notation.
+ R and T must be given in global coordinate system.
+ */
+ MoveArmResult moveArmDiffKin(const Eigen::Matrix4f &deltaPose, Eigen::VectorXf &storeCSpaceConf);
+
+
+ // Computes the grasp score for given configuration
+ // nId is needed, to store the id of the rrt node which holds pConfig
+ float calculateGraspScore(const Eigen::VectorXf &c, int nId = -1, bool bStoreGraspInfoOnSuccess = false);
+
+
+ VirtualRobot::SceneObjectSetPtr graspCollisionObjects; //!< These objects are considered as obstacles when closing the hand. The targetObject is handled explicitly and must not be part of these object set.
+
+ virtual Rrt::ExtensionResult connectComplete(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID);
+
+ void printGraspInfo (GraspInfo &GrInfo);
+
+ MoveArmResult extendGraspStatus;
+ float cartSamplingOriStepSize,cartSamplingPosStepSize;
+
+
+ std::vector<GraspInfo, Eigen::aligned_allocator<GraspInfo> > grasps;
+
+ ApproachDiscretizationPtr poseSphere;
+
+ VirtualRobot::DifferentialIKPtr diffIK;
+
+ Eigen::Vector3f targetObjectPosition; // global pose of manipulation object
+ float tryGraspsDistance2; // TCP<->object distance in which grasp movements are tried (squared value)
+
+ //bool m_bEndlessMode;
+
+ int colChecksOverall;
+ bool foundSolution;
+ boost::mutex graspInfoMutex;
+
+ bool verbose;
+
+ std::map<CSpaceNodePtr, Eigen::Matrix4f, std::less<CSpaceNodePtr>,
+ Eigen::aligned_allocator<std::pair<const CSpaceNodePtr, Eigen::Matrix4f> > > mapConfigTcp; //!< Store the tcp (gcp) poses that correspond with the config
+
+ // grasp measurement
+ VirtualRobot::BasicGraspQualityMeasurePtr graspQualityMeasure;
+ float probabGraspHypothesis;
+ float graspQualityMinScore;
+
+ bool plannerInitialized;
+
+};
+
+}
+
+#endif // _GraspRRTPlanner_h_
+
diff --git a/MotionPlanning/Planner/MotionPlanner.cpp b/MotionPlanning/Planner/MotionPlanner.cpp
index 1a5b69b33..3c7787798 100644
--- a/MotionPlanning/Planner/MotionPlanner.cpp
+++ b/MotionPlanning/Planner/MotionPlanner.cpp
@@ -165,7 +165,8 @@ void MotionPlanner::reset()
goalConfig.setZero(1);
startValid = false;
goalValid = false;
- cspace.reset();
+ if (cspace)
+ cspace->reset();
planningTime = 0;
cycles = 0;
diff --git a/MotionPlanning/Planner/Rrt.cpp b/MotionPlanning/Planner/Rrt.cpp
index 7987e636e..b515340f7 100644
--- a/MotionPlanning/Planner/Rrt.cpp
+++ b/MotionPlanning/Planner/Rrt.cpp
@@ -434,7 +434,6 @@ void Rrt::reset()
goalNode.reset();
if (tree)
tree->reset();
- cspace->reset();
}
diff --git a/MotionPlanning/Planner/Rrt.h b/MotionPlanning/Planner/Rrt.h
index 1f1df16df..0e529ba36 100644
--- a/MotionPlanning/Planner/Rrt.h
+++ b/MotionPlanning/Planner/Rrt.h
@@ -101,9 +101,9 @@ protected:
CSpaceTreePtr tree; //!< the rrt on which are operating
- ExtensionResult extend(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID);
- ExtensionResult connectComplete(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID);
- ExtensionResult connectUntilCollision(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID);
+ virtual ExtensionResult extend(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID);
+ virtual ExtensionResult connectComplete(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID);
+ virtual ExtensionResult connectUntilCollision(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID);
CSpaceNodePtr startNode; //!< start node (root of RRT)
CSpaceNodePtr goalNode; //!< goal node (set when RRT weas successfully connected to goalConfig)
diff --git a/MotionPlanning/Saba.h b/MotionPlanning/Saba.h
index 2682e33b7..f35a6b6dc 100644
--- a/MotionPlanning/Saba.h
+++ b/MotionPlanning/Saba.h
@@ -85,7 +85,9 @@ namespace Saba
class Rrt;
class BiRrt;
class GraspIkRrt;
+ class GraspRrt;
class ShortcutProcessor;
+ class ApproachDiscretization;
typedef boost::shared_ptr<CSpace> CSpacePtr;
typedef boost::shared_ptr<CSpaceSampled> CSpaceSampledPtr;
@@ -96,8 +98,10 @@ namespace Saba
typedef boost::shared_ptr<Rrt> RrtPtr;
typedef boost::shared_ptr<BiRrt> BiRrtPtr;
typedef boost::shared_ptr<GraspIkRrt> GraspIkRrtPtr;
+ typedef boost::shared_ptr<GraspRrt> GraspRrtPtr;
typedef boost::shared_ptr<ShortcutProcessor> ShortcutProcessorPtr;
typedef boost::shared_ptr<ConfigurationConstraint> ConfigurationConstraintPtr;
+ typedef boost::shared_ptr<ApproachDiscretization> ApproachDiscretizationPtr;
#define SABA_INFO VR_INFO
#define SABA_WARNING VR_WARNING
diff --git a/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp
index 4908e6c66..725a3aadd 100644
--- a/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp
+++ b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp
@@ -29,58 +29,18 @@ namespace Saba {
CoinRrtWorkspaceVisualization::CoinRrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, CSpacePtr cspace, const std::string &TCPName) :
RrtWorkspaceVisualization(robot,cspace,TCPName)
{
- init();
}
CoinRrtWorkspaceVisualization::CoinRrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr robotNodeSet, const std::string &TCPName) :
RrtWorkspaceVisualization(robot,robotNodeSet,TCPName)
{
- init();
}
void CoinRrtWorkspaceVisualization::init()
{
+ RrtWorkspaceVisualization::init();
visualization = new SoSeparator();
visualization->ref();
- setPathStyle();
- setTreeStyle();
-
- RenderColors red;
- red.nodeR = 1.0f;
- red.nodeG = 0;
- red.nodeB = 0;
- red.lineR = 0.5f;
- red.lineG = 0.5f;
- red.lineB = 0.5f;
- colors[CoinRrtWorkspaceVisualization::eRed] = red;
-
- RenderColors blue;
- blue.nodeR = 0;
- blue.nodeG = 0;
- blue.nodeB = 1.0f;
- blue.lineR = 0.5f;
- blue.lineG = 0.5f;
- blue.lineB = 0.5f;
- colors[CoinRrtWorkspaceVisualization::eBlue] = blue;
-
- RenderColors green;
- green.nodeR = 0;
- green.nodeG = 1.0f;
- green.nodeB = 0;
- green.lineR = 0.5f;
- green.lineG = 0.5f;
- green.lineB = 0.5f;
- colors[CoinRrtWorkspaceVisualization::eGreen] = green;
-
- RenderColors custom;
- custom.nodeR = 1.0f;
- custom.nodeG = 1.0f;
- custom.nodeB = 0;
- custom.lineR = 0.5f;
- custom.lineG = 0.5f;
- custom.lineB = 0.5f;
- colors[CoinRrtWorkspaceVisualization::eCustom] = custom;
-
}
/**
@@ -103,33 +63,6 @@ SoSeparator* CoinRrtWorkspaceVisualization::getCoinVisualization()
return visualization;
}
-void CoinRrtWorkspaceVisualization::setPathStyle(float lineSize, float nodeSize, float renderComplexity)
-{
- pathLineSize = lineSize;
- pathNodeSize = nodeSize;
- pathRenderComplexity = renderComplexity;
-}
-
-void CoinRrtWorkspaceVisualization::setTreeStyle(float lineSize, float nodeSize, float renderComplexity)
-{
- treeLineSize = lineSize;
- treeNodeSize = nodeSize;
- treeRenderComplexity = renderComplexity;
-}
-
-void CoinRrtWorkspaceVisualization::setCustomColor(float nodeR, float nodeG, float nodeB, float lineR, float lineG, float lineB )
-{
- RenderColors custom;
- custom.nodeR = nodeR;
- custom.nodeG = nodeG;
- custom.nodeB = nodeB;
- custom.lineR = lineR;
- custom.lineG = lineG;
- custom.lineB = lineB;
- colors[CoinRrtWorkspaceVisualization::eCustom] = custom;
-}
-
-
bool CoinRrtWorkspaceVisualization::addCSpacePath(CSpacePathPtr path, CoinRrtWorkspaceVisualization::ColorSet colorSet)
{
if (!path || !robotNodeSet || !TCPNode)
@@ -255,6 +188,19 @@ bool CoinRrtWorkspaceVisualization::addTree(CSpaceTreePtr tree, CoinRrtWorkspace
materialLine->diffuseColor.setValue(colors[colorSet].lineR,colors[colorSet].lineG,colors[colorSet].lineB);
materialNode->ambientColor.setValue(colors[colorSet].nodeR,colors[colorSet].nodeG,colors[colorSet].nodeB);
materialNode->diffuseColor.setValue(colors[colorSet].nodeR,colors[colorSet].nodeG,colors[colorSet].nodeB);
+ std::map<int,SoMaterial*> statusMaterials;
+ std::map<int,ColorSet>::iterator it = treeNodeStatusColor.begin();
+ bool considerStatus = false;
+ while (it != treeNodeStatusColor.end())
+ {
+ SoMaterial *materialNodeStatus = new SoMaterial();
+ materialNodeStatus->ambientColor.setValue(colors[it->second].nodeR,colors[it->second].nodeG,colors[it->second].nodeB);
+ materialNodeStatus->diffuseColor.setValue(colors[it->second].nodeR,colors[it->second].nodeG,colors[it->second].nodeB);
+ statusMaterials[it->first] = materialNodeStatus;
+ considerStatus = true;
+ it++;
+ }
+
SoSphere *sphereNode = new SoSphere();
sphereNode->radius.setValue(treeNodeSize);
@@ -303,8 +249,10 @@ bool CoinRrtWorkspaceVisualization::addTree(CSpaceTreePtr tree, CoinRrtWorkspace
// create 3D model for nodes
SoSeparator *s = new SoSeparator();
-
- s->addChild(materialNode);
+ if (considerStatus && statusMaterials.find(actualNode->status)!=statusMaterials.end())
+ s->addChild(statusMaterials[actualNode->status]);
+ else
+ s->addChild(materialNode);
// get tcp coords
p = tcpCoords[actualNode];
diff --git a/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h
index e41252ef2..63aca1466 100644
--- a/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h
+++ b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h
@@ -47,36 +47,28 @@ public:
~CoinRrtWorkspaceVisualization();
- enum ColorSet
- {
- eRed,
- eGreen,
- eBlue,
- eCustom
- };
-
/*!
Add visualization of a path in cspace.
*/
- virtual bool addCSpacePath(CSpacePathPtr path, CoinRrtWorkspaceVisualization::ColorSet colorSet = eBlue);
- void setPathStyle(float lineSize = 4.0f, float nodeSize= 15.0f, float renderComplexity = 1.0f);
+ virtual bool addCSpacePath(CSpacePathPtr path, RrtWorkspaceVisualization::ColorSet colorSet = eBlue);
+ //void setPathStyle(float lineSize = 4.0f, float nodeSize= 15.0f, float renderComplexity = 1.0f);
/*!
- Add visualization of a path in cspace.
+ Add visualization of a tree (e.g an RRT) in cspace.
*/
- virtual bool addTree(CSpaceTreePtr tree, CoinRrtWorkspaceVisualization::ColorSet colorSet = eRed);
- void setTreeStyle(float lineSize = 1.0f, float nodeSize= 15.0f, float renderComplexity = 0.1f);
+ virtual bool addTree(CSpaceTreePtr tree, RrtWorkspaceVisualization::ColorSet colorSet = eRed);
+ //void setTreeStyle(float lineSize = 1.0f, float nodeSize= 15.0f, float renderComplexity = 0.1f);
/*!
Add visualization of a configuration in cspace.
*/
- virtual bool addConfiguration(const Eigen::VectorXf &c, CoinRrtWorkspaceVisualization::ColorSet colorSet = eGreen, float nodeSizeFactor = 1.0f);
+ virtual bool addConfiguration(const Eigen::VectorXf &c, RrtWorkspaceVisualization::ColorSet colorSet = eGreen, float nodeSizeFactor = 1.0f);
/*!
Set the custom line and node color. Does not affect already added trees or paths.
*/
- void setCustomColor(float nodeR, float nodeG, float nodeB, float lineR = 0.5f, float lineG = 0.5f, float lineB = 0.5f);
+ //void setCustomColor(float nodeR, float nodeG, float nodeB, float lineR = 0.5f, float lineG = 0.5f, float lineB = 0.5f);
/*!
Clears all visualizations.
@@ -88,20 +80,9 @@ public:
protected:
- void init();
+ virtual void init();
SoSeparator* visualization;
-
- float pathLineSize, pathNodeSize, pathRenderComplexity;
- float treeLineSize, treeNodeSize, treeRenderComplexity;
-
- struct RenderColors
- {
- float nodeR, nodeG, nodeB, lineR, lineG, lineB;
- };
-
-
- std::map<ColorSet, RenderColors> colors;
};
typedef boost::shared_ptr<CoinRrtWorkspaceVisualization> CoinRrtWorkspaceVisualizationPtr;
diff --git a/MotionPlanning/Visualization/RrtWorkspaceVisualization.cpp b/MotionPlanning/Visualization/RrtWorkspaceVisualization.cpp
index 65ca4cb37..3145f0201 100644
--- a/MotionPlanning/Visualization/RrtWorkspaceVisualization.cpp
+++ b/MotionPlanning/Visualization/RrtWorkspaceVisualization.cpp
@@ -14,6 +14,7 @@ RrtWorkspaceVisualization::RrtWorkspaceVisualization(VirtualRobot::RobotPtr robo
robotNodeSet = cspace->getRobotNodeSet();
SABA_ASSERT(robotNodeSet);
setTCPName(TCPName);
+ init();
}
@@ -25,21 +26,83 @@ RrtWorkspaceVisualization::RrtWorkspaceVisualization(VirtualRobot::RobotPtr robo
SABA_ASSERT(robotNodeSet);
this->robotNodeSet = robotNodeSet;
setTCPName(TCPName);
+ init();
}
RrtWorkspaceVisualization::~RrtWorkspaceVisualization()
{
}
-/*
-bool RrtWorkspaceVisualization::addCSpacePath(CSpacePathPtr path)
+
+void RrtWorkspaceVisualization::init()
{
- return false;
+ setPathStyle();
+ setTreeStyle();
+
+ RenderColors red;
+ red.nodeR = 1.0f;
+ red.nodeG = 0;
+ red.nodeB = 0;
+ red.lineR = 0.5f;
+ red.lineG = 0.5f;
+ red.lineB = 0.5f;
+ colors[RrtWorkspaceVisualization::eRed] = red;
+
+ RenderColors blue;
+ blue.nodeR = 0;
+ blue.nodeG = 0;
+ blue.nodeB = 1.0f;
+ blue.lineR = 0.5f;
+ blue.lineG = 0.5f;
+ blue.lineB = 0.5f;
+ colors[RrtWorkspaceVisualization::eBlue] = blue;
+
+ RenderColors green;
+ green.nodeR = 0;
+ green.nodeG = 1.0f;
+ green.nodeB = 0;
+ green.lineR = 0.5f;
+ green.lineG = 0.5f;
+ green.lineB = 0.5f;
+ colors[RrtWorkspaceVisualization::eGreen] = green;
+
+ RenderColors custom;
+ custom.nodeR = 1.0f;
+ custom.nodeG = 1.0f;
+ custom.nodeB = 0;
+ custom.lineR = 0.5f;
+ custom.lineG = 0.5f;
+ custom.lineB = 0.5f;
+ colors[RrtWorkspaceVisualization::eCustom] = custom;
+
}
-bool RrtWorkspaceVisualization::addTree(CSpaceTreePtr tree)
+
+void RrtWorkspaceVisualization::setPathStyle(float lineSize, float nodeSize, float renderComplexity)
{
- return false;
-}*/
+ pathLineSize = lineSize;
+ pathNodeSize = nodeSize;
+ pathRenderComplexity = renderComplexity;
+}
+
+void RrtWorkspaceVisualization::setTreeStyle(float lineSize, float nodeSize, float renderComplexity)
+{
+ treeLineSize = lineSize;
+ treeNodeSize = nodeSize;
+ treeRenderComplexity = renderComplexity;
+}
+
+void RrtWorkspaceVisualization::setCustomColor(float nodeR, float nodeG, float nodeB, float lineR, float lineG, float lineB )
+{
+ RenderColors custom;
+ custom.nodeR = nodeR;
+ custom.nodeG = nodeG;
+ custom.nodeB = nodeB;
+ custom.lineR = lineR;
+ custom.lineG = lineG;
+ custom.lineB = lineB;
+ colors[RrtWorkspaceVisualization::eCustom] = custom;
+}
+
void RrtWorkspaceVisualization::reset()
{
@@ -54,6 +117,12 @@ void RrtWorkspaceVisualization::setTCPName(const std::string TCPName)
VR_ERROR << "No node with name " << TCPName << " available in robot.." << endl;
}
}
+
+void RrtWorkspaceVisualization::colorizeTreeNodes( int status, ColorSet colorSet )
+{
+ treeNodeStatusColor[status] = colorSet;
+}
+
/*
bool RrtWorkspaceVisualization::addConfig( const Eigen::VectorXf &c )
{
diff --git a/MotionPlanning/Visualization/RrtWorkspaceVisualization.h b/MotionPlanning/Visualization/RrtWorkspaceVisualization.h
index c276a7b1a..8114c06c3 100644
--- a/MotionPlanning/Visualization/RrtWorkspaceVisualization.h
+++ b/MotionPlanning/Visualization/RrtWorkspaceVisualization.h
@@ -51,36 +51,57 @@ public:
RrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, CSpacePtr cspace, const std::string &TCPName);
RrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr robotNodeSet, const std::string &TCPName);
+ enum ColorSet
+ {
+ eRed,
+ eGreen,
+ eBlue,
+ eCustom
+ };
+
/*!
*/
virtual ~RrtWorkspaceVisualization();
/*!
- Add visualization of a path in cspace.
+ Clears all visualizations.
*/
- //virtual bool addCSpacePath(CSpacePathPtr path);
+ virtual void reset();
/*!
- Add visualization of a path in cspace.
+ Set name of TCP joint. Does not affect already added paths or trees.
*/
- //virtual bool addTree(CSpaceTreePtr tree);
+ void setTCPName(const std::string TCPName);
+
/*!
Add visualization of a path in cspace.
*/
- //virtual bool addConfig(const Eigen::VectorXf &c);
+ virtual bool addCSpacePath(CSpacePathPtr path, RrtWorkspaceVisualization::ColorSet colorSet = eBlue) = 0;
+ virtual void setPathStyle(float lineSize = 4.0f, float nodeSize= 15.0f, float renderComplexity = 1.0f);
/*!
- Clears all visualizations.
+ Add visualization of a tree (e.g an RRT) in cspace.
*/
- virtual void reset();
+ virtual bool addTree(CSpaceTreePtr tree, RrtWorkspaceVisualization::ColorSet colorSet = eRed) = 0;
+ virtual void setTreeStyle(float lineSize = 1.0f, float nodeSize= 15.0f, float renderComplexity = 0.1f);
+
+ /*!
+ Add visualization of a configuration in cspace.
+ */
+ virtual bool addConfiguration(const Eigen::VectorXf &c, RrtWorkspaceVisualization::ColorSet colorSet = eGreen, float nodeSizeFactor = 1.0f) = 0;
/*!
- Set name of TCP joint. Does not affect already added paths or trees.
+ Set the custom line and node color. Does not affect already added trees or paths.
*/
- void setTCPName(const std::string TCPName);
+ virtual void setCustomColor(float nodeR, float nodeG, float nodeB, float lineR = 0.5f, float lineG = 0.5f, float lineB = 0.5f);
+ /*!
+ Set tree nodes with status flag equal to given paramter to the specified color.
+ */
+ virtual void colorizeTreeNodes(int status, ColorSet colorSet);
protected:
+ virtual void init();
VirtualRobot::RobotPtr robot;
CSpacePtr cspace;
VirtualRobot::RobotNodeSetPtr robotNodeSet;
@@ -88,6 +109,17 @@ protected:
std::string TCPName;
+ float pathLineSize, pathNodeSize, pathRenderComplexity;
+ float treeLineSize, treeNodeSize, treeRenderComplexity;
+
+ struct RenderColors
+ {
+ float nodeR, nodeG, nodeB, lineR, lineG, lineB;
+ };
+
+ std::map<ColorSet, RenderColors> colors;
+ std::map<int, ColorSet> treeNodeStatusColor;
+
};
} // namespace Saba
diff --git a/MotionPlanning/examples/CMakeLists.txt b/MotionPlanning/examples/CMakeLists.txt
index dd147f58a..1e9c81898 100644
--- a/MotionPlanning/examples/CMakeLists.txt
+++ b/MotionPlanning/examples/CMakeLists.txt
@@ -2,3 +2,7 @@
ADD_SUBDIRECTORY(RRT)
ADD_SUBDIRECTORY(RrtGui)
ADD_SUBDIRECTORY(IKRRT)
+
+# todo: check whether GraspStudio is present
+#ADD_SUBDIRECTORY(graspRRT)
+
diff --git a/MotionPlanning/examples/GraspRRT/CMakeLists.txt b/MotionPlanning/examples/GraspRRT/CMakeLists.txt
new file mode 100644
index 000000000..fd981f916
--- /dev/null
+++ b/MotionPlanning/examples/GraspRRT/CMakeLists.txt
@@ -0,0 +1,26 @@
+PROJECT ( GraspRrtDemo )
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6.4)
+
+CMAKE_POLICY(VERSION 2.6)
+
+IF(SIMOX_VISUALIZATION AND SIMOX_USE_COIN_VISUALIZATION)
+
+ # the variable "demo_SRCS" contains all .cpp files of this project
+ FILE(GLOB demo_SRCS ${PROJECT_SOURCE_DIR}/GraspRrtDemo.cpp ${PROJECT_SOURCE_DIR}/GraspRrtWindow.cpp)
+ FILE(GLOB demo_INCS ${PROJECT_SOURCE_DIR}/GraspRrtWindow.h)
+
+
+ ################################## moc'ing ##############################
+ set(GUI_MOC_HDRS
+ ${PROJECT_SOURCE_DIR}/GraspRrtWindow.h
+ )
+
+ set(GUI_UIS
+ ${PROJECT_SOURCE_DIR}/GraspRrt.ui
+ )
+
+
+ #MESSAGE( STATUS ${PROJECT_NAME} " will be placed into " ${SIMOX_BIN_DIR})
+ simox_add_example("${PROJECT_NAME}" "${demo_SRCS}" "${demo_INCS}" "${GUI_MOC_HDRS}" "${GUI_UIS}")
+ENDIF()
diff --git a/MotionPlanning/examples/GraspRRT/GraspRrt.ui b/MotionPlanning/examples/GraspRRT/GraspRrt.ui
new file mode 100644
index 000000000..0c7831eba
--- /dev/null
+++ b/MotionPlanning/examples/GraspRRT/GraspRrt.ui
@@ -0,0 +1,608 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindowGraspRRTDemo</class>
+ <widget class="QMainWindow" name="MainWindowGraspRRTDemo">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1055</width>
+ <height>769</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>GraspRrt Demo</string>
+ </property>
+ <widget class="QWidget" name="centralwidget">
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <widget class="QFrame" name="frameViewer">
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QGroupBox" name="groupBox">
+ <property name="maximumSize">
+ <size>
+ <width>350</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="title">
+ <string/>
+ </property>
+ <widget class="QGroupBox" name="groupBox_2">
+ <property name="geometry">
+ <rect>
+ <x>100</x>
+ <y>430</y>
+ <width>231</width>
+ <height>91</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Display options</string>
+ </property>
+ <widget class="QCheckBox" name="checkBoxColModel">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>20</y>
+ <width>101</width>
+ <height>21</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Collision model</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxShowRRT">
+ <property name="geometry">
+ <rect>
+ <x>130</x>
+ <y>40</y>
+ <width>51</width>
+ <height>20</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>RRT</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxShowSolution">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>40</y>
+ <width>71</width>
+ <height>20</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Solution</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxShowSolutionOpti">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>60</y>
+ <width>151</width>
+ <height>20</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Postprocessed Solution</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxStartGoal">
+ <property name="geometry">
+ <rect>
+ <x>130</x>
+ <y>20</y>
+ <width>71</width>
+ <height>20</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Start/Goal</string>
+ </property>
+ </widget>
+ </widget>
+ <widget class="QGroupBox" name="groupBox_3">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>550</y>
+ <width>331</width>
+ <height>111</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Execute</string>
+ </property>
+ <widget class="QLabel" name="label_2">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>50</y>
+ <width>141</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Position on Solution</string>
+ </property>
+ </widget>
+ <widget class="QSlider" name="horizontalSliderPos">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>70</y>
+ <width>211</width>
+ <height>20</height>
+ </rect>
+ </property>
+ <property name="maximum">
+ <number>1000</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ <widget class="QLCDNumber" name="lcdNumber">
+ <property name="geometry">
+ <rect>
+ <x>240</x>
+ <y>70</y>
+ <width>64</width>
+ <height>23</height>
+ </rect>
+ </property>
+ <property name="smallDecimalPoint">
+ <bool>true</bool>
+ </property>
+ <property name="numDigits">
+ <number>5</number>
+ </property>
+ <property name="digitCount">
+ <number>5</number>
+ </property>
+ <property name="value" stdset="0">
+ <double>0.000000000000000</double>
+ </property>
+ </widget>
+ <widget class="QRadioButton" name="radioButtonSolution">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>20</y>
+ <width>95</width>
+ <height>20</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Solution</string>
+ </property>
+ </widget>
+ <widget class="QRadioButton" name="radioButtonSolutionOpti">
+ <property name="geometry">
+ <rect>
+ <x>140</x>
+ <y>20</y>
+ <width>161</width>
+ <height>20</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Postprocessed Sol.</string>
+ </property>
+ </widget>
+ </widget>
+ <widget class="QGroupBox" name="groupBoxStart">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>10</y>
+ <width>331</width>
+ <height>401</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Setup</string>
+ </property>
+ <widget class="QPushButton" name="pushButtonLoad">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>20</y>
+ <width>81</width>
+ <height>28</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Load Scene</string>
+ </property>
+ </widget>
+ <widget class="QComboBox" name="comboBoxGoal">
+ <property name="geometry">
+ <rect>
+ <x>110</x>
+ <y>90</y>
+ <width>211</width>
+ <height>22</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_5">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>90</y>
+ <width>91</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Traget Object</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_4">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>60</y>
+ <width>91</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Start Conf.</string>
+ </property>
+ </widget>
+ <widget class="QComboBox" name="comboBoxStart">
+ <property name="geometry">
+ <rect>
+ <x>110</x>
+ <y>60</y>
+ <width>211</width>
+ <height>22</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxColChecking">
+ <property name="geometry">
+ <rect>
+ <x>120</x>
+ <y>340</y>
+ <width>62</width>
+ <height>22</height>
+ </rect>
+ </property>
+ <property name="decimals">
+ <number>3</number>
+ </property>
+ <property name="minimum">
+ <double>0.001000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.040000000000000</double>
+ </property>
+ </widget>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxCSpaceSampling">
+ <property name="geometry">
+ <rect>
+ <x>120</x>
+ <y>370</y>
+ <width>62</width>
+ <height>22</height>
+ </rect>
+ </property>
+ <property name="decimals">
+ <number>3</number>
+ </property>
+ <property name="minimum">
+ <double>0.001000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.080000000000000</double>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_7">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>350</y>
+ <width>101</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Collision Checking</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_8">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>370</y>
+ <width>91</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>CSpace Paths</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_9">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>330</y>
+ <width>91</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Sampling</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_11">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>120</y>
+ <width>91</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Robot Node Set</string>
+ </property>
+ </widget>
+ <widget class="QComboBox" name="comboBoxRNS">
+ <property name="geometry">
+ <rect>
+ <x>110</x>
+ <y>120</y>
+ <width>211</width>
+ <height>22</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QGroupBox" name="groupBox_4">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>190</y>
+ <width>311</width>
+ <height>111</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Collision Detection Setup</string>
+ </property>
+ <widget class="QComboBox" name="comboBoxColModelRobotStatic">
+ <property name="geometry">
+ <rect>
+ <x>130</x>
+ <y>50</y>
+ <width>171</width>
+ <height>22</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_10">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>50</y>
+ <width>111</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Robot B (e.g. body)</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_6">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>20</y>
+ <width>101</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Robot A (e.g arm)</string>
+ </property>
+ </widget>
+ <widget class="QComboBox" name="comboBoxColModelRobot">
+ <property name="geometry">
+ <rect>
+ <x>130</x>
+ <y>20</y>
+ <width>171</width>
+ <height>22</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_3">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>80</y>
+ <width>111</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Environment Set</string>
+ </property>
+ </widget>
+ <widget class="QComboBox" name="comboBoxColModelEnv">
+ <property name="geometry">
+ <rect>
+ <x>130</x>
+ <y>80</y>
+ <width>171</width>
+ <height>22</height>
+ </rect>
+ </property>
+ </widget>
+ </widget>
+ <widget class="QLabel" name="label_12">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>150</y>
+ <width>91</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>End Effector</string>
+ </property>
+ </widget>
+ <widget class="QComboBox" name="comboBoxEEF">
+ <property name="geometry">
+ <rect>
+ <x>110</x>
+ <y>150</y>
+ <width>211</width>
+ <height>22</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_13">
+ <property name="geometry">
+ <rect>
+ <x>150</x>
+ <y>310</y>
+ <width>91</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Min Grasp Score</string>
+ </property>
+ </widget>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxMinGraspScore">
+ <property name="geometry">
+ <rect>
+ <x>250</x>
+ <y>310</y>
+ <width>62</width>
+ <height>22</height>
+ </rect>
+ </property>
+ <property name="decimals">
+ <number>3</number>
+ </property>
+ <property name="minimum">
+ <double>0.001000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>1.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.001000000000000</double>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pushButtonGraspPose">
+ <property name="geometry">
+ <rect>
+ <x>240</x>
+ <y>350</y>
+ <width>81</width>
+ <height>21</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>grasppose</string>
+ </property>
+ </widget>
+ </widget>
+ <widget class="QGroupBox" name="groupBox_5">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>430</y>
+ <width>81</width>
+ <height>91</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Planning</string>
+ </property>
+ <widget class="QPushButton" name="pushButtonPlan">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>30</y>
+ <width>51</width>
+ <height>41</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Plan</string>
+ </property>
+ </widget>
+ </widget>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QMenuBar" name="menubar">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1055</width>
+ <height>21</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QStatusBar" name="statusbar"/>
+ </widget>
+ <resources/>
+ <connections>
+ <connection>
+ <sender>horizontalSliderPos</sender>
+ <signal>sliderMoved(int)</signal>
+ <receiver>lcdNumber</receiver>
+ <slot>display(int)</slot>
+ <hints>
+ <hint type="sourcelabel">
+ <x>700</x>
+ <y>375</y>
+ </hint>
+ <hint type="destinationlabel">
+ <x>751</x>
+ <y>397</y>
+ </hint>
+ </hints>
+ </connection>
+ </connections>
+</ui>
diff --git a/MotionPlanning/examples/GraspRRT/GraspRrtDemo.cpp b/MotionPlanning/examples/GraspRRT/GraspRrtDemo.cpp
new file mode 100644
index 000000000..8ba1e6176
--- /dev/null
+++ b/MotionPlanning/examples/GraspRRT/GraspRrtDemo.cpp
@@ -0,0 +1,119 @@
+
+#include "GraspRrtWindow.h"
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobotException.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/RuntimeEnvironment.h>
+
+#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/Qt/SoQt.h>
+#include <boost/shared_ptr.hpp>
+#include <string>
+#include <iostream>
+
+using std::cout;
+using std::endl;
+using namespace VirtualRobot;
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+
+int main (int argc, char** argv)
+{
+ SoDB::init();
+ SoQt::init(argc,argv,"GraspRrtDemo");
+ cout << " --- START --- " << endl;
+
+ std::string filenameScene(SABA_BASE_DIR "/examples/GraspRRT/scenes/planning.xml");
+ std::string startConfig("init");
+ std::string goalObject("Can");
+ std::string rnsName("Planning Left");
+ std::string colModel1("ColModel Robot Moving Left");
+ std::string colModel2("ColModel Robot Body");
+ std::string colModel3("ColModel Obstacles");
+ std::string eefName("Hand L");
+
+ std::string rnsNameB("Planning Right");
+ std::string colModel1B("ColModel Robot Moving Right");
+ std::string colModel2B("ColModel Robot Body");
+ std::string eefNameB("Hand R");
+
+ VirtualRobot::RuntimeEnvironment::considerKey("scene");
+ VirtualRobot::RuntimeEnvironment::considerKey("startConfig");
+ VirtualRobot::RuntimeEnvironment::considerKey("targetObject");
+ VirtualRobot::RuntimeEnvironment::considerKey("robotNodeSet_A");
+ VirtualRobot::RuntimeEnvironment::considerKey("eef_A");
+ VirtualRobot::RuntimeEnvironment::considerKey("colModelRobot1_A");
+ VirtualRobot::RuntimeEnvironment::considerKey("colModelRobot2_A");
+ VirtualRobot::RuntimeEnvironment::considerKey("robotNodeSet_B");
+ VirtualRobot::RuntimeEnvironment::considerKey("eef_B");
+ VirtualRobot::RuntimeEnvironment::considerKey("colModelRobot1_B");
+ VirtualRobot::RuntimeEnvironment::considerKey("colModelRobot2_B");
+ VirtualRobot::RuntimeEnvironment::considerKey("colModelEnv");
+ VirtualRobot::RuntimeEnvironment::processCommandLine(argc,argv);
+ VirtualRobot::RuntimeEnvironment::print();
+
+ std::string scFile = VirtualRobot::RuntimeEnvironment::getValue("scene");
+ if (!scFile.empty() && VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(scFile))
+ filenameScene = scFile;
+ std::string sConf = VirtualRobot::RuntimeEnvironment::getValue("startConfig");
+ if (!sConf.empty())
+ startConfig = sConf;
+ std::string gConf = VirtualRobot::RuntimeEnvironment::getValue("targetObject");
+ if (!gConf.empty())
+ goalObject = gConf;
+ std::string rns = VirtualRobot::RuntimeEnvironment::getValue("robotNodeSet_A");
+ if (!rns.empty())
+ rnsName = rns;
+ std::string eef = VirtualRobot::RuntimeEnvironment::getValue("eef_A");
+ if (!eef.empty())
+ eefName = eef;
+ std::string c1 = VirtualRobot::RuntimeEnvironment::getValue("colModelRobot1_A");
+ if (!c1.empty())
+ colModel1 = c1;
+ std::string c2 = VirtualRobot::RuntimeEnvironment::getValue("colModelRobot2_A");
+ if (!c2.empty())
+ colModel2 = c2;
+ rns = VirtualRobot::RuntimeEnvironment::getValue("robotNodeSet_B");
+ if (!rns.empty())
+ rnsNameB = rns;
+ eef = VirtualRobot::RuntimeEnvironment::getValue("eef_B");
+ if (!eef.empty())
+ eefNameB = eef;
+ c1 = VirtualRobot::RuntimeEnvironment::getValue("colModelRobot1_B");
+ if (!c1.empty())
+ colModel1B = c1;
+ c2 = VirtualRobot::RuntimeEnvironment::getValue("colModelRobot2_B");
+ if (!c2.empty())
+ colModel2B = c2;
+ std::string c3 = VirtualRobot::RuntimeEnvironment::getValue("colModelEnv");
+ if (!c3.empty())
+ colModel3 = c3;
+
+
+ cout << "Using scene: " << filenameScene << endl;
+ cout << "Using start config: <" << startConfig << ">" << endl;
+ cout << "Using target object: <" << goalObject << ">" << endl;
+ cout << "Using environment collision model set: <" << colModel3 << ">" << endl;
+ cout << "Set 1:" << endl;
+ cout << "\t Using RobotNodeSet for planning: <" << rnsName << ">" << endl;
+ cout << "\t Using EEF for grasping: <" << eefName << ">" << endl;
+ cout << "\t Using robot collision model sets: <" << colModel1 << "> and <" << colModel2 << ">" << endl;
+ cout << "Set 2:" << endl;
+ cout << "\t Using RobotNodeSet for planning: <" << rnsNameB << ">" << endl;
+ cout << "\t Using EEF for grasping: <" << eefNameB << ">" << endl;
+ cout << "\t Using robot collision model sets: <" << colModel1B << "> and <" << colModel2B << ">" << endl;
+
+
+ GraspRrtWindow rw(filenameScene, startConfig, goalObject,rnsName,rnsNameB,eefName,eefNameB,colModel1,colModel1B,colModel2,colModel2B,colModel3);
+
+ rw.main();
+
+ return 0;
+}
diff --git a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
new file mode 100644
index 000000000..791d43eb3
--- /dev/null
+++ b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
@@ -0,0 +1,749 @@
+
+#include "GraspRrtWindow.h"
+#include "VirtualRobot/EndEffector/EndEffector.h"
+#include "VirtualRobot/Workspace/Reachability.h"
+#include "VirtualRobot/ManipulationObject.h"
+#include "VirtualRobot/Grasping/Grasp.h"
+#include "VirtualRobot/IK/GenericIKSolver.h"
+#include "VirtualRobot/Grasping/GraspSet.h"
+#include "VirtualRobot/Obstacle.h"
+#include "VirtualRobot/CollisionDetection/CDManager.h"
+#include "VirtualRobot/XML/ObjectIO.h"
+#include "VirtualRobot/XML/RobotIO.h"
+#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h"
+#include "MotionPlanning/CSpace/CSpaceSampled.h"
+#include "MotionPlanning/Planner/Rrt.h"
+#include "MotionPlanning/Planner/GraspRrt.h"
+#include "MotionPlanning/PostProcessing/ShortcutProcessor.h"
+#include <MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h>
+#include <QFileDialog>
+#include <Eigen/Geometry>
+#include <time.h>
+#include <vector>
+#include <iostream>
+#include <cmath>
+
+#include "Inventor/actions/SoLineHighlightRenderAction.h"
+#include <Inventor/nodes/SoShapeHints.h>
+#include <Inventor/nodes/SoLightModel.h>
+#include <Inventor/sensors/SoTimerSensor.h>
+#include <Inventor/nodes/SoEventCallback.h>
+#include <Inventor/nodes/SoMatrixTransform.h>
+
+#include <sstream>
+using namespace std;
+using namespace VirtualRobot;
+
+float TIMER_MS = 200.0f;
+
+GraspRrtWindow::GraspRrtWindow(const std::string &sceneFile, const std::string &sConf, const std::string &goalObject,
+ const std::string &rns, const std::string &rnsB, const std::string &eefName, const std::string &eefNameB,
+ const std::string &colModelRob1, const std::string &colModelRob1B, const std::string &colModelRob2,const std::string &colModelRob2B,
+ const std::string &colModelEnv, Qt::WFlags flags)
+:QMainWindow(NULL)
+{
+ VR_INFO << " start " << endl;
+
+ this->sceneFile = sceneFile;
+
+ allSep = new SoSeparator;
+ allSep->ref();
+ sceneFileSep = new SoSeparator;
+ startGoalSep = new SoSeparator;
+ rrtSep = new SoSeparator;
+
+ allSep->addChild(sceneFileSep);
+ allSep->addChild(startGoalSep);
+ allSep->addChild(rrtSep);
+
+ planSetA.rns = rns;
+ planSetA.eef = eefName;
+ planSetA.colModelRob1 = colModelRob1;
+ planSetA.colModelRob2 = colModelRob2;
+ planSetA.colModelEnv = colModelEnv;
+
+ planSetB.rns = rnsB;
+ planSetB.eef = eefNameB;
+ planSetB.colModelRob1 = colModelRob1B;
+ planSetB.colModelRob2 = colModelRob2B;
+ planSetB.colModelEnv = colModelEnv;
+ setupUI();
+
+ loadScene();
+
+
+
+ selectPlanSet(0);
+
+ selectStart(sConf);
+ selectTargetObject(goalObject);
+
+
+
+ if (sConf!="")
+ UI.comboBoxStart->setEnabled(false);
+ if (goalObject!="")
+ UI.comboBoxGoal->setEnabled(false);
+ if (rns!="")
+ UI.comboBoxRNS->setEnabled(false);
+ //if (eefName!="")
+ // UI.comboBoxEEF->setEnabled(false);
+ if (colModelRob1!="")
+ UI.comboBoxColModelRobot->setEnabled(false);
+ if (colModelRob2!="")
+ UI.comboBoxColModelRobotStatic->setEnabled(false);
+ //if (colModelEnv!="")
+ // UI.comboBoxColModelEnv->setEnabled(false);
+
+ viewer->viewAll();
+
+ SoSensorManager *sensor_mgr = SoDB::getSensorManager();
+ SoTimerSensor *timer = new SoTimerSensor(timerCB, this);
+ timer->setInterval(SbTime(TIMER_MS/1000.0f));
+ sensor_mgr->insertTimerSensor(timer);
+}
+
+
+GraspRrtWindow::~GraspRrtWindow()
+{
+ allSep->unref();
+}
+
+
+void GraspRrtWindow::timerCB(void * data, SoSensor * sensor)
+{
+ GraspRrtWindow *ikWindow = static_cast<GraspRrtWindow*>(data);
+ ikWindow->redraw();
+}
+
+
+void GraspRrtWindow::setupUI()
+{
+ UI.setupUi(this);
+ viewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP);
+
+ // setup
+ viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
+ viewer->setAccumulationBuffer(true);
+
+ viewer->setAntialiasing(true, 4);
+
+ viewer->setGLRenderAction(new SoLineHighlightRenderAction);
+ viewer->setTransparencyType(SoGLRenderAction::BLEND);
+ viewer->setFeedbackVisibility(true);
+ viewer->setSceneGraph(allSep);
+ viewer->viewAll();
+
+ UI.radioButtonSolution->setChecked(true);
+
+ connect(UI.pushButtonLoad, SIGNAL(clicked()), this, SLOT(loadSceneWindow()));
+ connect(UI.checkBoxShowSolution, SIGNAL(clicked()), this, SLOT(buildVisu()));
+ connect(UI.checkBoxShowSolutionOpti, SIGNAL(clicked()), this, SLOT(buildVisu()));
+ connect(UI.checkBoxShowRRT, SIGNAL(clicked()), this, SLOT(buildVisu()));
+ connect(UI.checkBoxStartGoal, SIGNAL(clicked()), this, SLOT(buildVisu()));
+ connect(UI.checkBoxColModel, SIGNAL(clicked()), this, SLOT(colModel()));
+ connect(UI.pushButtonPlan, SIGNAL(clicked()), this, SLOT(plan()));
+ connect(UI.horizontalSliderPos, SIGNAL(sliderMoved(int)), this, SLOT(sliderSolution(int)));
+ connect(UI.radioButtonSolution, SIGNAL(clicked()), this, SLOT(solutionSelected()));
+ connect(UI.radioButtonSolutionOpti, SIGNAL(clicked()), this, SLOT(solutionSelected()));
+
+ connect(UI.comboBoxStart, SIGNAL(activated(int)), this, SLOT(selectStart(int)));
+ connect(UI.comboBoxGoal, SIGNAL(activated(int)), this, SLOT(selectTargetObject(int)));
+ connect(UI.comboBoxRNS, SIGNAL(activated(int)), this, SLOT(selectRNS(int)));
+ connect(UI.comboBoxEEF, SIGNAL(activated(int)), this, SLOT(selectEEF(int)));
+ connect(UI.comboBoxColModelRobot, SIGNAL(activated(int)), this, SLOT(selectColModelRobA(int)));
+ connect(UI.comboBoxColModelRobotStatic, SIGNAL(activated(int)), this, SLOT(selectColModelRobB(int)));
+ connect(UI.comboBoxColModelEnv, SIGNAL(activated(int)), this, SLOT(selectColModelEnv(int)));
+
+ connect(UI.pushButtonGraspPose, SIGNAL(clicked()), this, SLOT(testGraspPose()));
+
+}
+
+
+
+
+void GraspRrtWindow::closeEvent(QCloseEvent *event)
+{
+ quit();
+ QMainWindow::closeEvent(event);
+}
+
+
+void GraspRrtWindow::buildVisu()
+{
+ sceneFileSep->removeAllChildren();
+
+ SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked())?SceneObject::Collision:SceneObject::Full;
+
+ if (scene)
+ {
+ visualization = scene->getVisualization<CoinVisualization>(colModel);
+ SoNode* visualisationNode = NULL;
+ if (visualization)
+ visualisationNode = visualization->getCoinVisualization();
+
+ if (visualisationNode)
+ sceneFileSep->addChild(visualisationNode);
+ }
+
+ startGoalSep->removeAllChildren();
+ if (UI.checkBoxStartGoal->isChecked())
+ {
+ if (robotStart)
+ {
+ SoNode *st = CoinVisualizationFactory::getCoinVisualization(robotStart,colModel);
+ if (st)
+ startGoalSep->addChild(st);
+ }
+ if (robotGoal)
+ {
+ SoNode *go = CoinVisualizationFactory::getCoinVisualization(robotGoal,colModel);
+ if (go)
+ startGoalSep->addChild(go);
+ }
+ }
+ buildRRTVisu();
+
+ redraw();
+}
+
+int GraspRrtWindow::main()
+{
+ SoQt::show(this);
+ SoQt::mainLoop();
+ return 0;
+}
+
+
+void GraspRrtWindow::quit()
+{
+ std::cout << "GraspRrtWindow: Closing" << std::endl;
+ this->close();
+ SoQt::exitMainLoop();
+}
+
+void GraspRrtWindow::loadSceneWindow()
+{
+ QString fi = QFileDialog::getOpenFileName(this, tr("Open Scene File"), QString(), tr("XML Files (*.xml)"));
+ if (fi=="")
+ return;
+ sceneFile = std::string(fi.toAscii());
+ loadScene();
+}
+
+void GraspRrtWindow::loadScene()
+{
+ robot.reset();
+ scene = SceneIO::loadScene(sceneFile);
+ if (!scene)
+ {
+ VR_ERROR << " no scene ..." << endl;
+ return;
+ }
+ std::vector< RobotPtr > robots = scene->getRobots();
+ if (robots.size()!=1)
+ {
+ VR_ERROR << "Need exactly 1 robot" << endl;
+ return;
+ }
+ robot = robots[0];
+ robotStart = robot->clone("StartConfig");
+ robotGoal = robot->clone("GoalConfig");
+
+ // setup start Config combo box
+ configs = scene->getRobotConfigs(robot);
+ if (configs.size()<1)
+ {
+ VR_ERROR << "Need at least 1 Robot Configurations" << endl;
+ return;
+ }
+ UI.comboBoxStart->clear();
+ for (size_t i=0; i < configs.size(); i++)
+ {
+ QString qtext = configs[i]->getName().c_str();
+ UI.comboBoxStart->addItem(qtext);
+
+ }
+ UI.comboBoxStart->setCurrentIndex(0);
+ selectStart(0);
+
+ // setup target object combo box
+ obstacles = scene->getObstacles();
+ if (obstacles.size()<1)
+ {
+ VR_ERROR << "Need at least 1 Obstacle (target object)" << endl;
+ return;
+ }
+ UI.comboBoxGoal->clear();
+ for (size_t i=0; i < obstacles.size(); i++)
+ {
+ QString qtext = obstacles[i]->getName().c_str();
+ UI.comboBoxGoal->addItem(qtext);
+
+ }
+ UI.comboBoxGoal->setCurrentIndex(0);
+ //selectTargetObject(0);
+
+ // steup scene objects (col models env)
+ std::vector<SceneObjectSetPtr> soss = scene->getSceneObjectSets();
+ UI.comboBoxColModelEnv->clear();
+ QString qtext;
+ for (size_t i=0; i < soss.size(); i++)
+ {
+ qtext = soss[i]->getName().c_str();
+ UI.comboBoxColModelEnv->addItem(qtext);
+ }
+ qtext = "<none>";
+ UI.comboBoxColModelEnv->addItem(qtext);
+
+ //setup eefs
+ if (!robot->hasEndEffector(planSetA.eef))
+ {
+ VR_ERROR << "EEF with name " << planSetA.eef << " not known?!" << endl;
+ return;
+ }
+ UI.comboBoxEEF->clear();
+ qtext = planSetA.eef.c_str();
+ UI.comboBoxEEF->addItem(qtext);
+ if (robot->hasEndEffector(planSetB.eef))
+ {
+ qtext = planSetB.eef.c_str();
+ UI.comboBoxEEF->addItem(qtext);
+ }
+
+ /*std::vector<EndEffectorPtr> eefs = robot->getEndEffectors();
+ for (size_t i=0; i < eefs.size(); i++)
+ {
+ qtext = planSetA.eef.c_str();
+ UI.comboBoxEEF->addItem(qtext);
+ }*/
+
+ // Setup robot node sets and col models
+ std::vector<RobotNodeSetPtr> rnss = robot->getRobotNodeSets();
+ UI.comboBoxColModelRobot->clear();
+ UI.comboBoxColModelRobotStatic->clear();
+ UI.comboBoxRNS->clear();
+
+ for (size_t i=0; i < rnss.size(); i++)
+ {
+ qtext = rnss[i]->getName().c_str();
+ UI.comboBoxColModelRobot->addItem(qtext);
+ UI.comboBoxColModelRobotStatic->addItem(qtext);
+ UI.comboBoxRNS->addItem(qtext);
+ }
+ qtext = "<none>";
+ UI.comboBoxColModelRobot->addItem(qtext);
+ UI.comboBoxColModelRobotStatic->addItem(qtext);
+ robot->setThreadsafe(false);
+ buildVisu();
+}
+
+void GraspRrtWindow::selectStart(const std::string &conf)
+{
+ for (size_t i=0;i<configs.size();i++)
+ {
+ if (configs[i]->getName()==conf)
+ {
+ selectStart(i);
+ UI.comboBoxStart->setCurrentIndex(i);
+ return;
+ }
+ }
+ VR_ERROR << "No configuration with name <" << conf << "> found..." << endl;
+}
+
+void GraspRrtWindow::selectTargetObject(const std::string &conf)
+{
+ for (size_t i=0;i<obstacles.size();i++)
+ {
+ if (obstacles[i]->getName()==conf)
+ {
+ selectTargetObject(i);
+ UI.comboBoxGoal->setCurrentIndex(i);
+ return;
+ }
+ }
+ VR_ERROR << "No obstacle with name <" << conf << "> found..." << endl;
+}
+
+void GraspRrtWindow::selectRNS(const std::string &rns)
+{
+ if (!robot)
+ return;
+ std::vector< RobotNodeSetPtr > rnss = robot->getRobotNodeSets();
+ for (size_t i=0;i<rnss.size();i++)
+ {
+ if (rnss[i]->getName()==rns)
+ {
+ selectRNS(i);
+ UI.comboBoxRNS->setCurrentIndex(i);
+ return;
+ }
+ }
+ VR_ERROR << "No rns with name <" << rns << "> found..." << endl;
+}
+
+void GraspRrtWindow::selectEEF(const std::string &eefName)
+{
+ if (!robot)
+ return;
+
+ if (eefName==planSetA.eef && UI.comboBoxEEF->count()>0)
+ {
+ //selectEEF(0);
+ UI.comboBoxEEF->setCurrentIndex(0);
+ this->eef = robot->getEndEffector(eefName);
+ } else if (eefName==planSetB.eef && UI.comboBoxEEF->count()>1)
+ {
+ //selectEEF(1);
+ UI.comboBoxEEF->setCurrentIndex(1);
+ this->eef = robot->getEndEffector(eefName);
+ } else
+ {
+ VR_ERROR << "No eef with name <" << eefName << "> found..." << endl;
+ return;
+ }
+
+ /*std::vector< EndEffectorPtr > eefs = robot->getEndEffectors();
+ for (size_t i=0;i<eefs.size();i++)
+ {
+ if (eefs[i]->getName()==eefName)
+ {
+ selectEEF(i);
+ UI.comboBoxEEF->setCurrentIndex(i);
+ return;
+ }
+ }
+ VR_ERROR << "No eef with name <" << eefName << "> found..." << endl;*/
+}
+
+void GraspRrtWindow::selectColModelRobA(const std::string &colModel)
+{
+ if (!robot)
+ return;
+ std::vector< RobotNodeSetPtr > rnss = robot->getRobotNodeSets();
+ for (size_t i=0;i<rnss.size();i++)
+ {
+ if (rnss[i]->getName()==colModel)
+ {
+ selectColModelRobA(i);
+ UI.comboBoxColModelRobot->setCurrentIndex(i);
+ return;
+ }
+ }
+ VR_ERROR << "No col model set with name <" << colModel << "> found..." << endl;
+}
+void GraspRrtWindow::selectColModelRobB(const std::string &colModel)
+{
+ if (!robot)
+ return;
+ std::vector< RobotNodeSetPtr > rnss = robot->getRobotNodeSets();
+ for (size_t i=0;i<rnss.size();i++)
+ {
+ if (rnss[i]->getName()==colModel)
+ {
+ selectColModelRobB(i);
+ UI.comboBoxColModelRobotStatic->setCurrentIndex(i);
+ return;
+ }
+ }
+ VR_ERROR << "No col model set with name <" << colModel << "> found..." << endl;
+}
+void GraspRrtWindow::selectColModelEnv(const std::string &colModel)
+{
+ if (!scene)
+ return;
+ std::vector< SceneObjectSetPtr > rnss = scene->getSceneObjectSets();
+ for (size_t i=0;i<rnss.size();i++)
+ {
+ if (rnss[i]->getName()==colModel)
+ {
+ selectColModelEnv(i);
+ UI.comboBoxColModelEnv->setCurrentIndex(i);
+ return;
+ }
+ }
+ VR_ERROR << "No scene object set with name <" << colModel << "> found..." << endl;
+}
+
+void GraspRrtWindow::selectStart(int nr)
+{
+ if (nr<0 || nr>=(int)configs.size())
+ return;
+ if (robotStart)
+ configs[nr]->applyToRobot(robotStart);
+ if (robot)
+ configs[nr]->applyToRobot(robot);
+ configs[nr]->setJointValues();
+ if (rns)
+ rns->getJointValues(startConfig);
+}
+
+void GraspRrtWindow::selectTargetObject(int nr)
+{
+ if (nr<0 || nr>=(int)obstacles.size())
+ return;
+ //if (robotGoal)
+ // configs[nr]->applyToRobot(robotGoal);
+ //configs[nr]->setJointValues();
+ //if (rns)
+ // rns->getJointValues(goalConfig);
+ targetObject = obstacles[nr];
+ graspQuality.reset(new GraspStudio::GraspQualityMeasureWrenchSpace(targetObject));
+ int points = 400;
+#ifdef _DEBUG
+ points = 100;
+#endif
+ graspQuality->calculateOWS(points);
+}
+
+void GraspRrtWindow::selectRNS(int nr)
+{
+ this->rns.reset();
+ if (!robot)
+ return;
+ std::vector< RobotNodeSetPtr > rnss = robot->getRobotNodeSets();
+ if (nr<0 || nr>=(int)rnss.size())
+ return;
+ this->rns = rnss[nr];
+}
+
+void GraspRrtWindow::selectEEF(int nr)
+{
+ this->eef.reset();
+ if (!robot)
+ return;
+ selectPlanSet(nr);
+/* std::vector< EndEffectorPtr > eefs = robot->getEndEffectors();
+ if (nr<0 || nr>=(int)eefs.size())
+ return;
+ this->eef = eefs[nr];*/
+}
+
+void GraspRrtWindow::selectColModelRobA(int nr)
+{
+ colModelRobA.reset();
+ if (!robot)
+ return;
+ std::vector< RobotNodeSetPtr > rnss = robot->getRobotNodeSets();
+ if (nr<0 || nr>=(int)rnss.size())
+ return;
+ this->colModelRobA = robot->getSceneObjectSet(rnss[nr]->getName());
+}
+
+void GraspRrtWindow::selectColModelRobB(int nr)
+{
+ colModelRobB.reset();
+ if (!robot)
+ return;
+ std::vector< RobotNodeSetPtr > rnss = robot->getRobotNodeSets();
+ if (nr<0 || nr>=(int)rnss.size())
+ return;
+ this->colModelRobB = robot->getSceneObjectSet(rnss[nr]->getName());
+}
+
+void GraspRrtWindow::selectColModelEnv(int nr)
+{
+ colModelEnv.reset();
+ if (!scene)
+ return;
+ std::vector< SceneObjectSetPtr > rnss = scene->getSceneObjectSets();
+ if (nr<0 || nr>=(int)rnss.size())
+ return;
+ this->colModelEnv = scene->getSceneObjectSet(rnss[nr]->getName());
+}
+
+void GraspRrtWindow::buildRRTVisu()
+{
+ rrtSep->removeAllChildren();
+ if (!cspace || !robot)
+ return;
+
+ boost::shared_ptr<Saba::CoinRrtWorkspaceVisualization> w(new Saba::CoinRrtWorkspaceVisualization(robot,cspace,eef->getGCP()->getName()));
+ if (UI.checkBoxShowRRT->isChecked())
+ {
+ if (tree)
+ {
+ w->addTree(tree);
+ w->colorizeTreeNodes(2,Saba::RrtWorkspaceVisualization::eRed);
+ }
+ }
+ if (UI.checkBoxShowSolution->isChecked() && solution)
+ w->addCSpacePath(solution);
+ if (UI.checkBoxShowSolutionOpti->isChecked() && solutionOptimized)
+ w->addCSpacePath(solutionOptimized,Saba::CoinRrtWorkspaceVisualization::eGreen);
+ w->addConfiguration(startConfig,Saba::CoinRrtWorkspaceVisualization::eGreen,3.0f);
+ SoSeparator *sol = w->getCoinVisualization();
+ rrtSep->addChild(sol);
+}
+
+void GraspRrtWindow::testInit()
+{
+ // setup collision detection
+ if (!test_cspace)
+ {
+
+ CDManagerPtr cdm(new CDManager());
+ if (colModelRobA)
+ cdm->addCollisionModel(colModelRobA);
+ if (colModelRobB)
+ cdm->addCollisionModel(colModelRobB);
+ if (colModelEnv)
+ cdm->addCollisionModel(colModelEnv);
+
+ test_cspace.reset(new Saba::CSpaceSampled(robot,cdm,rns));
+ }
+ float sampl = (float)UI.doubleSpinBoxCSpaceSampling->value();
+ float samplDCD = (float)UI.doubleSpinBoxColChecking->value();
+ test_cspace->setSamplingSize(sampl);
+ test_cspace->setSamplingSizeDCD(samplDCD);
+ float minGraspScore = (float)UI.doubleSpinBoxMinGraspScore->value();
+
+ test_graspRrt.reset(new Saba::GraspRrt(test_cspace,eef,targetObject,graspQuality,colModelEnv,0.1f,minGraspScore));
+
+ test_graspRrt->setStart(startConfig);
+ eef->getGCP()->showCoordinateSystem(true);
+ test_graspRrt->init();
+}
+
+void GraspRrtWindow::testGraspPose()
+{
+ if (!robot || !rns || !eef || !graspQuality)
+ return;
+
+ if (!test_cspace || !test_graspRrt)
+ testInit();
+
+ // create taregt on objects surface
+ Eigen::Matrix4f globalGrasp;
+ Eigen::VectorXf c(rns->getSize());
+ //cspace->getRandomConfig(c);
+ //rns->setJointValues(c);
+ rns->getJointValues(c);
+ test_graspRrt->calculateGlobalGraspPose(c,globalGrasp);
+
+ VirtualRobot::ObstaclePtr o = VirtualRobot::Obstacle::createBox(10,10,10);
+ o->setGlobalPose(globalGrasp);
+ o->showCoordinateSystem(true);
+ startGoalSep->removeAllChildren();
+ startGoalSep->addChild(VirtualRobot::CoinVisualizationFactory::getCoinVisualization(o, VirtualRobot::SceneObject::Full));
+
+ // move towards object
+ Eigen::Matrix4f p = eef->getGCP()->getGlobalPose();
+ test_graspRrt->createWorkSpaceSamplingStep(p,globalGrasp,c);
+ rns->setJointValues(c);
+
+ // test
+ /*Eigen::Matrix4f p1;
+ p1.setIdentity();
+ Eigen::Matrix4f p2;
+ p2.setIdentity();
+ p1(0,3) = 50;
+ p2(0,3) = 200;
+ Eigen::Matrix4f deltaPose = p1.inverse() * p2;
+ cout << deltaPose << endl;
+
+ deltaPose = p2 * p1.inverse();
+ cout << deltaPose << endl;
+
+ p2 = deltaPose * p1;
+ cout << p2 << endl;
+ p2 = p1 * deltaPose;
+ cout << p2 << endl;*/
+}
+
+void GraspRrtWindow::plan()
+{
+ if (!robot || !rns || !eef || !graspQuality)
+ return;
+
+ // setup collision detection
+ CDManagerPtr cdm(new CDManager());
+ if (colModelRobA)
+ cdm->addCollisionModel(colModelRobA);
+ if (colModelRobB)
+ cdm->addCollisionModel(colModelRobB);
+ if (colModelEnv)
+ cdm->addCollisionModel(colModelEnv);
+
+ cspace.reset(new Saba::CSpaceSampled(robot,cdm,rns));
+ float sampl = (float)UI.doubleSpinBoxCSpaceSampling->value();
+ float samplDCD = (float)UI.doubleSpinBoxColChecking->value();
+ cspace->setSamplingSize(sampl);
+ cspace->setSamplingSizeDCD(samplDCD);
+ float minGraspScore = (float)UI.doubleSpinBoxMinGraspScore->value();
+
+ Saba::GraspRrtPtr graspRrt(new Saba::GraspRrt(cspace,eef,targetObject,graspQuality,colModelEnv,0.1f,minGraspScore));
+
+ graspRrt->setStart(startConfig);
+
+ bool planOk = false;
+ bool planOK = graspRrt->plan();
+ if (planOK)
+ {
+ VR_INFO << " Planning succeeded " << endl;
+ solution = graspRrt->getSolution();
+ Saba::ShortcutProcessorPtr postProcessing(new Saba::ShortcutProcessor(solution,cspace,false));
+ solutionOptimized = postProcessing->optimize(100);
+ tree = graspRrt->getTree();
+ } else
+ VR_INFO << " Planning failed" << endl;
+
+ sliderSolution(1000);
+
+ buildVisu();
+}
+
+void GraspRrtWindow::colModel()
+{
+ buildVisu();
+}
+void GraspRrtWindow::solutionSelected()
+{
+ sliderSolution(UI.horizontalSliderPos->sliderPosition());
+}
+void GraspRrtWindow::sliderSolution( int pos )
+{
+ if (!solution)
+ return;
+ Saba::CSpacePathPtr s = solution;
+ if (UI.radioButtonSolutionOpti->isChecked() && solutionOptimized)
+ s = solutionOptimized;
+ float p = (float)pos/1000.0f;
+ Eigen::VectorXf iPos;
+ s->interpolate(p,iPos);
+ rns->setJointValues(iPos);
+ redraw();
+}
+
+void GraspRrtWindow::redraw()
+{
+ viewer->scheduleRedraw();
+ UI.frameViewer->update();
+ viewer->scheduleRedraw();
+ this->update();
+ viewer->scheduleRedraw();
+}
+
+
+void GraspRrtWindow::selectPlanSet(int nr)
+{
+ if (nr==0)
+ {
+ selectRNS(planSetA.rns);
+ selectEEF(planSetA.eef);
+ selectColModelRobA(planSetA.colModelRob1);
+ selectColModelRobB(planSetA.colModelRob2);
+ selectColModelEnv(planSetA.colModelEnv);
+ selectStart(UI.comboBoxStart->currentIndex());
+ } else
+ {
+ selectRNS(planSetB.rns);
+ selectEEF(planSetB.eef);
+ selectColModelRobA(planSetB.colModelRob1);
+ selectColModelRobB(planSetB.colModelRob2);
+ selectColModelEnv(planSetB.colModelEnv);
+ selectStart(UI.comboBoxStart->currentIndex());
+ }
+}
+
diff --git a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.h b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.h
new file mode 100644
index 000000000..82e7b5d26
--- /dev/null
+++ b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.h
@@ -0,0 +1,152 @@
+#ifndef __GraspRrt_WINDOW_H__
+#define __GraspRrt_WINDOW_H__
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobotException.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/XML/SceneIO.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/Obstacle.h>
+#include <VirtualRobot/ManipulationObject.h>
+
+#include <GraspPlanning/GraspStudio.h>
+#include <GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h>
+
+#include "MotionPlanning/Saba.h"
+#include "MotionPlanning/CSpace/CSpacePath.h"
+
+#include <string.h>
+#include <QtCore/QtGlobal>
+#include <QtGui/QtGui>
+#include <QtCore/QtCore>
+
+#include <Inventor/sensors/SoTimerSensor.h>
+#include <Inventor/nodes/SoEventCallback.h>
+#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
+#include <Inventor/Qt/SoQt.h>
+#include <Inventor/nodes/SoSeparator.h>
+
+
+#include <vector>
+
+#include "ui_GraspRrt.h"
+
+class GraspRrtWindow : public QMainWindow
+{
+ Q_OBJECT
+public:
+ GraspRrtWindow( const std::string &sceneFile, const std::string &sConf, const std::string &gConf,
+ const std::string &rns, const std::string &rnsB, const std::string &eefName, const std::string &eefNameB,
+ const std::string &colModelRob1, const std::string &colModelRob1B, const std::string &colModelRob2,const std::string &colModelRob2B,
+ const std::string &colModelEnv, Qt::WFlags flags = 0);
+ ~GraspRrtWindow();
+
+ /*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
+ int main();
+
+ void redraw();
+public slots:
+ /*! Closes the window and exits SoQt runloop. */
+ void quit();
+
+ /*!< Overriding the close event, so we know when the window was closed by the user. */
+ void closeEvent(QCloseEvent *event);
+
+ void loadSceneWindow();
+
+ void selectStart(int nr);
+ void selectTargetObject(int nr);
+ void selectRNS(int nr);
+ void selectEEF(int nr);
+
+ void selectColModelRobA(int nr);
+ void selectColModelRobB(int nr);
+ void selectColModelEnv(int nr);
+
+ void colModel();
+ void solutionSelected();
+
+ void sliderSolution(int pos);
+
+ void buildVisu();
+
+ void plan();
+
+ void testGraspPose();
+
+protected:
+
+ struct planSet
+ {
+ std::string eef;
+ std::string rns;
+ std::string colModelRob1;
+ std::string colModelRob2;
+ std::string colModelEnv;
+ };
+
+ planSet planSetA,planSetB;
+
+
+ void loadScene();
+
+ void setupUI();
+ QString formatString(const char *s, float f);
+ void buildRRTVisu();
+ void selectStart(const std::string &conf);
+ void selectTargetObject(const std::string &conf);
+ void selectRNS(const std::string &rns);
+ void selectEEF(const std::string &eefName);
+
+ static void timerCB(void * data, SoSensor * sensor);
+ void buildRrtVisu();
+ void selectColModelRobA(const std::string &colModel);
+ void selectColModelRobB(const std::string &colModel);
+ void selectColModelEnv(const std::string &colModel);
+ void selectPlanSet(int nr);
+
+ void testInit();
+
+ Ui::MainWindowGraspRRTDemo UI;
+ SoQtExaminerViewer *viewer; /*!< Viewer to display the 3D model of the robot and the environment. */
+
+ SoSeparator *allSep;
+ SoSeparator *sceneFileSep;
+ SoSeparator *startGoalSep;
+ SoSeparator *rrtSep;
+
+ VirtualRobot::RobotPtr robot;
+ VirtualRobot::RobotPtr robotStart;
+ VirtualRobot::RobotPtr robotGoal;
+
+ Saba::CSpaceSampledPtr cspace;
+ Eigen::VectorXf startConfig;
+ Eigen::VectorXf goalConfig;
+
+ VirtualRobot::RobotNodeSetPtr rns;
+ VirtualRobot::EndEffectorPtr eef;
+ VirtualRobot::SceneObjectSetPtr colModelRobA;
+ VirtualRobot::SceneObjectSetPtr colModelRobB;
+ VirtualRobot::SceneObjectSetPtr colModelEnv;
+
+ std::vector< VirtualRobot::RobotConfigPtr > configs;
+ std::vector< VirtualRobot::ObstaclePtr > obstacles;
+ VirtualRobot::ObstaclePtr targetObject;
+
+ std::string sceneFile;
+ VirtualRobot::ScenePtr scene;
+
+ Saba::CSpacePathPtr solution;
+ Saba::CSpacePathPtr solutionOptimized;
+ Saba::CSpaceTreePtr tree;
+ GraspStudio::GraspQualityMeasureWrenchSpacePtr graspQuality;
+
+ boost::shared_ptr<VirtualRobot::CoinVisualization> visualization;
+
+ Saba::GraspRrtPtr test_graspRrt;
+ Saba::CSpaceSampledPtr test_cspace;
+};
+
+#endif // __GraspRrt_WINDOW_H__
diff --git a/MotionPlanning/examples/GraspRRT/scenes/planning.xml b/MotionPlanning/examples/GraspRRT/scenes/planning.xml
new file mode 100644
index 000000000..bb8182ef7
--- /dev/null
+++ b/MotionPlanning/examples/GraspRRT/scenes/planning.xml
@@ -0,0 +1,145 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+
+<Scene name="GraspRrtScene">
+
+ <Robot name="Armar-III" initConfig="init">
+ <File>/robots/ArmarIII/ArmarIII.xml</File>
+ <Configuration name="init">
+ <Node name="Hip Pitch" unit="radian" value="0"/>
+ <Node name="Hip Roll" unit="radian" value="0"/>
+ <Node name="Hip Yaw" unit="radian" value="0"/>
+ <Node name="Shoulder 1 L" unit="radian" value="0.26"/>
+ <Node name="Shoulder 2 L" unit="radian" value="0"/>
+ <Node name="Upperarm L" unit="radian" value="0.46"/>
+ <Node name="Elbow L" unit="radian" value="0.2"/>
+ <Node name="Underarm L" unit="radian" value="0"/>
+ <Node name="Wrist 1 L" unit="radian" value="0"/>
+ <Node name="Wrist 2 L" unit="radian" value="0"/>
+ <Node name="Shoulder 1 R" unit="radian" value="0.26"/>
+ <Node name="Shoulder 2 R" unit="radian" value="0"/>
+ <Node name="Upperarm R" unit="radian" value="-0.46"/>
+ <Node name="Elbow R" unit="radian" value="0.2"/>
+ <Node name="Underarm R" unit="radian" value="0"/>
+ <Node name="Wrist 1 R" unit="radian" value="0"/>
+ <Node name="Wrist 2 R" unit="radian" value="0"/>
+ </Configuration>
+
+
+ <!-- These joints are considered for motion planning-->
+ <RobotNodeSet name="Planning Left" kinematicRoot="Platform" tcp="TCP L">
+ <Node name="Hip Pitch"/>
+ <Node name="Hip Roll"/>
+ <Node name="Hip Yaw"/>
+ <Node name="Shoulder 1 L"/>
+ <Node name="Shoulder 2 L"/>
+ <Node name="Upperarm L"/>
+ <Node name="Elbow L"/>
+ <Node name="Underarm L"/>
+ <Node name="Wrist 1 L"/>
+ <Node name="Wrist 2 L"/>
+ </RobotNodeSet>
+ <RobotNodeSet name="Planning Right" kinematicRoot="Platform" tcp="TCP R">
+ <Node name="Hip Pitch"/>
+ <Node name="Hip Roll"/>
+ <Node name="Hip Yaw"/>
+ <Node name="Shoulder 1 R"/>
+ <Node name="Shoulder 2 R"/>
+ <Node name="Upperarm R"/>
+ <Node name="Elbow R"/>
+ <Node name="Underarm R"/>
+ <Node name="Wrist 1 R"/>
+ <Node name="Wrist 2 R"/>
+ </RobotNodeSet>
+
+ <!-- The first collision model (arm and hand) -->
+ <RobotNodeSet name="ColModel Robot Moving Left">
+ <Node name="Upperarm L"/>
+ <Node name="Underarm L"/>
+ <Node name="Hand Palm 1 L"/>
+ <Node name="Hand Palm 2 L"/>
+ <Node name="Thumb L J0"/>
+ <Node name="Thumb L J1"/>
+ <Node name="Index L J0"/>
+ <Node name="Index L J1"/>
+ <Node name="Middle L J0"/>
+ <Node name="Middle L J1"/>
+ <Node name="Ring L J0"/>
+ <Node name="Ring L J1"/>
+ <Node name="Pinky L J0"/>
+ <Node name="Pinky L J1"/>
+ </RobotNodeSet>
+
+ <RobotNodeSet name="ColModel Robot Moving Right">
+ <Node name="Upperarm R"/>
+ <Node name="Underarm R"/>
+ <Node name="Hand Palm 1 R"/>
+ <Node name="Hand Palm 2 R"/>
+ <Node name="Thumb R J0"/>
+ <Node name="Thumb R J1"/>
+ <Node name="Index R J0"/>
+ <Node name="Index R J1"/>
+ <Node name="Middle R J0"/>
+ <Node name="Middle R J1"/>
+ <Node name="Ring R J0"/>
+ <Node name="Ring R J1"/>
+ <Node name="Pinky R J0"/>
+ <Node name="Pinky R J1"/>
+ </RobotNodeSet>
+
+ <!-- The second collision model (torso, head and platform)-->
+ <RobotNodeSet name="ColModel Robot Body">
+ <Node name="Platform"/>
+ <Node name="Hip Yaw"/>
+ <Node name="Head_Tilt"/>
+ </RobotNodeSet>
+
+ <GlobalPose>
+ <Transform>
+ <Translation x="-6115.0" y="3580.0" z="0"/>
+ <rollpitchyaw units="degree" roll="0" pitch="0" yaw="0"/>
+ </Transform>
+ </GlobalPose>
+ </Robot>
+
+ <Obstacle name="Can">
+ <Visualization>
+ <File type='inventor'>objects/iv/can_online.wrl</File>
+ <UseAsCollisionModel/>
+ </Visualization>
+ <GlobalPose>
+ <Transform>
+ <Translation x="-6170" y="4200" z="930"/>
+ <rollpitchyaw units="degree" roll="0" pitch="0" yaw="0"/>
+ </Transform>
+ </GlobalPose>
+ </Obstacle>
+
+
+ <Obstacle name="Vitalis">
+ <Visualization>
+ <File type='inventor'>objects/iv/vitalis.iv</File>
+ <UseAsCollisionModel/>
+ </Visualization>
+ <GlobalPose>
+ <Transform>
+ <Translation x="-6025" y="4150" z="1025"/>
+ <rollpitchyaw units="degree" roll="90" pitch="0" yaw="90"/>
+ </Transform>
+ </GlobalPose>
+ </Obstacle>
+
+ <Obstacle name="Environment">
+ <Visualization>
+ <File type='inventor'>environment/KIT_kitchen.wrl</File>
+ </Visualization>
+ <CollisionModel>
+ <File type='inventor'>environment/KIT_kitchen_sideboard.wrl</File>
+ </CollisionModel>
+ </Obstacle>
+
+ <SceneObjectSet name="ColModel Obstacles">
+ <SceneObject name="Environment"/>
+ <SceneObject name="Vitalis"/>
+ </SceneObjectSet>
+
+</Scene>
\ No newline at end of file
diff --git a/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp b/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp
index 444754b83..f04baccf3 100644
--- a/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp
+++ b/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp
@@ -3,9 +3,9 @@
#include "VirtualRobot/EndEffector/EndEffector.h"
#include "VirtualRobot/Workspace/Reachability.h"
#include "VirtualRobot/ManipulationObject.h"
-#include "VirtualRobot/Grasp.h"
+#include "VirtualRobot/Grasping/Grasp.h"
#include "VirtualRobot/IK/GenericIKSolver.h"
-#include "VirtualRobot/GraspSet.h"
+#include "VirtualRobot/Grasping/GraspSet.h"
#include "VirtualRobot/CollisionDetection/CDManager.h"
#include "VirtualRobot/XML/ObjectIO.h"
#include "VirtualRobot/XML/RobotIO.h"
diff --git a/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp b/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp
index c5978454f..7d1952e51 100644
--- a/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp
+++ b/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp
@@ -3,9 +3,9 @@
#include "VirtualRobot/EndEffector/EndEffector.h"
#include "VirtualRobot/Workspace/Reachability.h"
#include "VirtualRobot/ManipulationObject.h"
-#include "VirtualRobot/Grasp.h"
+#include "VirtualRobot/Grasping/Grasp.h"
#include "VirtualRobot/IK/GenericIKSolver.h"
-#include "VirtualRobot/GraspSet.h"
+#include "VirtualRobot/Grasping/GraspSet.h"
#include "VirtualRobot/CollisionDetection/CDManager.h"
#include "VirtualRobot/XML/ObjectIO.h"
#include "VirtualRobot/XML/RobotIO.h"
--
GitLab