Now Gazebo grasp fix should work for several arms, but not directly tested on current test bed

gazebo_grasp_plugin/CMakeLists.txt

@@ -53,7 +53,7 @@ link_directories(
 )
 
 ## Declare a cpp library
-add_library(gazebo_grasp_fix SHARED src/GazeboGraspFix.cpp)
+add_library(gazebo_grasp_fix SHARED src/GazeboGraspFix.cpp src/GazeboGraspGripper.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes

gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h

+#ifndef GAZEBO_GAZEBOGRASPFIX_H
+#define GAZEBO_GAZEBOGRASPFIX_H
+
 #include <boost/bind.hpp>
 #include <gazebo/gazebo.hh>
 #include <gazebo/physics/physics.hh>
 #include <gazebo/common/common.hh>
 #include <gazebo/transport/TransportTypes.hh>
 #include <stdio.h>
+#include <gazebo_grasp_plugin/GazeboGraspGripper.h>
+// #include <gazebo_grasp_plugin/CollidingPoint.h>
 
 namespace gazebo {
 
@@ -17,10 +22,13 @@ namespace gazebo {
  * ```xml
  *   <gazebo>
  *       <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
- *           <palm_link> hand_link_name  </palm_link>
- *           <gripper_link> finger_index_link_1 </gripper_link>
- *           <gripper_link> finger_index_link_2 </gripper_link>
- *           <gripper_link> ... </gripper_link>
+ *          <arm>
+ *              <arm_name>name-of-arm</arm_name>
+ *              <palm_link> hand_link_name  </palm_link>
+ *              <gripper_link> finger_index_link_1 </gripper_link>
+ *              <gripper_link> finger_index_link_2 </gripper_link>
+ *              <gripper_link> ... </gripper_link>
+ *           </arm>
  *           <forces_angle_tolerance>100</forces_angle_tolerance>
  *           <update_rate>4</update_rate>
  *           <grip_count_threshold>4</grip_count_threshold>
@@ -34,10 +42,12 @@ namespace gazebo {
  *
  *    Description of the arguments:
  *
- *    - ``<palm_link>`` has to be the link to which the finger joints are attached.
- *    - ``<gripper_link>`` tags have to include -all- link names of the gripper/hand which are used to
- *        actively grasp objects (these are the links which determine whether a "grasp" exists according to 
- *        above described criterion).
+ *    - ``<arm>`` contains a collections of specification for each arm which can grasp one object (there may be several ``<arm>`` tags):
+ *        - ``<arm_name>`` is the name of this arm. Has to be unique.
+ *        - ``<palm_link>`` has to be the link to which the finger joints are attached.
+ *        - ``<gripper_link>`` tags have to include -all- link names of the gripper/hand which are used to
+ *              actively grasp objects (these are the links which determine whether a "grasp" exists according to 
+ *              above described criterion).
  *    - ``<update_rate>`` is the rate at which all contact points are checked against the "gripping criterion".
  *          Note that in-between such updates, existing contact points may be collected at
  *          a higher rate (the Gazebo world update rate). The ``update_rate`` is only the rate at
@@ -71,9 +81,12 @@ namespace gazebo {
  *            robot still keeps wobbling.
  *    - ``<contact_topic>`` is the gazebo topic of contacts. Should normally be left at -\_\_default_topic\_\_-.
  *
- * Current limitation:
- *  - Only one object can be attached per gripper.
+ * Current limitations:
  *
+ *  - Only one object can be attached per gripper.
+ *  - Only partial support for an object cannot be gripped with two grippers (release condition may be
+ *     triggered wrongly, or not at all, if two grippers are involved)
+ * 
  * \author Jennifer Buehler
  */ 
 class GazeboGraspFix : public ModelPlugin {
@@ -83,13 +96,13 @@ public:
     virtual ~GazeboGraspFix();
 
     /**
-     * Gets called just after the object has been attached to the link
+     * Gets called just after the object has been attached to the palm link on \e armName
      */
-    virtual void OnAttach(const std::string& objectName){}
+    virtual void OnAttach(const std::string& objectName, const std::string& armName){}
     /**
-     * Gets called just after the object has been detached to the link
+     * Gets called just after the object has been detached to the palm link on \e armName
      */
-    virtual void OnDetach(const std::string& objectName){}
+    virtual void OnDetach(const std::string& objectName, const std::string& armName){}
 
 private: 
     virtual void Init(); 
@@ -99,12 +112,12 @@ private:
      * Then, for each object, checks whether of all the forces applied,
      * there are opposing forces. This is done by calling checkGrip() with the
      * list of all forces applied.
-     * If checkGrip() returns true, the number of "grip counts" (field \e gripCounts)
-     * is increased (but grip counts will never exceed \e max_grip_count).
+     * If checkGrip() returns true, the number of "grip counts" 
+     * is increased for the holding arm (but grip counts will never exceed \e max_grip_count).
      * If the number of grip counts for this object exceeds \e grip_count_threshold,
-     * the object is attached by calling HandleAttach(object-name),
-     * setting \e attached and \e attachedObjName, and \e attachGripContacts is updated with the
-     * contact points currently existing for this object (current entry in \e contacts).
+     * the object is attached by calling GazeboGraspGripper::HandleAttach(object-name),
+     * setting \e attached and \e attachedObjName, and \e GazeboGraspGripper::attachGripContacts
+     * is updated with the contact points currently existing for this object (current entry in \e contacts).
      *
      * Then, goes through all entries in \e gripCount, and unless it's an object
      * we just detected as "gripped", the counter is decreased.
@@ -135,23 +148,69 @@ private:
      */
     void OnContact(const ConstContactsPtr& ptr);
 
-    bool HandleAttach(const std::string& objName);
-    void HandleDetach(const std::string& objName);
-
     /**
      * Checks whether any two vectors in the set have an angle greater
      * than minAngleDiff (in rad), and one is at least
      * lengthRatio (0..1) of the other in it's length.
      */
     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;
+    }   
+
+    /**
+     * 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;
+    }   
+
+
+    /**
+     * Helper class to collect contact info per object.
+     * Forward declaration here.
+     */
+    class ObjectContactInfo;
     
-    physics::ModelPtr model;
+    /**
+     * Helper function to determine if object attached to a gripper in ObjectContactInfo.
+     */
+    bool objectAttachedToGripper(const ObjectContactInfo& objContInfo, std::string& attachedToGripper) const;
+
+    /**
+     * Helper function to determine if object attached to this gripper
+     */
+    bool objectAttachedToGripper(const std::string& gripperName, std::string& attachedToGripper) const;
 
-    physics::PhysicsEnginePtr physics;
+ 
+    // physics::ModelPtr model;
+    // physics::PhysicsEnginePtr physics;
     physics::WorldPtr world;
-    physics::JointPtr fixedJoint;
-    
-    physics::LinkPtr palmLink;
+
+    // sorted by their name, all grippers of the robot
+    std::map<std::string, GazeboGraspGripper> grippers;
+ 
     event::ConnectionPtr update_connection;
     transport::NodePtr node;
     transport::SubscriberPtr contactSub; //subscriber to contact updates
@@ -164,12 +223,15 @@ private:
     // robot still keeps wobbling.
     bool disableCollisionsOnAttach;
  
-    // list of current collisions per gripper link 
-    std::map<std::string, physics::CollisionPtr> collisions;
+    // all collisions per gazebo collision link (each entry
+    // belongs to a physics::CollisionPtr element). The key
+    // is the collision link name, the value is the gripper name
+    // this collision link belongs to.
+    std::map<std::string, std::string> collisions;
+
 
     /**
-     * Helper class to encapsulate a collision information.
-     * Forward declaration here.
+     * Helper class to encapsulate a collision information. Forward declaration here.
      */
     class CollidingPoint;
 
@@ -179,10 +241,15 @@ private:
     boost::mutex mutexContacts; //mutex protects contacts
 
     // when an object was first attached, it had these colliding points.
-    // Key is object name.
+    // First key is object name, second is the link colliding, as in \e contacts.
+    // Only the links of *one* gripper are stored here. This indirectly imposes the
+    // limitation that no two grippers can grasp the object (while it would be
+    // possible, the release condition is tied to only one link, so the object may
+    // not be released properly).
     std::map<std::string, std::map<std::string, CollidingPoint> > attachGripContacts; 
+   
     
-    // Records how many update loops (at updateRate) the grip on that object has been recorded 
+    // Records how many subsequent update calls the grip on that object has been recorded 
     // as "holding". Every loop, if a grip is not recorded, this number decreases. 
     // When it reaches \e grip_count_threshold, it will be attached.
     // The number won't increase above max_grip_count once it has reached that number.
@@ -190,7 +257,7 @@ private:
 
     // *maximum* number in \e gripCounts to be recorded.
     int maxGripCount;    
-
+    
     // 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;
@@ -200,11 +267,6 @@ private:
     // the object is released.
     float releaseTolerance;
 
-    // flag holding whether an object is attached. Object name in \e attachedObjName
-    bool attached;
-    // name of the object currently attached.
-    std::string attachedObjName;    
-
     //nano seconds between two updates
     common::Time updateRate;
 
@@ -213,3 +275,5 @@ private:
 };
 
 }
+
+#endif  // GAZEBO_GAZEBOGRASPFIX_H

gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspGripper.h

+#ifndef GAZEBO_GAZEBOGRASPGRIPPER_H
+#define GAZEBO_GAZEBOGRASPGRIPPER_H
+
+#include <boost/bind.hpp>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/transport/TransportTypes.hh>
+#include <stdio.h>
+
+namespace gazebo {
+
+/**
+
+ * \author Jennifer Buehler
+ */ 
+class GazeboGraspGripper {
+public:
+    GazeboGraspGripper();
+    GazeboGraspGripper(const GazeboGraspGripper& o);
+    virtual ~GazeboGraspGripper();
+   
+    /**
+     *
+     * \param disableCollisionsOnAttach when an object is attached, collisions with it will be disabled. This is useful
+     *      if the robot then still keeps wobbling.
+     */ 
+    bool Init(physics::ModelPtr& _model,
+        const std::string& _gripperName,
+        const std::string& palmLinkName,
+        const std::vector<std::string>& fingerLinkNames,
+        bool _disableCollisionsOnAttach,
+        std::map<std::string, physics::CollisionPtr>& _collisions);
+
+    const std::string& getGripperName() const
+    {
+        return gripperName;
+    }
+
+    /**
+     * 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;
+    }
+
+    /**
+     * Has the collision link name (Gazebo collision element name)
+     */
+    bool hasCollisionLink(const std::string& linkName) const
+    {
+        return collisionElems.find(linkName) != collisionElems.end();
+    }
+
+
+    bool isObjectAttached() const
+    {
+        return attached;
+    }
+
+    const std::string& attachedObject() const
+    {
+        return attachedObjName; 
+    }
+
+    /**
+     * \param gripContacts contact forces on the object sorted by the link name colliding.
+     */
+    bool HandleAttach(const std::string& objName);
+    void HandleDetach(const std::string& objName);
+
+private: 
+
+    physics::ModelPtr model;
+
+    // name of the gripper
+    std::string gripperName;
+    
+    // names of the gripper links
+    std::vector<std::string> linkNames;
+    // names and Collision objects of the collision links in Gazebo (scoped names)
+    // Not necessarily equal names and size to linkNames.
+    std::map<std::string, physics::CollisionPtr> collisionElems;
+
+    physics::JointPtr fixedJoint;
+
+    physics::LinkPtr palmLink;
+
+    // when an object is attached, collisions with it may be disabled, in case the
+    // 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.
+    std::string attachedObjName;    
+};
+
+}
+
+#endif  // GAZEBO_GAZEBOGRASPGRIPPER_H

gazebo_grasp_plugin/src/GazeboGraspGripper.cpp

+#include <boost/bind.hpp>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/physics/ContactManager.hh>
+#include <gazebo/physics/Contact.hh>
+#include <gazebo/common/common.hh>
+#include <stdio.h>
+ 
+#include <gazebo_grasp_plugin/GazeboGraspGripper.h>
+
+using gazebo::GazeboGraspGripper;
+
+#define DEFAULT_FORCES_ANGLE_TOLERANCE 120
+#define DEFAULT_UPDATE_RATE 5
+#define DEFAULT_MAX_GRIP_COUNT 10    
+#define DEFAULT_RELEASE_TOLERANCE 0.005
+#define DEFAULT_DISABLE_COLLISIONS_ON_ATTACH false
+
+GazeboGraspGripper::GazeboGraspGripper():
+    attached(false)
+{
+}
+
+GazeboGraspGripper::GazeboGraspGripper(const GazeboGraspGripper& o):
+    model(o.model),
+    gripperName(o.gripperName),
+    linkNames(o.linkNames),
+    collisionElems(o.collisionElems),
+    fixedJoint(o.fixedJoint),
+    palmLink(o.palmLink),
+    disableCollisionsOnAttach(o.disableCollisionsOnAttach),
+    attached(o.attached),
+    attachedObjName(o.attachedObjName) 
+{}
+
+GazeboGraspGripper::~GazeboGraspGripper() {
+    this->model.reset();
+}
+
+
+bool GazeboGraspGripper::Init(physics::ModelPtr& _model,
+    const std::string& _gripperName,
+    const std::string& palmLinkName,
+    const std::vector<std::string>& fingerLinkNames,
+    bool _disableCollisionsOnAttach,
+    std::map<std::string, physics::CollisionPtr>& _collisionElems)
+{
+    this->gripperName=_gripperName;
+    this->attached = false;
+    this->disableCollisionsOnAttach = _disableCollisionsOnAttach;
+    this->model = _model;
+    physics::PhysicsEnginePtr physics = this->model->GetWorld()->GetPhysicsEngine();
+    this->fixedJoint = physics->CreateJoint("revolute");
+
+    this->palmLink = this->model->GetLink(palmLinkName);
+    if (!this->palmLink)
+    {
+        gzerr << "GazeboGraspGripper: Palm link "<<palmLinkName<<" not found. The gazebo grasp plugin will not work."<<std::endl;
+        return false;
+    }
+    for (std::vector<std::string>::const_iterator fingerIt=fingerLinkNames.begin();
+        fingerIt!=fingerLinkNames.end(); ++fingerIt)
+    {
+        physics::LinkPtr link = this->model->GetLink(*fingerIt);
+        //std::cout<<"Got link "<<fingerLinkElem->Get<std::string>()<<std::endl;
+        if (!link.get()){
+            gzerr << "GazeboGraspGripper ERROR: Link "<<*fingerIt<<" can't be found in gazebo for GazeboGraspGripper model plugin. Skipping."<<std::endl;
+            continue;
+        }
+        for (unsigned int j = 0; j < link->GetChildCount(); ++j)
+        {
+            physics::CollisionPtr collision = link->GetCollision(j);
+            std::string collName = collision->GetScopedName();
+            //collision->SetContactsEnabled(true);
+            std::map<std::string, physics::CollisionPtr>::iterator collIter = collisionElems.find(collName);
+            if (collIter != this->collisionElems.end()) { //this collision was already added before
+                gzwarn <<"GazeboGraspGripper: Adding Gazebo collision link element "<<collName<<" multiple times, the gazebo grasp handler may not work properly"<<std::endl;
+                continue;
+            }
+            this->collisionElems[collName] = collision;
+            _collisionElems[collName] = collision;
+        }
+    }
+    return !this->collisionElems.empty();
+}
+
+// #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.
+bool GazeboGraspGripper::HandleAttach(const std::string& objName)
+{
+    if (!this->palmLink) {
+        gzwarn << "GazeboGraspGripper: palm link not found, not enforcing grasp hack!\n"<<std::endl;
+        return false;
+    }
+    physics::WorldPtr world = this->model->GetWorld();
+    if (!world.get())
+    {
+        gzerr << "GazeboGraspGripper: world is NULL"<<std::endl;
+        return false;
+    }
+#ifdef USE_MODEL_ATTACH
+    physics::ModelPtr obj = world->GetModel(objName);
+    if (!obj.get()){
+        std::cerr<<"ERROR: Object ModelPtr "<<objName<<" not found in world, can't attach it"<<std::endl;
+        return false;
+    }
+    gazebo::math::Pose diff = obj->GetLink()->GetWorldPose() - this->palmLink->GetWorldPose();
+    this->palmLink->AttachStaticModel(obj,diff);
+#else
+    physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>(world->GetEntity(objName));
+    if (!obj.get()){
+        std::cerr<<"ERROR: Object "<<objName<<" not found in world, can't attach it"<<std::endl;
+        return false;
+    }
+    gazebo::math::Pose diff = obj->GetLink()->GetWorldPose() - this->palmLink->GetWorldPose();
+    this->fixedJoint->Load(this->palmLink,obj->GetLink(), diff);
+    this->fixedJoint->Init();
+    this->fixedJoint->SetHighStop(0, 0);
+    this->fixedJoint->SetLowStop(0, 0);
+    if (this->disableCollisionsOnAttach) {
+        // we can disable collisions of the grasped object, because when the fingers keep colliding with
+        // it, the fingers keep wobbling, which can create difficulties when moving the arm. 
+        obj->GetLink()->SetCollideMode("none");
+    }
+#endif  // USE_MODEL_ATTACH
+    this->attached=true;
+    this->attachedObjName=objName;
+    return true;
+}
+
+void GazeboGraspGripper::HandleDetach(const std::string& objName)
+{
+    physics::WorldPtr world = this->model->GetWorld();
+    if (!world.get())
+    {
+        gzerr << "GazeboGraspGripper: world is NULL"<<std::endl<<std::endl;
+        return;
+    }
+#ifdef USE_MODEL_ATTACH
+     physics::ModelPtr obj = world->GetModel(objName);
+    if (!obj.get()){
+        std::cerr<<"ERROR: Object ModelPtr "<<objName<<" not found in world, can't detach it"<<std::endl;
+        return;
+    }
+    this->palmLink->DetachStaticModel(objName);
+#else
+    physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>(world->GetEntity(objName));
+    if (!obj.get()){
+        std::cerr<<"ERROR: Object "<<objName<<" not found in world, can't attach it"<<std::endl;
+        return;
+    }
+    else if (this->disableCollisionsOnAttach)
+    {
+        obj->GetLink()->SetCollideMode("all");
+    }
+    this->fixedJoint->Detach();
+#endif  // USE_MODEL_ATTACH
+    this->attached=false;
+    this->attachedObjName="";
+}