tidy up

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.

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

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();

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.