diff --git a/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h b/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h
index 721c3cf179277052dfb7977bb70a7db61093c598..9ec47578c47877d2f5ecc4176893d3d343978f0d 100644
--- a/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h
+++ b/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h
@@ -155,37 +155,12 @@ private:
*/
bool checkGrip(const std::vector<math::Vector3>& forces, float minAngleDiff, float lengthRatio);
- bool isGripperLink(const std::string& linkName, std::string& gripperName) const
- {
- for (std::map<std::string, GazeboGraspGripper>::const_iterator it=grippers.begin(); it!=grippers.end(); ++it)
- {
- if (it->second.hasLink(linkName))
- {
- gripperName=it->first;
- return true;
- }
- }
- return false;
- }
+ bool isGripperLink(const std::string& linkName, std::string& gripperName) const;
/**
* return objects (key) and the gripper (value) to which it is attached
*/
- std::map<std::string, std::string> getAttachedObjects() const
- {
- std::map<std::string, std::string> ret;
- for (std::map<std::string, GazeboGraspGripper>::const_iterator it=grippers.begin(); it!=grippers.end(); ++it)
- {
- const std::string& gripperName = it->first;
- const GazeboGraspGripper& gripper = it->second;
- if (gripper.isObjectAttached())
- {
- ret[gripper.attachedObject()]=gripperName;
- }
- }
- return ret;
- }
-
+ std::map<std::string, std::string> getAttachedObjects() const;
/**
* Helper class to collect contact info per object.
diff --git a/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspGripper.h b/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspGripper.h
index 251155d3e3d152eba477e1157b5e6f164652e05a..44ec84e5d2d9cc44f269066bae4bbc65be9cffc9 100644
--- a/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspGripper.h
+++ b/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspGripper.h
@@ -11,7 +11,9 @@
namespace gazebo {
/**
-
+ * \brief Helper class for GazeboGraspFix which holds information for one arm.
+ * Attaches /detaches objects to the palm of this arm.
+ *
* \author Jennifer Buehler
*/
class GazeboGraspGripper {
@@ -32,41 +34,21 @@ public:
bool _disableCollisionsOnAttach,
std::map<std::string, physics::CollisionPtr>& _collisions);
- const std::string& getGripperName() const
- {
- return gripperName;
- }
+ const std::string& getGripperName() const;
/**
* Has the link name (URDF)
*/
- bool hasLink(const std::string& linkName) const
- {
- for (std::vector<std::string>::const_iterator it=linkNames.begin(); it!=linkNames.end(); ++it)
- {
- if (*it==linkName) return true;
- }
- return false;
- }
+ bool hasLink(const std::string& linkName) const;
/**
* Has the collision link name (Gazebo collision element name)
*/
- bool hasCollisionLink(const std::string& linkName) const
- {
- return collisionElems.find(linkName) != collisionElems.end();
- }
-
+ bool hasCollisionLink(const std::string& linkName) const;
- bool isObjectAttached() const
- {
- return attached;
- }
+ bool isObjectAttached() const;
- const std::string& attachedObject() const
- {
- return attachedObjName;
- }
+ const std::string& attachedObject() const;
/**
* \param gripContacts contact forces on the object sorted by the link name colliding.
@@ -95,29 +77,6 @@ private:
// robot still keeps wobbling.
bool disableCollisionsOnAttach;
-#if 0
- // tolerance (in degrees) between force vectors to
- // beconsidered "opposing"
- float forcesAngleTolerance;
-
-
- // list of current collisions per gripper link
- std::map<std::string, physics::CollisionPtr> collisions;
-
- // when an object was first attached, it had these colliding points.
- // Key is object name.
- std::map<std::string, std::map<std::string, CollidingPoint> > attachGripContacts;
-
- // number of recorded "grips" in the past (in gripCount) which, when it is exceeded, counts
- // as the object grasped, and when it is lower, as released.
- int gripCountThreshold;
-
- // once an object is gripped, the relative position of the collision link surface to the
- // object is remembered. As soon as this distance changes more than release_tolerance,
- // the object is released.
- float releaseTolerance;
-#endif
-
// flag holding whether an object is attached. Object name in \e attachedObjName
bool attached;
// name of the object currently attached.
@@ -125,5 +84,4 @@ private:
};
}
-
#endif // GAZEBO_GAZEBOGRASPGRIPPER_H
diff --git a/gazebo_grasp_plugin/src/GazeboGraspFix.cpp b/gazebo_grasp_plugin/src/GazeboGraspFix.cpp
index a761f5c1e91d21daa2829b20e9e3b7bb2340e020..9ef994bbce299a13abe562de66ce6ac657c1dc58 100644
--- a/gazebo_grasp_plugin/src/GazeboGraspFix.cpp
+++ b/gazebo_grasp_plugin/src/GazeboGraspFix.cpp
@@ -231,6 +231,39 @@ class GazeboGraspFix::ObjectContactInfo
};
+
+bool GazeboGraspFix::isGripperLink(const std::string& linkName, std::string& gripperName) const
+{
+ for (std::map<std::string, GazeboGraspGripper>::const_iterator it=grippers.begin(); it!=grippers.end(); ++it)
+ {
+ if (it->second.hasLink(linkName))
+ {
+ gripperName=it->first;
+ return true;
+ }
+ }
+ return false;
+}
+
+std::map<std::string, std::string> GazeboGraspFix::getAttachedObjects() const
+{
+ std::map<std::string, std::string> ret;
+ for (std::map<std::string, GazeboGraspGripper>::const_iterator it=grippers.begin(); it!=grippers.end(); ++it)
+ {
+ const std::string& gripperName = it->first;
+ const GazeboGraspGripper& gripper = it->second;
+ if (gripper.isObjectAttached())
+ {
+ ret[gripper.attachedObject()]=gripperName;
+ }
+ }
+ return ret;
+}
+
+
+
+
+
bool GazeboGraspFix::objectAttachedToGripper(const ObjectContactInfo& objContInfo, std::string& attachedToGripper) const
{
for (std::map<std::string, int>::const_iterator gripInvIt=objContInfo.grippersInvolved.begin();
diff --git a/gazebo_grasp_plugin/src/GazeboGraspGripper.cpp b/gazebo_grasp_plugin/src/GazeboGraspGripper.cpp
index 5880fe69cbdc0d6019d25cdfa0342d394f3dc2b6..c3effcc4c18331a61e7315a7a9d1a863c6f71b7f 100644
--- a/gazebo_grasp_plugin/src/GazeboGraspGripper.cpp
+++ b/gazebo_grasp_plugin/src/GazeboGraspGripper.cpp
@@ -38,6 +38,7 @@ GazeboGraspGripper::~GazeboGraspGripper() {
}
+
bool GazeboGraspGripper::Init(physics::ModelPtr& _model,
const std::string& _gripperName,
const std::string& palmLinkName,
@@ -84,6 +85,40 @@ bool GazeboGraspGripper::Init(physics::ModelPtr& _model,
return !this->collisionElems.empty();
}
+
+
+const std::string& GazeboGraspGripper::getGripperName() const
+{
+ return gripperName;
+}
+
+bool GazeboGraspGripper::hasLink(const std::string& linkName) const
+{
+ for (std::vector<std::string>::const_iterator it=linkNames.begin(); it!=linkNames.end(); ++it)
+ {
+ if (*it==linkName) return true;
+ }
+ return false;
+}
+
+bool GazeboGraspGripper::hasCollisionLink(const std::string& linkName) const
+{
+ return collisionElems.find(linkName) != collisionElems.end();
+}
+
+
+bool GazeboGraspGripper::isObjectAttached() const
+{
+ return attached;
+}
+
+const std::string& GazeboGraspGripper::attachedObject() const
+{
+ return attachedObjName;
+}
+
+
+
// #define USE_MODEL_ATTACH // this only works if the object is a model in itself, which is usually not
// the case. Leaving this in here anyway for documentation of what has been
// tried, and for and later re-evaluation.