diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
index 54cfc67cba0898e48eddc02a79f2ebce36d0604d..cb065f58bb9a63f6c0cd284c5139785e45eab24a 100644
--- a/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
+++ b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
@@ -82,7 +82,7 @@ VirtualRobot::GraspPtr GenericGraspPlanner::planGrasp()
if (!bRes)
return VirtualRobot::GraspPtr();
- std::vector< VirtualRobot::EndEffector::ContactInfo > contacts;
+ VirtualRobot::EndEffector::ContactInfoVector contacts;
contacts = eef->closeActors(object);
if (contacts.size()<2)
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp
index c91dbd3b9003ef89b7cfc0b8826f7d208bfff34f..7b847067c408d21a16340e8b1a35522f18edc0bc 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp
@@ -49,7 +49,7 @@ void GraspQualityMeasureWrenchSpace::setContactPoints( const std::vector<Virtual
GWSCalculated = false;
}
-void GraspQualityMeasureWrenchSpace::setContactPoints( const std::vector<EndEffector::ContactInfo> &contactPoints )
+void GraspQualityMeasureWrenchSpace::setContactPoints( const VirtualRobot::EndEffector::ContactInfoVector &contactPoints )
{
GraspQualityMeasure::setContactPoints(contactPoints);
GWSCalculated = false;
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h
index 6667e34d16e62b396b4ebfd40e04ae382bd53624..7ed55e442b2a1ecee6ab6f6941ea522507b00139 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h
@@ -101,7 +101,7 @@ public:
the contact normals are normalize to unit length
*/
virtual void setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint> &contactPoints);
- virtual void setContactPoints(const std::vector<VirtualRobot::EndEffector::ContactInfo> &contactPoints);
+ virtual void setContactPoints(const VirtualRobot::EndEffector::ContactInfoVector &contactPoints);
virtual bool calculateGraspQuality();
virtual bool calculateObjectProperties();
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
index 6e8c440decfcdccba8d83a407953869c9883a689..7221262f22f6947993758cb55c9179e2403f488e 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
@@ -315,6 +315,7 @@ void GraspPlannerWindow::plan()
int start = (int)grasps->getSize()-nrGrasps-1;
if (start<0)
start = 0;
+ grasps->setPreshape(preshape);
for (int i=start; i<(int)grasps->getSize()-1; i++)
{
Eigen::Matrix4f m = grasps->getGrasp(i)->getTcpPoseGlobal(object->getGlobalPose());
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
index 1d9d11d71dc413e2f72044836be28204346a2781..a4e12a202f4d05f8de1dfe2a4365acf6a609f57e 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
@@ -90,7 +90,7 @@ protected:
VirtualRobot::GraspSetPtr grasps;
- std::vector< VirtualRobot::EndEffector::ContactInfo > contacts;
+ VirtualRobot::EndEffector::ContactInfoVector contacts;
std::string robotFile;
diff --git a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
index 5e0b957c8260ce33778431d3d373dbc00ab2b7fa..9005fe0a8ba8465377b9f50e05d91479e1b30a0d 100644
--- a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
+++ b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
@@ -396,7 +396,7 @@ void GraspQualityWindow::showGWS()
GraspStudio::ContactConeGeneratorPtr cgMM(new GraspStudio::ContactConeGenerator(8,0.25f,100.0f));
std::vector<MathTools::ContactPoint> resultsMM;
- std::vector<EndEffector::ContactInfo>::const_iterator objPointsIter;
+ VirtualRobot::EndEffector::ContactInfoVector::const_iterator objPointsIter;
for (objPointsIter = contacts.begin(); objPointsIter != contacts.end(); objPointsIter++)
{
MathTools::ContactPoint point;
diff --git a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.h b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.h
index 0e62b4e385fbd89ac166e1b7197401be8ecdbb60..24e485578802068a5dc1ebe59fac1a7b1034531d 100644
--- a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.h
+++ b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.h
@@ -102,7 +102,7 @@ protected:
VirtualRobot::EndEffectorPtr eef;
std::vector< VirtualRobot::EndEffectorPtr > eefs;
- std::vector< VirtualRobot::EndEffector::ContactInfo > contacts;
+ VirtualRobot::EndEffector::ContactInfoVector contacts;
std::string robotFile;
diff --git a/MotionPlanning/Planner/GraspRrt.cpp b/MotionPlanning/Planner/GraspRrt.cpp
index 3e70e2cd83249ab07a56863bc99a6b83f4e51c71..aa911c87e9f7b5b80810ff1efecd29a37b82b793 100644
--- a/MotionPlanning/Planner/GraspRrt.cpp
+++ b/MotionPlanning/Planner/GraspRrt.cpp
@@ -776,8 +776,8 @@ float GraspRrt::calculateGraspScore(const Eigen::VectorXf &c, int nId, bool bSto
performanceMeasure.numberOfGraspScorings++;
rns->setJointValues(c);
- std::vector< VirtualRobot::EndEffector::ContactInfo > contactsAll = eef->closeActors(graspCollisionObjects);
- std::vector< VirtualRobot::EndEffector::ContactInfo > contacts;
+ VirtualRobot::EndEffector::ContactInfoVector contactsAll = eef->closeActors(graspCollisionObjects);
+ VirtualRobot::EndEffector::ContactInfoVector contacts;
// we only need the targetObject contacts
for (size_t i=0;i<contactsAll.size();i++)
{
@@ -924,7 +924,7 @@ ApproachDiscretizationPtr GraspRrt::getPoseRelationSphere()
return poseSphere;
}
-void GraspRrt::getGraspInfoResult(std::vector<GraspInfo, Eigen::aligned_allocator<GraspInfo> > &storeGraspInfo)
+void GraspRrt::getGraspInfoResult(GraspRrt::GraspInfoVector &storeGraspInfo)
{
graspInfoMutex.lock();
storeGraspInfo = grasps;
diff --git a/MotionPlanning/Planner/GraspRrt.h b/MotionPlanning/Planner/GraspRrt.h
index b9efa7dc169a2402628d6a76d02fa955de97cf63..a63af0bb8e39e6673fdf91bf32d3a8d12b5b5c5c 100644
--- a/MotionPlanning/Planner/GraspRrt.h
+++ b/MotionPlanning/Planner/GraspRrt.h
@@ -106,11 +106,14 @@ public:
int rrtNodeId;
float graspScore;
float distanceToObject;
- std::vector<VirtualRobot::EndEffector::ContactInfo> contacts;
+ VirtualRobot::EndEffector::ContactInfoVector contacts;
};
+ typedef std::vector< GraspInfo, Eigen::aligned_allocator<GraspInfo> > GraspInfoVector;
+
+
//! Stores all found grasps to given vector (thread safe)
- void getGraspInfoResult(std::vector<GraspInfo, Eigen::aligned_allocator<GraspInfo> > &vStoreGraspInfo);
+ void getGraspInfoResult(GraspInfoVector &vStoreGraspInfo);
//! Returns a specific grasp result (thread safe)
bool getGraspInfoResult(int index, GraspInfo &storeGraspInfo);
@@ -232,7 +235,7 @@ protected:
float cartSamplingOriStepSize,cartSamplingPosStepSize;
- std::vector<GraspInfo, Eigen::aligned_allocator<GraspInfo> > grasps;
+ GraspInfoVector grasps;
ApproachDiscretizationPtr poseSphere;
diff --git a/VirtualRobot/EndEffector/EndEffector.cpp b/VirtualRobot/EndEffector/EndEffector.cpp
index 16d2ca46c0346bfacde0ad892c1e5d29ab513fa4..cb4fbbc291eaf043b0b68fb08c35c0c3add0f7da 100644
--- a/VirtualRobot/EndEffector/EndEffector.cpp
+++ b/VirtualRobot/EndEffector/EndEffector.cpp
@@ -101,10 +101,10 @@ void EndEffector::getStatics(std::vector<RobotNodePtr> &statics)
statics = this->statics;
}
-std::vector<VirtualRobot::EndEffector::ContactInfo> EndEffector::closeActors(SceneObjectSetPtr obstacles, float stepSize)
+EndEffector::ContactInfoVector EndEffector::closeActors(SceneObjectSetPtr obstacles, float stepSize)
{
std::vector<bool> actorCollisionStatus(actors.size(), false);
- std::vector<ContactInfo> result;
+ EndEffector::ContactInfoVector result;
bool finished = false;
int loop = 0;
@@ -138,7 +138,7 @@ std::vector<VirtualRobot::EndEffector::ContactInfo> EndEffector::closeActors(Sce
}
-std::vector<EndEffector::ContactInfo> EndEffector::closeActors( SceneObjectPtr obstacle, float stepSize /*= 0.02*/ )
+EndEffector::ContactInfoVector EndEffector::closeActors( SceneObjectPtr obstacle, float stepSize /*= 0.02*/ )
{
if (!obstacle)
return closeActors(SceneObjectSetPtr(),stepSize);
diff --git a/VirtualRobot/EndEffector/EndEffector.h b/VirtualRobot/EndEffector/EndEffector.h
index 008a76174c1e00b75866383a706e142cb8187774..84b5f6422c8f3a1cff9927897f70bbc670ce7c4f 100644
--- a/VirtualRobot/EndEffector/EndEffector.h
+++ b/VirtualRobot/EndEffector/EndEffector.h
@@ -56,6 +56,8 @@ public:
Eigen::Vector3f approachDirectionGlobal; // the movement of the contact point while closing the finger (in this direction force can be applied)
};
+ typedef std::vector< ContactInfo, Eigen::aligned_allocator<ContactInfo> > ContactInfoVector;
+
EndEffector(const std::string& nameString, const std::vector<EndEffectorActorPtr>& actorsVector, const std::vector<RobotNodePtr>& staticPartVector, RobotNodePtr baseNodePtr, RobotNodePtr tcpNodePtr, RobotNodePtr gcpNodePtr = RobotNodePtr(), std::vector< RobotConfigPtr > preshapes = std::vector< RobotConfigPtr >());
virtual ~EndEffector();
@@ -106,8 +108,8 @@ public:
Closes each actor until a joint limit is hit or a collision occurred.
This method is intended for gripper or hand-like end-effectors.
*/
- std::vector<ContactInfo> closeActors(SceneObjectSetPtr obstacles = SceneObjectSetPtr(), float stepSize = 0.02);
- std::vector<ContactInfo> closeActors(SceneObjectPtr obstacle, float stepSize = 0.02);
+ ContactInfoVector closeActors(SceneObjectSetPtr obstacles = SceneObjectSetPtr(), float stepSize = 0.02);
+ ContactInfoVector closeActors(SceneObjectPtr obstacle, float stepSize = 0.02);
/*!
Opens each actor until a joint limit is hit or a collision occurred.
diff --git a/VirtualRobot/EndEffector/EndEffectorActor.cpp b/VirtualRobot/EndEffector/EndEffectorActor.cpp
index c3e8cdf553f17cf8d9ba2b170a97070117128e72..4e060e40689f7877a4c0d2d98ade07176e4404e7 100644
--- a/VirtualRobot/EndEffector/EndEffectorActor.cpp
+++ b/VirtualRobot/EndEffector/EndEffectorActor.cpp
@@ -65,7 +65,7 @@ bool EndEffectorActor::moveActor(float angle)
return res;
}
-bool EndEffectorActor::moveActorCheckCollision( EndEffectorPtr eef, std::vector< EndEffector::ContactInfo > &storeContacts, SceneObjectSetPtr obstacles /*= SceneObjectSetPtr()*/, float angle /*= 0.02*/ )
+bool EndEffectorActor::moveActorCheckCollision( EndEffectorPtr eef, EndEffector::ContactInfoVector &storeContacts, SceneObjectSetPtr obstacles /*= SceneObjectSetPtr()*/, float angle /*= 0.02*/ )
{
if (!eef)
{
@@ -76,7 +76,7 @@ bool EndEffectorActor::moveActorCheckCollision( EndEffectorPtr eef, std::vector<
eef->getActors(eefActors);
std::vector<RobotNodePtr> eefStatic;
eef->getStatics(eefStatic);
- std::vector< EndEffector::ContactInfo > newContacts;
+ EndEffector::ContactInfoVector newContacts;
for(std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++)
{
@@ -163,7 +163,7 @@ bool EndEffectorActor::moveActorCheckCollision( EndEffectorPtr eef, std::vector<
}
-bool EndEffectorActor::isColliding(EndEffectorPtr eef, SceneObjectSetPtr obstacles, std::vector< EndEffector::ContactInfo > &storeContacts, CollisionMode checkColMode)
+bool EndEffectorActor::isColliding(EndEffectorPtr eef, SceneObjectSetPtr obstacles, EndEffector::ContactInfoVector &storeContacts, CollisionMode checkColMode)
{
std::vector<SceneObjectPtr> colModels = obstacles->getSceneObjects();
//Eigen::Vector3f contact;
@@ -258,7 +258,7 @@ bool EndEffectorActor::isColliding(EndEffectorPtr obstacle)
return false;
}
-bool EndEffectorActor::isColliding( EndEffectorPtr eef, EndEffectorPtr obstacle, std::vector< EndEffector::ContactInfo > &storeContacts )
+bool EndEffectorActor::isColliding( EndEffectorPtr eef, EndEffectorPtr obstacle, EndEffector::ContactInfoVector &storeContacts )
{
std::vector<EndEffectorActorPtr> obstacleActors;
obstacle->getActors(obstacleActors);
@@ -282,7 +282,7 @@ bool EndEffectorActor::isColliding( EndEffectorPtr eef, EndEffectorPtr obstacle,
return false;
}
-bool EndEffectorActor::isColliding( EndEffectorPtr eef, EndEffectorActorPtr obstacle, std::vector< EndEffector::ContactInfo > &storeContacts )
+bool EndEffectorActor::isColliding( EndEffectorPtr eef, EndEffectorActorPtr obstacle, EndEffector::ContactInfoVector &storeContacts )
{
for(std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++)
{
@@ -294,7 +294,7 @@ bool EndEffectorActor::isColliding( EndEffectorPtr eef, EndEffectorActorPtr obst
}
-bool EndEffectorActor::isColliding( EndEffectorPtr eef, SceneObjectPtr obstacle, std::vector< EndEffector::ContactInfo > &storeContacts, CollisionMode checkColMode /*= EndEffectorActor::eAll*/ )
+bool EndEffectorActor::isColliding( EndEffectorPtr eef, SceneObjectPtr obstacle, EndEffector::ContactInfoVector &storeContacts, CollisionMode checkColMode /*= EndEffectorActor::eAll*/ )
{
if(!obstacle || !obstacle->getCollisionModel())
return false;
diff --git a/VirtualRobot/EndEffector/EndEffectorActor.h b/VirtualRobot/EndEffector/EndEffectorActor.h
index e01cee60a25c03a8d0bd5ac4e7d87ca5af4bb809..c8f2175e1916e01f3b86636f40f783a823b1b2c7 100644
--- a/VirtualRobot/EndEffector/EndEffectorActor.h
+++ b/VirtualRobot/EndEffector/EndEffectorActor.h
@@ -75,32 +75,32 @@ public:
\param angle How far should the eef actor move [rad]
Returns true if all joints do either hit their limit or are in collision, e.g. the actor cannot be moved any further.
*/
- bool moveActorCheckCollision(EndEffectorPtr eef, std::vector< EndEffector::ContactInfo > &storeContacts, SceneObjectSetPtr obstacles = SceneObjectSetPtr(), float angle = 0.02);
+ bool moveActorCheckCollision(EndEffectorPtr eef, EndEffector::ContactInfoVector &storeContacts, SceneObjectSetPtr obstacles = SceneObjectSetPtr(), float angle = 0.02);
/*!
Checks if the actor collides with one of the given obstacles
*/
bool isColliding(SceneObjectSetPtr obstacles, CollisionMode checkColMode = EndEffectorActor::eAll);
- bool isColliding(EndEffectorPtr eef, SceneObjectSetPtr obstacles, std::vector< EndEffector::ContactInfo > &storeContacts, CollisionMode checkColMode = EndEffectorActor::eAll);
+ bool isColliding(EndEffectorPtr eef, SceneObjectSetPtr obstacles, EndEffector::ContactInfoVector &storeContacts, CollisionMode checkColMode = EndEffectorActor::eAll);
/*!
Checks if the actor collides with the given obstacle.
\p checkColMode If set, the collisionMode of the actor's robotNodes is checked against it (e.g. to avoid collision checks with the static part of the eef)
*/
bool isColliding(SceneObjectPtr obstacle, CollisionMode checkColMode = EndEffectorActor::eAll);
- bool isColliding(EndEffectorPtr eef, SceneObjectPtr obstacle, std::vector< EndEffector::ContactInfo > &storeContacts, CollisionMode checkColMode = EndEffectorActor::eAll);
+ bool isColliding(EndEffectorPtr eef, SceneObjectPtr obstacle, EndEffector::ContactInfoVector &storeContacts, CollisionMode checkColMode = EndEffectorActor::eAll);
/*!
Checks if the actor collides with a given second actor
*/
bool isColliding(EndEffectorActorPtr obstacle);
- bool isColliding(EndEffectorPtr eef, EndEffectorActorPtr obstacle, std::vector< EndEffector::ContactInfo > &storeContacts);
+ bool isColliding(EndEffectorPtr eef, EndEffectorActorPtr obstacle, EndEffector::ContactInfoVector &storeContacts);
/*!
Checks if the actor collides with a given end effector
*/
bool isColliding(EndEffectorPtr obstacle);
- bool isColliding(EndEffectorPtr eef, EndEffectorPtr obstacle, std::vector< EndEffector::ContactInfo > &storeContacts);
+ bool isColliding(EndEffectorPtr eef, EndEffectorPtr obstacle, EndEffector::ContactInfoVector &storeContacts);
std::vector< RobotNodePtr > getRobotNodes();
diff --git a/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp b/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp
index 2a48f771ead4febc1825827a40c36543e4cec0f2..e690a880308c3a673873124563a5e41c9c1ea590 100644
--- a/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp
+++ b/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp
@@ -82,11 +82,11 @@ void BasicGraspQualityMeasure::setContactPoints( const std::vector<VirtualRobot:
}
}
-void BasicGraspQualityMeasure::setContactPoints( const std::vector<EndEffector::ContactInfo> &contactPoints )
+void BasicGraspQualityMeasure::setContactPoints( const EndEffector::ContactInfoVector &contactPoints )
{
this->contactPoints.clear();
this->contactPointsM.clear();
- std::vector<EndEffector::ContactInfo>::const_iterator objPointsIter;
+ EndEffector::ContactInfoVector::const_iterator objPointsIter;
for (objPointsIter = contactPoints.begin(); objPointsIter != contactPoints.end(); objPointsIter++)
{
MathTools::ContactPoint point;
diff --git a/VirtualRobot/Grasping/BasicGraspQualityMeasure.h b/VirtualRobot/Grasping/BasicGraspQualityMeasure.h
index 34b94b20455b5aebfbd8b9b3a2b9b751f118f4b3..60d6067c6dfb6c757f1067a438158233bf0c7152 100644
--- a/VirtualRobot/Grasping/BasicGraspQualityMeasure.h
+++ b/VirtualRobot/Grasping/BasicGraspQualityMeasure.h
@@ -52,7 +52,7 @@ public:
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 VirtualRobot::EndEffector::ContactInfoVector &contactPoints);
virtual void setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint> &contactPoints6d);
/*!
diff --git a/VirtualRobot/Grasping/Grasp.cpp b/VirtualRobot/Grasping/Grasp.cpp
index 04dd3da2a78af6ba044b531005c563ac63367023..1b7ce179dc416f7515ca957d87441c3ccac184ad 100644
--- a/VirtualRobot/Grasping/Grasp.cpp
+++ b/VirtualRobot/Grasping/Grasp.cpp
@@ -8,8 +8,8 @@ namespace VirtualRobot
{
-Grasp::Grasp(const std::string &name, const std::string &robotType, const std::string &eef, const Eigen::Matrix4f &poseInTCPCoordSystem, const std::string &creation, float quality)
-:name(name),robotType(robotType),eef(eef),poseTcp(poseInTCPCoordSystem),creation(creation),quality(quality)
+Grasp::Grasp(const std::string &name, const std::string &robotType, const std::string &eef, const Eigen::Matrix4f &poseInTCPCoordSystem, const std::string &creation, float quality, const std::string &eefPreshape)
+:name(name),robotType(robotType),eef(eef),poseTcp(poseInTCPCoordSystem),creation(creation),quality(quality),preshape(eefPreshape)
{
}
@@ -24,6 +24,7 @@ void Grasp::print( bool printDecoration /*= true*/ )
cout << "**** Grasp ****" << endl;
cout << " * Robot type: " << robotType << endl;
cout << " * End Effector: " << eef << endl;
+ cout << " * EEF Preshape: " << preshape << endl;
}
cout << " * Name: " << name << endl;
cout << " * Creation Method: " << creation << endl;
@@ -72,6 +73,11 @@ std::string Grasp::getName()
return name;
}
+std::string Grasp::getPreshapeName()
+{
+ return preshape;
+}
+
Eigen::Matrix4f Grasp::getTransformation()
{
return poseTcp;
@@ -92,8 +98,11 @@ std::string Grasp::getXMLString(int tabs)
tt += "\t";
std::string ttt = tt;
ttt += "\t";
-
- ss << t << "<Grasp name='" << name << "' quality='" << quality << "' Creation='" << creation << "'>\n";
+ ss << t << "<Grasp name='" << name << "' quality='" << quality << "' Creation='" << creation;
+ if (preshape.empty())
+ ss << "'>\n";
+ else
+ ss << "' Preshape='" << preshape << "'>\n";
ss << tt<< "<Transform>\n";
ss << MathTools::getTransformXMLString(poseTcp, ttt);
ss << tt << "</Transform>\n";
@@ -126,7 +135,7 @@ Eigen::Matrix4f Grasp::getObjectTargetPoseGlobal( const Eigen::Matrix4f &graspin
VirtualRobot::GraspPtr Grasp::clone()
{
- GraspPtr result(new Grasp(name,robotType,eef,poseTcp,creation,quality));
+ GraspPtr result(new Grasp(name,robotType,eef,poseTcp,creation,quality,preshape));
result->setConfiguration(eefConfiguration);
return result;
}
@@ -141,6 +150,11 @@ std::map< std::string, float > Grasp::getConfiguration()
return eefConfiguration;
}
+void Grasp::setPreshape( const std::string &preshapeName )
+{
+ preshape = preshapeName;
+}
+
} // namespace
diff --git a/VirtualRobot/Grasping/Grasp.h b/VirtualRobot/Grasping/Grasp.h
index ea1cfa9186bca35a7e52fc7b8098f55f4be3799a..6bca480a1e1713534bffda8abe9779faeb24df61 100644
--- a/VirtualRobot/Grasping/Grasp.h
+++ b/VirtualRobot/Grasping/Grasp.h
@@ -50,7 +50,7 @@ public:
\param creation A custom string explaining how the grasp was created.
\param quality A custom quality index.
*/
- Grasp(const std::string &name, const std::string &robotType, const std::string &eef, const Eigen::Matrix4f &poseInTCPCoordSystem, const std::string &creation= std::string(""), float quality = 0.0f);
+ Grasp(const std::string &name, const std::string &robotType, const std::string &eef, const Eigen::Matrix4f &poseInTCPCoordSystem, const std::string &creation= std::string(""), float quality = 0.0f, const std::string &eefPreshape= std::string(""));
/*!
*/
@@ -59,6 +59,7 @@ public:
void print(bool printDecoration = true);
void setName(const std::string &name);
+ void setPreshape(const std::string &preshapeName);
/*!
Get the (current) global pose of the target object when the grasp is applied on the corresponding EEF of robot.
@@ -88,6 +89,7 @@ public:
std::string getRobotType();
std::string getEefName();
std::string getCreationMethod();
+ std::string getPreshapeName();
float getQuality();
/*!
@@ -122,7 +124,7 @@ protected:
std::string creation;
float quality;
-
+ std::string preshape; //!< Optionally an eef-preshape can be defined.
};
} // namespace
diff --git a/VirtualRobot/Grasping/GraspSet.cpp b/VirtualRobot/Grasping/GraspSet.cpp
index 38c2ad059b82b01a237e72275dadb2e0e0f9e6e9..e5172987b618d33c590b4983e11f47e9859b4606 100644
--- a/VirtualRobot/Grasping/GraspSet.cpp
+++ b/VirtualRobot/Grasping/GraspSet.cpp
@@ -173,6 +173,12 @@ std::vector< GraspPtr > GraspSet::getGrasps()
return res;
}
+void GraspSet::setPreshape( const std::string &preshape )
+{
+ for (size_t i=0;i<grasps.size();i++)
+ grasps[i]->setPreshape(preshape);
+}
+
} // namespace
diff --git a/VirtualRobot/Grasping/GraspSet.h b/VirtualRobot/Grasping/GraspSet.h
index 2cd578aecce9f047a31a2cd59e5b6d6432f93035..9c3d77c5a65b70786e8bdbb6ce06d17337906b91 100644
--- a/VirtualRobot/Grasping/GraspSet.h
+++ b/VirtualRobot/Grasping/GraspSet.h
@@ -78,6 +78,10 @@ public:
GraspSetPtr clone();
std::vector< GraspPtr > getGrasps();
+
+ //! Sets preshape string of all grasps
+ void setPreshape (const std::string &preshape);
+
protected:
std::vector< GraspPtr > grasps;
std::string name;
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index 04264c2c09a092bee49e592834d951f0368adcef..0b567918d75d62874ed248f642db5f2c896d135f 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -799,7 +799,7 @@ SoNode * CoinVisualizationFactory::getCoinVisualization( EndEffector::ContactInf
return result;
}
-SoNode * CoinVisualizationFactory::getCoinVisualization( std::vector <EndEffector::ContactInfo> &contacts, float frictionConeHeight, float frictionConeRadius, bool scaleAccordingToApproachDir )
+SoNode * CoinVisualizationFactory::getCoinVisualization( EndEffector::ContactInfoVector &contacts, float frictionConeHeight, float frictionConeRadius, bool scaleAccordingToApproachDir )
{
SoSeparator *res = new SoSeparator;
res->ref();
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index 8f7f570f83c4f0bc224bac141f274499f4fccc0a..91bc8c394c2aa0f2c26f8247e0bbf8d7760d3258 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -130,7 +130,7 @@ public:
\param frictionConeRadius The radius of the cone [mm].
\param scaleAccordingToApproachDir If true, the parallel component of the contact normal is computed according to the approahcDirection of the contact.
*/
- static SoNode *getCoinVisualization(std::vector <EndEffector::ContactInfo> &contacts, float frictionConeHeight = 30.0f, float frictionConeRadius = 15.0f, bool scaleAccordingToApproachDir = true);
+ static SoNode *getCoinVisualization(VirtualRobot::EndEffector::ContactInfoVector &contacts, float frictionConeHeight = 30.0f, float frictionConeRadius = 15.0f, bool scaleAccordingToApproachDir = true);
/*!
Convenient method to retrieve a coin visualization for a contact.
\param contact The contact to be visualized
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index 2bda187e8c229d342905b9b7a2601f184a65b417..fa1a2af9443debc27bbbcbab1b154547f8162e8a 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -957,7 +957,7 @@ bool BaseIO::processConfigurationNode(rapidxml::xml_node<char>* configXMLNode, s
storeConfigDefinitions.push_back(c);
} else
{
- THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in scene's Configuration definition wiuth name '" << storeConfigName << "'." << endl);
+ THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in configuration definition with name '" << storeConfigName << "'." << endl);
}
node = node->next_sibling();
diff --git a/VirtualRobot/XML/ObjectIO.cpp b/VirtualRobot/XML/ObjectIO.cpp
index 1c3359a3e188a3d1b2dcda0370b66fc96fbf9575..17e4f00af5ef488de505fa1d7437cfd6e5a4da33 100644
--- a/VirtualRobot/XML/ObjectIO.cpp
+++ b/VirtualRobot/XML/ObjectIO.cpp
@@ -50,11 +50,12 @@ GraspPtr ObjectIO::processGrasp(rapidxml::xml_node<char>* graspXMLNode, const st
std::string name = processNameAttribute(graspXMLNode,true);
std::string method = processStringAttribute("creation",graspXMLNode,true);
float quality = processFloatAttribute(std::string("quality"),graspXMLNode,true);
-
+ std::string preshapeName = processStringAttribute("preshape",graspXMLNode,true);
Eigen::Matrix4f pose;
pose.setIdentity();
std::vector< RobotConfig::Configuration > configDefinitions;
std::string configName;
+
rapidxml::xml_node<>* node = graspXMLNode->first_node();
while (node)
{
@@ -66,10 +67,10 @@ GraspPtr ObjectIO::processGrasp(rapidxml::xml_node<char>* graspXMLNode, const st
} else if (nodeName == "configuration")
{
THROW_VR_EXCEPTION_IF( configDefinitions.size()>0, "Only one configuration per grasp allowed");
- bool cOK = processConfigurationNode(graspXMLNode, configDefinitions, configName);
+ bool cOK = processConfigurationNode(node, configDefinitions, configName);
THROW_VR_EXCEPTION_IF(!cOK, "Invalid configuration defined in grasp tag '" << name << "'." << endl);
- } else
+ } else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Grasp <" << name << ">." << endl);
}
@@ -77,7 +78,7 @@ GraspPtr ObjectIO::processGrasp(rapidxml::xml_node<char>* graspXMLNode, const st
node = node->next_sibling();
}
- GraspPtr grasp(new Grasp(name,robotType,eef,pose,method,quality));
+ GraspPtr grasp(new Grasp(name,robotType,eef,pose,method,quality,preshapeName));
if (configDefinitions.size()>0)
{
// create & register configs
diff --git a/VirtualRobot/examples/GraspEditor/GraspEditor.cpp b/VirtualRobot/examples/GraspEditor/GraspEditor.cpp
index 77895456483a2a0dc7ea289786819343d1059f93..b8ea582c4a2a69035ae0a3122bd119938d8e8462 100644
--- a/VirtualRobot/examples/GraspEditor/GraspEditor.cpp
+++ b/VirtualRobot/examples/GraspEditor/GraspEditor.cpp
@@ -32,9 +32,9 @@ int main(int argc, char *argv[])
std::string filename1(VR_BASE_DIR "/data/objects/plate.xml");
std::string filename2(VR_BASE_DIR "/data/robots/ArmarIII/ArmarIII.xml");
-#if 0
- filename1 = VR_BASE_DIR "/data/objects/plate_iCub.xml";
- filename2 = "C:/Projects/IIT_Projects/iCubRobot/robot/iCub.xml";
+#if 1
+ filename1 = VR_BASE_DIR "/data/objects/iCub/LegoXWing_Righthand_200.xml";
+ filename2 = VR_BASE_DIR "/data/robots/iCub/iCub.xml";
#endif
GraspEditorWindow rw(filename1,filename2);
diff --git a/VirtualRobot/examples/GraspEditor/GraspEditorWindow.cpp b/VirtualRobot/examples/GraspEditor/GraspEditorWindow.cpp
index 3ab5f40688ceb26fd3f6c26ffd758d43926cba84..bb5b6a1221c43c7382108f6ce4f638f5f67ecc39 100644
--- a/VirtualRobot/examples/GraspEditor/GraspEditorWindow.cpp
+++ b/VirtualRobot/examples/GraspEditor/GraspEditorWindow.cpp
@@ -342,6 +342,10 @@ void GraspEditorWindow::selectGrasp(int n)
Eigen::Matrix4f gp;
gp = currentGrasp->getTransformation().inverse();
gp = object->toGlobalCoordinateSystem(gp);
+ std::string preshape = currentGrasp->getPreshapeName();
+ if (!preshape.empty() && robotEEF_EEF->hasPreshape(preshape))
+ robotEEF_EEF->setPreshape(preshape);
+
setCurrentGrasp(gp);
}
buildVisu();