diff --git a/GraspPlanning/ApproachMovementGenerator.cpp b/GraspPlanning/ApproachMovementGenerator.cpp
index 9a651a1fb64f66e6f6ca71a6bf4fafe0bf82b1df..e50e315378682e272ed7e043e625c04d454ca3d3 100644
--- a/GraspPlanning/ApproachMovementGenerator.cpp
+++ b/GraspPlanning/ApproachMovementGenerator.cpp
@@ -37,6 +37,7 @@ namespace GraspStudio
THROW_VR_EXCEPTION_IF(!eef_cloned->hasPreshape(graspPreshape), "Preshape with name " << graspPreshape << " not present in EEF");
eef_cloned->setPreshape(graspPreshape);
}
+ aporachDirGlobal << 1.0f, 0, 0;
}
ApproachMovementGenerator::~ApproachMovementGenerator()
@@ -101,6 +102,11 @@ namespace GraspStudio
return eef;
}
+ Eigen::Vector3f ApproachMovementGenerator::getApproachDirGlobal()
+ {
+ return aporachDirGlobal;
+ }
+
std::string ApproachMovementGenerator::getName()
{
return name;
diff --git a/GraspPlanning/ApproachMovementGenerator.h b/GraspPlanning/ApproachMovementGenerator.h
index 006a0b8733f49b0d9812294cbc22742decf110d0..9484098463abbfd383de71664429842cab10c718 100644
--- a/GraspPlanning/ApproachMovementGenerator.h
+++ b/GraspPlanning/ApproachMovementGenerator.h
@@ -83,6 +83,8 @@ namespace GraspStudio
VirtualRobot::EndEffectorPtr getEEF();
VirtualRobot::EndEffectorPtr getEEFOriginal();
+ Eigen::Vector3f getApproachDirGlobal();
+
std::string getName();
protected:
@@ -92,6 +94,8 @@ namespace GraspStudio
VirtualRobot::TriMeshModelPtr objectModel;
VirtualRobot::EndEffectorPtr eef;
+ Eigen::Vector3f aporachDirGlobal;
+
//! This robot is moved around
VirtualRobot::RobotPtr eefRobot;
VirtualRobot::EndEffectorPtr eef_cloned;
diff --git a/GraspPlanning/ApproachMovementSurfaceNormal.cpp b/GraspPlanning/ApproachMovementSurfaceNormal.cpp
index 87cc1915f7e7980b9731bc5106410280e087df68..425df2d1942695d01d45f36dd14c6f5a3bfd617a 100644
--- a/GraspPlanning/ApproachMovementSurfaceNormal.cpp
+++ b/GraspPlanning/ApproachMovementSurfaceNormal.cpp
@@ -41,7 +41,6 @@ namespace GraspStudio
int nVert3 = (objectModel->faces[nRandFace]).id3;
storePos = VirtualRobot::MathTools::randomPointInTriangle(objectModel->vertices[nVert1], objectModel->vertices[nVert2], objectModel->vertices[nVert3]);
-
//storePos = (objectModel->vertices[nVert1] + objectModel->vertices[nVert2] + objectModel->vertices[nVert3]) / 3.0f;
/*position(0) = (objectModel->vertices[nVert1].x + objectModel->vertices[nVert2].x + objectModel->vertices[nVert3].x) / 3.0f;
position(1) = (objectModel->vertices[nVert1].y + objectModel->vertices[nVert2].y + objectModel->vertices[nVert3].y) / 3.0f;
@@ -65,12 +64,14 @@ namespace GraspStudio
return pose;
}
+ this->aporachDirGlobal = approachDir;
// set new pose
- setEEFToApproachPose(position, approachDir);
+ setEEFToApproachPose(position,approachDir);
+
// move away until valid
- moveEEFAway(approachDir, 3.0f);
+ moveEEFAway(approachDir, 1.0f);
Eigen::Matrix4f poseB = getEEFPose();
@@ -196,5 +197,4 @@ namespace GraspStudio
loop++;
}
}
-
}
diff --git a/GraspPlanning/ApproachMovementSurfaceNormal.h b/GraspPlanning/ApproachMovementSurfaceNormal.h
index 49ef1767ff7088594f0414683e05ba43bd38b272..bb68179fb459cc4399f43d0e6c02e46d857395bb 100644
--- a/GraspPlanning/ApproachMovementSurfaceNormal.h
+++ b/GraspPlanning/ApproachMovementSurfaceNormal.h
@@ -64,10 +64,11 @@ namespace GraspStudio
bool getPositionOnObject(Eigen::Vector3f& storePos, Eigen::Vector3f& storeApproachDir);
//! Sets EEF to a position so that the Z component of the GCP coord system is aligned with -approachDir
- bool setEEFToApproachPose(const Eigen::Vector3f& position, const Eigen::Vector3f& approachDir);
+ bool virtual setEEFToApproachPose(const Eigen::Vector3f& position, const Eigen::Vector3f& approachDir);
void moveEEFAway(const Eigen::Vector3f& approachDir, float step, int maxLoops = 1000);
+
protected:
float randomDistanceMax;
diff --git a/VirtualRobot/Grasping/GraspSet.cpp b/VirtualRobot/Grasping/GraspSet.cpp
index 62cf50eb596234cb4d782407456bbfd645e0649e..6e2b3fd258266fddc1e8a2c3a45f934605cd3839 100644
--- a/VirtualRobot/Grasping/GraspSet.cpp
+++ b/VirtualRobot/Grasping/GraspSet.cpp
@@ -63,6 +63,19 @@ namespace VirtualRobot
grasps.clear();
}
+ void GraspSet::includeGraspSet(GraspSetPtr grasps)
+ {
+ std::vector<GraspPtr> includedGrasp=grasps->getGrasps();
+
+ for(size_t i=0;i<includedGrasp.size();i++)
+ {
+ if(!hasGrasp(includedGrasp.at(i)))
+ {
+ addGrasp(includedGrasp.at(i));
+ }
+ }
+ }
+
void GraspSet::print()
{
cout << "**** Grasp set ****" << endl;
diff --git a/VirtualRobot/Grasping/GraspSet.h b/VirtualRobot/Grasping/GraspSet.h
index 08820dd3e4b816f90e7430f5a95e56e874ac0282..8b1e2ef4824c3618628042b90d242330bfda66a7 100644
--- a/VirtualRobot/Grasping/GraspSet.h
+++ b/VirtualRobot/Grasping/GraspSet.h
@@ -29,6 +29,11 @@
#include <string>
#include <vector>
+
+
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include "Grasp.h"
+
#include <Eigen/Core>
namespace VirtualRobot
@@ -53,11 +58,13 @@ namespace VirtualRobot
void addGrasp(GraspPtr grasp);
bool hasGrasp(GraspPtr grasp);
+
bool hasGrasp(const std::string& name);
bool removeGrasp(GraspPtr grasp);
void removeAllGrasps();
bool isCompatibleGrasp(GraspPtr grasp);
void clear();
+ void includeGraspSet(GraspSetPtr grasps);
/*!
Return number of grasps stored in this set.
diff --git a/VirtualRobot/ManipulationObject.cpp b/VirtualRobot/ManipulationObject.cpp
index d3cc88020f95af718115fd9036565d8189955be9..b9b581484a04cc701fc28281aa2d5d9dcdc4cf9b 100644
--- a/VirtualRobot/ManipulationObject.cpp
+++ b/VirtualRobot/ManipulationObject.cpp
@@ -47,6 +47,26 @@ namespace VirtualRobot
this->graspSets.push_back(graspSet);
}
+ void ManipulationObject::includeGraspSet(GraspSetPtr toBeIncludedGraspSet) //maybe delete
+ {
+ THROW_VR_EXCEPTION_IF(!toBeIncludedGraspSet,"NULL data");
+ std::string robotType=toBeIncludedGraspSet->getRobotType();
+ std::string eef=toBeIncludedGraspSet->getEndEffector();
+
+ //include new Grasps
+ //check if grasp is existing
+ int index=-1;
+ for(size_t i = 0 ; i < graspSets.size(); i++ )
+ {
+ if (graspSets.at(i)->getRobotType() == robotType && graspSets.at(i)->getEndEffector() == eef)
+ {
+ index = i;
+ }
+ }
+ THROW_VR_EXCEPTION_IF(index==-1,"Index wrong defined");
+ graspSets.at(index)->includeGraspSet(toBeIncludedGraspSet);
+ }
+
bool ManipulationObject::hasGraspSet(GraspSetPtr graspSet)
{
VR_ASSERT_MESSAGE(graspSet, "NULL data");
@@ -102,6 +122,11 @@ namespace VirtualRobot
return GraspSetPtr();
}
+ std::vector<GraspSetPtr> ManipulationObject::getAllGraspSets()
+ {
+ return graspSets;
+ }
+
std::string ManipulationObject::toXML(const std::string& basePath, int tabs, bool storeLinkToFile)
{
std::stringstream ss;
diff --git a/VirtualRobot/ManipulationObject.h b/VirtualRobot/ManipulationObject.h
index 24fb053cd38e775ec8ba929e5389f77a39718205..4dc7dac1357dc5644a613b806817a126f29d870b 100644
--- a/VirtualRobot/ManipulationObject.h
+++ b/VirtualRobot/ManipulationObject.h
@@ -54,6 +54,12 @@ namespace VirtualRobot
*/
void addGraspSet(GraspSetPtr graspSet);
+ /*!
+ * \brief includeGraspSet
+ * \param graspSet
+ */
+ void includeGraspSet(GraspSetPtr graspSet);
+
/*!
Get grasp set for the given end effector. In case multiple grasp sets for the eef are present, the first one is returned.
An empty GraspSetPtr is returned when no GraspSet for eef is found.
@@ -73,6 +79,11 @@ namespace VirtualRobot
*/
GraspSetPtr getGraspSet(const std::string& name);
+ /*!
+ Get grasp set vector
+ */
+ std::vector<GraspSetPtr> getAllGraspSets();
+
/*!
Creates an XML representation of this object.
\param basePath If set, all visualization and collision model files are made relative to this path.