From 8305845c6ec28e22bc66d9068cf2b3950c355074 Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Tue, 17 Apr 2012 20:47:42 +0000
Subject: [PATCH] * Bugfix: Using std::vectors with a struct that contains
Eigen objects sometimes causes crashes depending on OS and compiler.
EndEffector::ContactInfo contains several Eigen::Vector3f variables and hence
in some constellations a std::vector of it caused a crash. Now a typedef is
used that encapsulates the Eigen::aligned_allocator setup and this issue
should be fixed. * The grasp preshape is now handled by grasps in order to
save/load the preshape data.
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@238 042f3d55-54a8-47e9-b7fb-15903f145c44
---
.../GraspPlanner/GenericGraspPlanner.cpp | 2 +-
.../GraspQualityMeasureWrenchSpace.cpp | 2 +-
.../GraspQualityMeasureWrenchSpace.h | 2 +-
.../GraspPlanner/GraspPlannerWindow.cpp | 1 +
.../GraspPlanner/GraspPlannerWindow.h | 2 +-
.../GraspQuality/GraspQualityWindow.cpp | 2 +-
.../GraspQuality/GraspQualityWindow.h | 2 +-
MotionPlanning/Planner/GraspRrt.cpp | 6 ++---
MotionPlanning/Planner/GraspRrt.h | 9 ++++---
VirtualRobot/EndEffector/EndEffector.cpp | 6 ++---
VirtualRobot/EndEffector/EndEffector.h | 6 +++--
VirtualRobot/EndEffector/EndEffectorActor.cpp | 12 +++++-----
VirtualRobot/EndEffector/EndEffectorActor.h | 10 ++++----
.../Grasping/BasicGraspQualityMeasure.cpp | 4 ++--
.../Grasping/BasicGraspQualityMeasure.h | 2 +-
VirtualRobot/Grasping/Grasp.cpp | 24 +++++++++++++++----
VirtualRobot/Grasping/Grasp.h | 6 +++--
VirtualRobot/Grasping/GraspSet.cpp | 6 +++++
VirtualRobot/Grasping/GraspSet.h | 4 ++++
.../CoinVisualizationFactory.cpp | 2 +-
.../CoinVisualizationFactory.h | 2 +-
VirtualRobot/XML/BaseIO.cpp | 2 +-
VirtualRobot/XML/ObjectIO.cpp | 9 +++----
.../examples/GraspEditor/GraspEditor.cpp | 6 ++---
.../GraspEditor/GraspEditorWindow.cpp | 4 ++++
25 files changed, 85 insertions(+), 48 deletions(-)
diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp
index 54cfc67cb..cb065f58b 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 c91dbd3b9..7b847067c 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 6667e34d1..7ed55e442 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 6e8c440de..7221262f2 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 1d9d11d71..a4e12a202 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 5e0b957c8..9005fe0a8 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 0e62b4e38..24e485578 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 3e70e2cd8..aa911c87e 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 b9efa7dc1..a63af0bb8 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 16d2ca46c..cb4fbbc29 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 008a76174..84b5f6422 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 c3e8cdf55..4e060e406 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 e01cee60a..c8f2175e1 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 2a48f771e..e690a8803 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 34b94b204..60d6067c6 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 04dd3da2a..1b7ce179d 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 ea1cfa918..6bca480a1 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 38c2ad059..e5172987b 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 2cd578aec..9c3d77c5a 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 04264c2c0..0b567918d 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 8f7f570f8..91bc8c394 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 2bda187e8..fa1a2af94 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 1c3359a3e..17e4f00af 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 778954564..b8ea582c4 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 3ab5f4068..bb5b6a122 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();
--
GitLab