diff --git a/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h b/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h
index 57cf932f5aa3cafaf633d2ecca09420905ea6fba..f792ebc52ba7f4d56e9cc938ed4514de3f692857 100644
--- a/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h
+++ b/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h
@@ -17,14 +17,62 @@ namespace gazebo {
  * ```xml
  *   <gazebo>
  *       <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
+ *           <!-- Palm link: The link to which the object is attached -->
  *           <palm_link> hand_link_name  </palm_link>
+ *
+ *           <!-- All gripper links which collide with the object to *actively* grasp it -->
  *           <gripper_link> finger_index_link_1 </gripper_link>
  *           <gripper_link> finger_index_link_2 </gripper_link>
  *           <gripper_link> ... </gripper_link>
+ *           <!-- tolerance angle (in degrees) between two force vectors to be considered
+ *                "opposing forces". If the angle is smaller than this, they are not opposing.
+ *             -->
+ *           <forces_angle_tolerance>120</forces_angle_tolerance>
+ *
+ *           <!-- the rate at which all collision points are checked against the "gripping criterion".
+ *              Note that in-between such updates, existing collisions may be collected at
+ *              a higher rate (the Gazebo world update rate). This rate is only the rate at
+ *              which they are processed, which takes a bit of computation time, and therefore
+ *              should be lower than the gazebo world update rate.  -->
+ *           <update_rate>5</update_rate>
+ *
+ *           <!-- Number of times an object has to be detected as "gripped" in order to
+ *                attach the object. Adjust this with the update rate. -->
+ *           <grip_count_threshold>5</grip_count_threshold>
+ *
+ *           <!-- Maximum number of times that a "grasped" condition counted for an
+ *                object. Should be at least double of grip_count_threshold. If the object
+ *                has been counted this amount of times in subsequent update iterations as
+ *                "grasped", the number will not increase any further. As soon as the grasp
+ *                criterion does not hold any more, the number will start to decrease by one time
+ *                each time the object is detected as "not grasped" in an update iteration. This is
+ *                like a "buffer" which, when it is full, maintains the state, and when it is empty again,
+ *                the object is released -->
+ *           <max_grip_count>10</max_grip_count>
+ *
+ *           <!-- The distance which the gripper links are allowed to move away from the object
+ *                *during* a grasp without the object being detached, even if there are currently no
+ *                contacts. This can happen if the fingers "wobble" or move ever so slightly, and therefore
+ *                the grasp is not detected as active any more. Setting this number too high will also lead
+ *                to the object not being detached even if the grippers have opened up to release it. -->
+ *           <release_tolerance>0.005</release_tolerance>
+ *
+ *           <!-- when an object is attached, collisions with it may be disabled, in case the
+ *                robot still keeps wobbling. -->  
+ *           <disable_collisions_on_attach>false</disable_collisions_on_attach>
+ *
+ *           <!-- The gazebo topic of contacts. Should normally be left at __default_topic__  -->
+ *           <contact_topic>__default_topic__</contact_topic>
  *       </plugin>
  *   </gazebo>
  * ```
  *
+ * Methods OnUpdate() and OnContact() contain detailed documentation on how the plugin works.
+ * Summarizing it:
+ *  XXX TODO
+ *
+ * Current limitation:
+ *  - Only one object can be attached per gripper.
  * \author Jennifer Buehler
  */ 
 class GazeboGraspFix : public ModelPlugin {
@@ -34,18 +82,55 @@ public:
     virtual ~GazeboGraspFix();
     virtual void Init(); 
     virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
-    void OnUpdate();
 private: 
+    /**
+     * Collects for each object all forces which are currently applied on it.
+     * 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 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).
+     *
+     * Then, goes through all entries in \e gripCount, and unless it's an object
+     * we just detected as "gripped", the counter is decreased.
+     * If the counter is is smaller than \e grip_count_threshold, the object should
+     * potentially be released, but this criterion happens too easily
+     * (the fingers in gazebo may have started wobbling as the arm moves around, and although they are still
+     * close to the object, the grip is not detected any more).
+     * So to be sure, and additional criterion has to be satisfied before the object is released:
+     * check that the collision point (the place on the link where the contact originally
+     * was detected) has not moved too far from where it originally was, relative to the object.
+     */
+    void OnUpdate();
 
     void InitValues();
+
+
+    /**
+     * Gets called upon detection of contacts.
+     * A list of contacts is passed in \_msg. One contact has two bodies, and only
+     * the ones where one of the bodies is a gripper link are considered.
+     * Each contact consists of a *list* of forces with their own origin/position each
+     * (e.g. when the object and gripper are colliding at several places).
+     * The averages of each contact's force vectors along with their origins is computed.
+     * This "average contact force/origin" for each contact is then added to the \e this->contacts map.
+     * If an entry for this object/link pair already exists, the average force (and its origin)
+     * is *added* to the existing force/origin, and the average count is increased. This is to get
+     * the average force application over time between link and object.  
+     */
     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.
+     * 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);
     
@@ -59,7 +144,16 @@ private:
     event::ConnectionPtr update_connection;
     transport::NodePtr node;
     transport::SubscriberPtr contactSub; //subscriber to contact updates
+  
+    // tolerance (in degrees) between force vectors to
+    // beconsidered "opposing"
+    float forcesAngleTolerance;
     
+    // when an object is attached, collisions with it may be disabled, in case the
+    // robot still keeps wobbling.
+    bool disableCollisionsOnAttach;
+ 
+    // list of current collisions per gripper link 
     std::map<std::string, physics::CollisionPtr> collisions;
 
     /**
@@ -68,31 +162,36 @@ private:
      */
     class CollidingPoint;
 
-    //Contact forces sorted by object name the gripper collides with, and the link colliding. 
-    //This is a vector summed up over time, sum count is kept in contactSumCnt.
+    // Contact forces sorted by object name the gripper collides with (first key)
+    // and the link colliding (second key). 
     std::map<std::string, std::map<std::string, CollidingPoint> > contacts; 
     boost::mutex mutexContacts; //mutex protects contacts
 
-    //when an object was attached, it had these colliding points.
+    // 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; 
     
-    //records how many update loops (at updateRate) the grip on that object has been recorded 
-    //as "holding". Every loop, if a grip is not recorded, this number decreases. 
-    //When it reaches grip_count_threshold, it will be attached.
-    //The number won't increase above max_grip_count once it has reached that number.
+    // Records how many update loops (at updateRate) 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.
     std::map<std::string, int> gripCounts; 
-    //maximum record in gripCounts
-    int max_grip_count;    
-    //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 grip_count_threshold;
 
-    //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 release_tolerance;
+    // *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;
+
+    // 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;
 
+    // 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
diff --git a/gazebo_grasp_plugin/src/GazeboGraspFix.cpp b/gazebo_grasp_plugin/src/GazeboGraspFix.cpp
index b268cfacfcf00ef383414e1b1e6f146bf2214141..81a208937bd269d25a56bf1e0d22b7054b37f094 100644
--- a/gazebo_grasp_plugin/src/GazeboGraspFix.cpp
+++ b/gazebo_grasp_plugin/src/GazeboGraspFix.cpp
@@ -10,11 +10,28 @@
 
 using gazebo::GazeboGraspFix;
 
+
+#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
+
 // Register this plugin with the simulator
 GZ_REGISTER_MODEL_PLUGIN(GazeboGraspFix)
 
 /**
- * Helper class to encapsulate a collision information
+ * Helper class to encapsulate a collision information.
+ * One contact has two bodies, and only
+ * the ones where one of the bodies is a gripper link are considered.
+ * Each contact consists of a *list* of forces with their own origin/position each
+ * (e.g. when the object and gripper are colliding at several places).
+ * The averages of each contact's force vectors along with their origins are
+ * *accumulated* in the given Vector3 \e pos and \eforce objects.
+ * The number of additions is stored in \e sum.
+ * This is to get the average force application over time between link and object.
+ * 
+ * \author Jennifer Buehler
  */
 class GazeboGraspFix::CollidingPoint{
 public:
@@ -27,17 +44,23 @@ public:
         objPos(o.objPos),
         sum(o.sum){}
 
-    //the collision        
+    // the collision        
     physics::CollisionPtr collLink, collObj;
 
-    //average force vector of the colliding point
+    // average force vector of the colliding point
     gazebo::math::Vector3 force;
-    //position (relative to reference frame of gripper collision surface) where the contact happens on collision surface
+
+    // position (relative to reference frame of gripper
+    // collision surface) where the contact happens on collision surface
     gazebo::math::Vector3 pos;
-    //position (relative to reference frame of gripper collision surface) where the object center is located during collision. 
+
+    // position (relative to reference frame of *gripper* collision surface)
+    // where the object center is located during collision. 
     gazebo::math::Vector3 objPos;
      
-    //sum of force and pose (they are actually summed up from several contact points), divide force and pose by this to obtain average
+    // sum of force and pose (they are actually summed
+    // up from several contact points).
+    // Divide both \e force and \e pos by this to obtain average
     int sum;
 };
 
@@ -75,19 +98,16 @@ void GazeboGraspFix::Init(){
 
 void GazeboGraspFix::InitValues(){
 
-    float timeDiff=0.25;
-    this->release_tolerance=0.005;
+    // float timeDiff=0.25;
+    //this->releaseTolerance=0.005;
     this->attached = false;
-    this->updateRate = common::Time(0, common::Time::SecToNano(timeDiff));
+    // this->updateRate = common::Time(0, common::Time::SecToNano(timeDiff));
     this->prevUpdateTime = common::Time::GetWallTime();
-
-
-    float graspedSecs=2;
-    this->max_grip_count=floor(graspedSecs/timeDiff);    
-    this->grip_count_threshold=floor(max_grip_count/2);
+    //float graspedSecs=2;
+    //this->maxGripCount=floor(graspedSecs/timeDiff);    
+    //this->gripCountThreshold=floor(this->maxGripCount/2);
 
     this->attached = false;
-
     this->node = transport::NodePtr(new transport::Node());
 }
 
@@ -108,21 +128,78 @@ void GazeboGraspFix::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
 
     sdf::ElementPtr handLinkElem = _sdf->GetElement("palm_link");
     sdf::ElementPtr fingerLinkElem = _sdf->GetElement("gripper_link");
-
     if (!handLinkElem.get() || !fingerLinkElem.get()) {
-        gzerr<<"Cannot use GazeboGraspFix model plugin because no <palm_link> and/or <gripper_link> elements specified in URDF/SDF."<<std::endl;
+        std::cerr<<"ERROR: GazeboGraspFix: Cannot use GazeboGraspFix model plugin because no <palm_link> and/or <gripper_link> elements specified in URDF/SDF."<<std::endl;
         return;
     }
 
-      this->palmLink = this->model->GetLink(handLinkElem->Get<std::string>());
-    
+    sdf::ElementPtr forcesAngleToleranceElem = _sdf->GetElement("forces_angle_tolerance");
+    if (!forcesAngleToleranceElem.get()){
+        std::cout<<"GazeboGraspFix: Using default tolerance of "<<DEFAULT_FORCES_ANGLE_TOLERANCE<<" because no <forces_angle_tolerance> element specified."<<std::endl;
+        this->forcesAngleTolerance = DEFAULT_FORCES_ANGLE_TOLERANCE * M_PI/180;
+    } else {
+        this->forcesAngleTolerance = forcesAngleToleranceElem->Get<float>() * M_PI/180;
+    }
+
+    sdf::ElementPtr updateRateElem = _sdf->GetElement("update_rate");
+    double _updateSecs;
+    if (!updateRateElem.get()){
+        std::cout<<"GazeboGraspFix: Using  "<<DEFAULT_UPDATE_RATE<<" because no <updateRate_tag> element specified."<<std::endl;
+        _updateSecs = 1.0 / DEFAULT_UPDATE_RATE;
+    } else {
+        int _rate = updateRateElem->Get<int>();
+        double _updateRate = _rate;
+        _updateSecs = 1.0/_updateRate;
+        std::cout<<"GazeboGraspFix: Using update rate "<<_rate<<std::endl;
+    }
+    this->updateRate = common::Time(0, common::Time::SecToNano(_updateSecs));
+
+    sdf::ElementPtr maxGripCountElem = _sdf->GetElement("max_grip_count");
+    if (!maxGripCountElem.get()){
+        std::cout<<"GazeboGraspFix: Using  "<<DEFAULT_MAX_GRIP_COUNT<<" because no <max_grip_count> element specified."<<std::endl;
+        this->maxGripCount = DEFAULT_MAX_GRIP_COUNT;
+    } else {
+        this->maxGripCount = maxGripCountElem->Get<int>();
+        std::cout<<"GazeboGraspFix: Using max_grip_count "<<this->maxGripCount<<std::endl;
+    }
+
+    sdf::ElementPtr gripCountThresholdElem = _sdf->GetElement("grip_count_threshold");
+    if (!gripCountThresholdElem.get()){
+        this->gripCountThreshold=floor(this->maxGripCount/2.0);
+        std::cout<<"GazeboGraspFix: Using  "<<this->gripCountThreshold<<" because no <grip_count_threshold> element specified."<<std::endl;
+    } else {
+        this->gripCountThreshold = gripCountThresholdElem->Get<int>();
+        std::cout<<"GazeboGraspFix: Using grip_count_threshold "<<this->gripCountThreshold<<std::endl;
+    }
+
+    sdf::ElementPtr releaseToleranceElem = _sdf->GetElement("release_tolerance");
+    if (!releaseToleranceElem.get()){
+        std::cout<<"GazeboGraspFix: Using  "<<DEFAULT_RELEASE_TOLERANCE<<" because no <release_tolerance> element specified."<<std::endl;
+        this->releaseTolerance = DEFAULT_RELEASE_TOLERANCE;
+    } else {
+        this->releaseTolerance = releaseToleranceElem->Get<float>();
+        std::cout<<"GazeboGraspFix: Using release_tolerance "<<this->releaseTolerance<<std::endl;
+    }
+
+    sdf::ElementPtr disableCollisionsOnAttachElem = _sdf->GetElement("disable_collisions_on_attach");
+    if (!disableCollisionsOnAttachElem.get()){
+        std::cout<<"GazeboGraspFix: Using  "<<DEFAULT_DISABLE_COLLISIONS_ON_ATTACH<<" because no <disable_collisions_on_attach> element specified."<<std::endl;
+        this->disableCollisionsOnAttach = DEFAULT_DISABLE_COLLISIONS_ON_ATTACH;
+    } else {
+        this->disableCollisionsOnAttach = disableCollisionsOnAttachElem->Get<bool>();
+        std::cout<<"GazeboGraspFix: Using disable_collisions_on_attach "<<this->disableCollisionsOnAttach<<std::endl;
+    }
+
+
+    this->palmLink = this->model->GetLink(handLinkElem->Get<std::string>());
+
     std::vector<std::string> collisionScopedNames;
 
     while (fingerLinkElem) {
         physics::LinkPtr link = this->model->GetLink(fingerLinkElem->Get<std::string>());
         //std::cout<<"Got link "<<fingerLinkElem->Get<std::string>()<<std::endl;
         if (!link.get()){
-            gzerr<<"Link "<<fingerLinkElem->Get<std::string>()<<" specified in Model URDF can't be found in gazebo for GazeboGraspFix model plugin. Skipping."<<std::endl;
+            std::cerr<<"ERROR: Link "<<fingerLinkElem->Get<std::string>()<<" specified in Model URDF can't be found in gazebo for GazeboGraspFix model plugin. Skipping."<<std::endl;
             fingerLinkElem = fingerLinkElem->GetNextElement("gripper_link");
             continue;
         }
@@ -156,174 +233,205 @@ void GazeboGraspFix::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
 }
 
 void GazeboGraspFix::OnUpdate() {
-
-    //std::cout<<"UPDATE!"<<std::endl;
-
-    if (common::Time::GetWallTime() - this->prevUpdateTime < this->updateRate)
+    if ((common::Time::GetWallTime() - this->prevUpdateTime) < this->updateRate)
         return;
 
-    //first, copy all contact data into local struct. Don't do the complex grip check (checkGrip)
-    //within the mutex, because that slows down OnContact().
+    // first, copy all contact data into local struct. Don't do the complex grip check (checkGrip)
+    // within the mutex, because that slows down OnContact().
     this->mutexContacts.lock();
-    std::map<std::string, std::map<std::string, CollidingPoint> > contPoints(contacts);
-    this->contacts.clear();
+    std::map<std::string, std::map<std::string, CollidingPoint> > contPoints(this->contacts);
+    this->contacts.clear(); // clear so it can be filled anew by OnContact().
     this->mutexContacts.unlock();
 
-    //Gather all summed forces for all the objects (so we have all forces on one object). 
+    // contPoints now contains CollidingPoint objects for each *object* and *link*.
+
+    // Gather all summed forces for all the objects (so we have all forces on one object). 
     std::map<std::string, std::vector<gazebo::math::Vector3> > appliedForces; //sorted by object name, all forces effecting on the object
     std::map<std::string, std::map<std::string, CollidingPoint> >::iterator objIt;
-    for (objIt=contPoints.begin(); objIt!=contPoints.end(); ++objIt){
+    for (objIt=contPoints.begin(); objIt!=contPoints.end(); ++objIt)
+    {
         std::string objName=objIt->first;
         //std::cout<<"Examining object collisions with "<<objName<<std::endl;
         std::map<std::string, CollidingPoint>::iterator lIt;
-        
+       
+        // for all links colliding with this object... 
         std::vector<gazebo::math::Vector3> forces;
         for (lIt=objIt->second.begin(); lIt!=objIt->second.end(); ++lIt){
             std::string linkName=lIt->first;
             CollidingPoint& collP=lIt->second;
             gazebo::math::Vector3 avgForce=collP.force/collP.sum;
-            //std::cout<<"Found collision with "<<linkName<<": "<<avgForce.x<<", "<<avgForce.y<<", "<<avgForce.z<<" (avg over "<<collP.sum<<")"<<std::endl;
+            // std::cout << "Found collision with "<<linkName<<": "<<avgForce.x<<", "<<avgForce.y<<", "<<avgForce.z<<" (avg over "<<collP.sum<<")"<<std::endl;
             forces.push_back(avgForce);
         }
         appliedForces[objName]=forces;
     }
 
-
+    // XXX TODO: We actually don't currently need this, because only one object can
+    // be attached at this stage! This must be a remnant of a previous version of the plugin,
+    // unless I'm missing something here. Leaving it in for now.
     std::set<std::string> grippedObjects;    
 
     std::map<std::string, std::vector<gazebo::math::Vector3> >::iterator it;
     for (it=appliedForces.begin(); it!=appliedForces.end(); ++it) {
         
         std::string objName=it->first;
-        float minAngleDiff=120 * M_PI/180;
+        float minAngleDiff= this->forcesAngleTolerance; //120 * M_PI/180;
     
-        //std::cout<<"Number applied forces on "<<objName<<": "<<it->second.size()<<std::endl;
+        // std::cout<<"Number applied forces on "<<objName<<": "<<it->second.size()<<std::endl;
         
-        if (checkGrip(it->second,minAngleDiff,0.3)) {
-            int& counts=gripCounts[objName];
+        if (checkGrip(it->second, minAngleDiff, 0.3)) {
+            int& counts = this->gripCounts[objName];
             
-            if (counts < max_grip_count) ++counts;                
+            if (counts < this->maxGripCount) ++counts;                
             
-            //std::cout<<"GRIPPING "<<objName<<", grip count "<<counts<<" (threshold "<<grip_count_threshold<<") !!!!!!! "<<std::endl;
-        
+            //std::cout<<"GRIPPING "<<objName<<", grip count "<<counts<<" (threshold "<<this->gripCountThreshold<<") !!!!!!! "<<std::endl;
 
-            if (attached && (attachedObjName != objName)) {
-                //if (counts > grip_count_threshold) 
+            if (this->attached && (this->attachedObjName != objName)) {
+                //if (counts > this->gripCountThreshold) 
                     gzwarn<<"GazeboGraspFix has found that object "<<
-                        attachedObjName<<" is already attached, can't grasp "<<objName<<" as well !"<<std::endl;
+                        this->attachedObjName<<" is already attached, can't grasp "<<objName<<" as well !"<<std::endl;
                 continue;
             }
             
-            if (!attached && (counts > grip_count_threshold))  {
+            if (!this->attached && (counts > this->gripCountThreshold))  {
                 std::cout<<"GazeboGraspFix: Attaching "<<objName<<" !!!!!!!"<<std::endl;
                 //for this grip, backup the array of contact poses which played part in the grip, sorted by colliding link
-                attachGripContacts[objName]=contPoints[objName];
+                this->attachGripContacts[objName]=contPoints[objName];
                 if (this->HandleAttach(objName)){
-                    attached=true;
-                    attachedObjName=objName;
+                    this->attached=true;
+                    this->attachedObjName=objName;
                 }
             }
             grippedObjects.insert(objName);
         }
     }
-    
 
 
-    //now, for all objects that are not gripped, decrease grip counter, and possibly release object.
+    // now, for all objects that are not currently gripped,
+    // decrease grip counter, and possibly release object.
     std::map<std::string, int>::iterator gripCntIt;
-    for (gripCntIt=gripCounts.begin(); gripCntIt!=gripCounts.end(); ++gripCntIt){
+    for (gripCntIt = this->gripCounts.begin(); gripCntIt != this->gripCounts.end(); ++gripCntIt){
 
         std::string objName=gripCntIt->first;
 
-        if (grippedObjects.find(objName)!=grippedObjects.end()) continue;
+        if (grippedObjects.find(objName) != grippedObjects.end())
+        {   // this object is one we just detected as "gripped", so no need to check for releasing it...
+            // std::cout<<"NOT considering "<<objName<<" for detachment."<<std::endl;
+            continue;
+        }
         
-        //std::cout<<"NOT-GRIPPING "<<objName<<", grip count "<<gripCntIt->second<<" (threshold "<<grip_count_threshold<<") !!!!!!! "<<std::endl;
+        // the object does not satisfy "gripped" criteria, so potentially has to be released.
 
-        if (gripCntIt->second > 0) --(gripCntIt->second);
-
-        if (!attached || (gripCntIt->second > grip_count_threshold)) continue;
+        //std::cout<<"NOT-GRIPPING "<<objName<<", grip count "<<gripCntIt->second<<" (threshold "<<this->gripCountThreshold<<") !!!!!!! "<<std::endl;
 
-        //object should potentially be detached now. However, this happens too easily when just considering the count, as the fingers
-        //in gazebo start wobbling as the arm moves around, and albeit they are still close to the object, the grip is not 
-        //detected any more. So to be sure, we will check that the collision point has not moved too far from where it originally was.
+        if (gripCntIt->second > 0) --(gripCntIt->second);
 
-        std::map<std::string, std::map<std::string, CollidingPoint> >::iterator collIt=attachGripContacts.find(objName);
-        if (collIt==attachGripContacts.end()) {
-            gzerr<<"Consistency: Could not find attachGripContacts for "<<objName<<std::endl;
+        if (!this->attached || (gripCntIt->second > this->gripCountThreshold)) continue;
+        
+        // std::cout<<"Considering "<<objName<<" for detachment."<<std::endl;
+
+        // Object should potentially be detached now.
+        // However, this happens too easily when just considering the count, as the fingers
+        // in gazebo start wobbling as the arm moves around, and although they are still
+        // close to the object, the grip is not detected any more. So to be sure, we will
+        // check that the collision point (the place on the link where the contact originally
+        // was detected) has not moved too far from where it originally was, relative to the object.
+
+        // get the initial set of CollidingPoints for this object
+        // note that it is enough to use the initial contact points, because the object won't
+        // have "slipped" after being attached, and the location of the original contact point
+        // on the link itself is considered as a fixed point on the link, regardless whether this
+        // point is currently still colliding with the object or not. We only want to know whether
+        // this fixed point on the link has increased in distance from the corresponding fixed
+        // point (where the contact originally happened) on the object.
+        std::map<std::string, std::map<std::string, CollidingPoint> >::iterator initCollIt=this->attachGripContacts.find(objName);
+        if (initCollIt == this->attachGripContacts.end()) {
+            std::cerr<<"ERROR: Consistency: Could not find attachGripContacts for "<<objName<<std::endl;
             continue;
         }
 
-        std::map<std::string, CollidingPoint>& colls=collIt->second;
+        std::map<std::string, CollidingPoint>& initColls=initCollIt->second;
         int cntRelease=0;
-        
+       
+        // for all links which have initially been detected to collide: 
         std::map<std::string, CollidingPoint>::iterator pointIt;
-        for (pointIt=colls.begin(); pointIt!=colls.end(); ++pointIt) {
-
+        for (pointIt=initColls.begin(); pointIt!=initColls.end(); ++pointIt)
+        {
             CollidingPoint& cpInfo=pointIt->second;
+            // initial distance from link to contact point (relative to link)
             gazebo::math::Vector3 relContactPos=cpInfo.pos/cpInfo.sum;
+            // initial distance from link to object (relative to link)
             gazebo::math::Vector3 relObjPos=cpInfo.objPos/cpInfo.sum;
-            
+           
+            // get current world pose of object 
             gazebo::math::Pose currObjWorldPose=cpInfo.collObj->GetLink()->GetWorldPose();
 
-            //now, get the contact location on the collision surface in world coordinates now.
-            gazebo::math::Pose currCollWorldPose=cpInfo.collLink->GetLink()->GetWorldPose();
-            gazebo::math::Matrix4 worldToCollision=currCollWorldPose.rot.GetAsMatrix4();
-            worldToCollision.SetTranslate(currCollWorldPose.pos);
+            // get world pose of link
+            gazebo::math::Pose currLinkWorldPose=cpInfo.collLink->GetLink()->GetWorldPose();
+
+            // Get transform for currLinkWorldPose as matrix
+            gazebo::math::Matrix4 worldToLink=currLinkWorldPose.rot.GetAsMatrix4();
+            worldToLink.SetTranslate(currLinkWorldPose.pos);
 
-            //transform from collision link to contact point
-            gazebo::math::Matrix4 collToContact=gazebo::math::Matrix4::IDENTITY;
-            collToContact.SetTranslate(relContactPos);
+            // Get the transform from collision link to contact point
+            gazebo::math::Matrix4 linkToContact=gazebo::math::Matrix4::IDENTITY;
+            linkToContact.SetTranslate(relContactPos);
                     
-            //now, current world position of the contact point is:
-            gazebo::math::Matrix4 _currContactWorldPose=worldToCollision*collToContact;
+            // the current world position of the contact point right now is:
+            gazebo::math::Matrix4 _currContactWorldPose=worldToLink*linkToContact;
             gazebo::math::Vector3 currContactWorldPose=_currContactWorldPose.GetTranslation();
 
-            gazebo::math::Vector3 oldObjDist=relContactPos-relObjPos;
-            gazebo::math::Vector3 newObjDist=currContactWorldPose-currObjWorldPose.pos;
+            // the initial contact point location on the link should still correspond
+            // to the initial contact point location on the object.
+
+            // initial vector from object center to contact point (relative to link,
+            // because relObjPos and relContactPos are from center of link)
+            gazebo::math::Vector3 oldObjDist= relContactPos - relObjPos;
+            // the same vector as \e oldObjDist, but calculated by the current world pose
+            // of object and the current location of the initial contact location on the link.
+            gazebo::math::Vector3 newObjDist= currContactWorldPose - currObjWorldPose.pos; // new distance from contact to object
             
             //std::cout<<"Obj Trans "<<cpInfo.collLink->GetName()<<": "<<relObjPos.x<<", "<<relObjPos.y<<", "<<relObjPos.z<<std::endl;
             //std::cout<<"Cont Trans "<<cpInfo.collLink->GetName()<<": "<<relContactPos.x<<", "<<relContactPos.y<<", "<<relContactPos.z<<std::endl;
-
+        
+            // the difference between these vectors should not be too large...
             float diff=fabs(oldObjDist.GetLength() - newObjDist.GetLength());
             //std::cout<<"Diff for link "<<cpInfo.collLink->GetName()<<": "<<diff<<std::endl;
 
-            if (diff > release_tolerance) {
+            if (diff > releaseTolerance) {
                 ++cntRelease;
             }
         }
 
-        if (cntRelease > 0){ //sufficient links did not meet the criteria to be close enough to the object.
+        if (cntRelease > 0)
+        { // sufficient links did not meet the criteria to be close enough to the object.
             std::cout<<"GazeboGraspFix: Detaching "<<objName<<" !!!!!!!"<<std::endl;
             this->HandleDetach(objName);
-            attached=false;
-            attachedObjName="";
+            this->attached=false;
+            this->attachedObjName="";
             gripCntIt->second=0;
-            attachGripContacts.erase(collIt);
+            this->attachGripContacts.erase(initCollIt);
         }
     }
 
-
-
     this->prevUpdateTime = common::Time::GetWallTime();
-
 }
-
     
 double angularDistance(const gazebo::math::Vector3& _v1, const gazebo::math::Vector3& _v2) {
     gazebo::math::Vector3 v1=_v1;        
     gazebo::math::Vector3 v2=_v2;
     v1.Normalize();
-    v2.Normalize();    
+    v2.Normalize();
     return acos(v1.Dot(v2));    
 }
 
 bool GazeboGraspFix::checkGrip(const std::vector<gazebo::math::Vector3>& forces, float minAngleDiff, float lengthRatio){
     if (((lengthRatio > 1) || (lengthRatio < 0)) && (lengthRatio > 1e-04 && (fabs(lengthRatio-1) > 1e-04)))  {
-        gzerr<<"checkGrip: always specify a lengthRatio of [0..1]"<<std::endl;
+        std::cerr<<"ERROR: checkGrip: always specify a lengthRatio of [0..1]"<<std::endl;
         return false;
     }
     if (minAngleDiff < M_PI_2){
-        gzerr<<"checkGrip: min angle must be at least 90 degrees (PI/2)"<<std::endl;
+        std::cerr<<"ERROR: checkGrip: min angle must be at least 90 degrees (PI/2)"<<std::endl;
         return false;
     }
     std::vector<gazebo::math::Vector3>::const_iterator it1, it2;
@@ -334,46 +442,52 @@ bool GazeboGraspFix::checkGrip(const std::vector<gazebo::math::Vector3>& forces,
             float l1=v1.GetLength();
             float l2=v2.GetLength();
             if ((l1<1e-04) || (l2<1e-04)) continue;
-            gazebo::math::Vector3 _v1=v1;
+            /*gazebo::math::Vector3 _v1=v1;
             gazebo::math::Vector3 _v2=v2;
             _v1/=l1;
             _v2/=l2;
-            float angle=acos(_v1.Dot(_v2));
+            float angle=acos(_v1.Dot(_v2));*/
+            float angle=angularDistance(v1, v2);
+            // std::cout<<"Angular distance between v1.len="<<v1.GetLength()<<" and v2.len="<<v2.GetLength()<<": "<<angle*180/M_PI<<std::endl;
             if (angle > minAngleDiff) {
                 float ratio;
                 if (l1>l2) ratio=l2/l1;
                 else ratio=l1/l2;
-                if (ratio >= lengthRatio) return true;
+                // std::cout<<"Got angle "<<angle<<", ratio "<<ratio<<std::endl;
+                if (ratio >= lengthRatio)
+                {
+                    // std::cout<<"checkGrip() is true"<<std::endl;
+                    return true;
+                }
             }            
         }
     }
+    return false;
 }
 
-
-void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg) {
-
-    
+void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg)
+{
     //std::cout<<"CONTACT! "<<std::endl;//<<_contact<<std::endl;
+    // for all contacts...
     for (int i = 0; i < _msg->contact_size(); ++i) {
         physics::CollisionPtr collision1 = boost::dynamic_pointer_cast<physics::Collision>(
                 this->world->GetEntity(_msg->contact(i).collision1()));
         physics::CollisionPtr collision2 = boost::dynamic_pointer_cast<physics::Collision>(
                 this->world->GetEntity(_msg->contact(i).collision2()));
 
-        if ((collision1 && !collision1->IsStatic()) && (collision2 && !collision2->IsStatic())) {
-            
+        if ((collision1 && !collision1->IsStatic()) && (collision2 && !collision2->IsStatic()))
+        {
             std::string name1 = collision1->GetScopedName();
             std::string name2 = collision2->GetScopedName();
-            
 
             //std::cout<<"OBJ CONTACT! "<<name1<<" / "<<name2<<std::endl;
-
             int count = _msg->contact(i).position_size();
 
             // Check to see if the contact arrays all have the same size.
             if ((count != _msg->contact(i).normal_size()) ||
                 (count != _msg->contact(i).wrench_size()) ||
-                (count != _msg->contact(i).depth_size())) {
+                (count != _msg->contact(i).depth_size()))
+            {
                 gzerr << "GazeboGraspFix: Contact message has invalid array sizes\n";
                 continue;
             }
@@ -381,142 +495,167 @@ void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg) {
             std::string collidingObjName, collidingLink;
             physics::CollisionPtr linkCollision;
             physics::CollisionPtr objCollision;
+
             physics::Contact contact;
             contact = _msg->contact(i);
 
-            if (contact.count<1){
-                gzerr<<"GazeboGraspFix: Not enough forces given for contact of ."<<name1<<" / "<<name2<<std::endl;
+            if (contact.count<1)
+            {
+                std::cerr<<"ERROR: GazeboGraspFix: Not enough forces given for contact of ."<<name1<<" / "<<name2<<std::endl;
                 continue;
             }
 
+            // all force vectors which are part of this contact
             std::vector<gazebo::math::Vector3> force;
             
-            //find out which part of the colliding objects is not the gripper
-            if (this->collisions.find(name1) == this->collisions.end()) {
+            // find out which part of the colliding entities is the object, *not* the gripper,
+            // and copy all the forces applied to it into the vector 'force'.
+            if (this->collisions.find(name1) == this->collisions.end())
+            {   // collision 1 is the object
                 collidingObjName=name1;
                 collidingLink=name2;
                 linkCollision=collision2;
                 objCollision=collision1;
-                for (int k=0; k<contact.count; ++k) force.push_back(contact.wrench[k].body1Force);
-            }else if (this->collisions.find(name2) == this->collisions.end()) {
+                for (int k=0; k<contact.count; ++k)
+                    force.push_back(contact.wrench[k].body1Force);
+            }
+            else if (this->collisions.find(name2) == this->collisions.end())
+            {   // collision 2 is the object
                 collidingObjName=name2;
                 collidingLink=name1;
                 linkCollision=collision1;
                 objCollision=collision2;
-                for (int k=0; k<contact.count; ++k) force.push_back(contact.wrench[k].body2Force);
+                for (int k=0; k<contact.count; ++k)
+                    force.push_back(contact.wrench[k].body2Force);
             }
 
-            gazebo::math::Vector3 sumForce;
-            //compute sum of the body wrenches
+            gazebo::math::Vector3 avgForce;
+            // compute average/sum of the forces applied on the object
             for (int k=0; k<force.size(); ++k){
-                sumForce+=force[k];
+                avgForce+=force[k];
             }    
-            sumForce/=force.size();
+            avgForce/=force.size();
 
             gazebo::math::Vector3 avgPos;
+            // compute center point (average pose) of all the origin positions of the forces appied
             for (int k=0; k<contact.count; ++k) avgPos+=contact.positions[k];
             avgPos/=contact.count;
 
-            //now, get average pose relative to the colliding link
-            gazebo::math::Pose collWorldPose=linkCollision->GetLink()->GetWorldPose();
+            // now, get average pose relative to the colliding link
+            gazebo::math::Pose linkWorldPose=linkCollision->GetLink()->GetWorldPose();
 
-            //To find out the collision point relative to the Link's local coordinate system, first get the Poses as 4x4 matrices
-            gazebo::math::Matrix4 worldToCollision=collWorldPose.rot.GetAsMatrix4();
-            worldToCollision.SetTranslate(collWorldPose.pos);
+            // To find out the collision point relative to the Link's local coordinate system, first get the Poses as 4x4 matrices
+            gazebo::math::Matrix4 worldToLink=linkWorldPose.rot.GetAsMatrix4();
+            worldToLink.SetTranslate(linkWorldPose.pos);
             
             gazebo::math::Matrix4 worldToContact=gazebo::math::Matrix4::IDENTITY;
             //we can assume that the contact has identity rotation because we don't care about its orientation.
             //We could always set another rotation here too.
             worldToContact.SetTranslate(avgPos);
 
-
-            //now, worldToCollision * contactInLocal = worldToContact
-            //hence, contactInLocal = worldToCollision.Inv * worldToContact
-            gazebo::math::Matrix4 worldToCollisionInv = worldToCollision.Inverse();
-            gazebo::math::Matrix4 contactInLocal = worldToCollisionInv * worldToContact;
+            // now, worldToLink * contactInLocal = worldToContact
+            // hence, contactInLocal = worldToLink.Inv * worldToContact
+            gazebo::math::Matrix4 worldToLinkInv = worldToLink.Inverse();
+            gazebo::math::Matrix4 contactInLocal = worldToLinkInv * worldToContact;
             gazebo::math::Vector3 contactPosInLocal = contactInLocal.GetTranslation();
             
             //std::cout<<"---------"<<std::endl;    
             //std::cout<<"CNT in loc: "<<contactPosInLocal.x<<","<<contactPosInLocal.y<<","<<contactPosInLocal.z<<std::endl;
 
-            /*gazebo::math::Vector3 sDiff=avgPos-collWorldPose.pos;
+            /*gazebo::math::Vector3 sDiff=avgPos-linkWorldPose.pos;
             std::cout<<"SIMPLE trans: "<<sDiff.x<<","<<sDiff.y<<","<<sDiff.z<<std::endl;
-            std::cout<<"coll world pose: "<<collWorldPose.pos.x<<", "<<collWorldPose.pos.y<<", "<<collWorldPose.pos.z<<std::endl; 
+            std::cout<<"coll world pose: "<<linkWorldPose.pos.x<<", "<<linkWorldPose.pos.y<<", "<<linkWorldPose.pos.z<<std::endl; 
             std::cout<<"contact avg pose: "<<avgPos.x<<", "<<avgPos.y<<", "<<avgPos.z<<std::endl; 
 
-            gazebo::math::Vector3 lX=collWorldPose.rot.GetXAxis();    
-            gazebo::math::Vector3 lY=collWorldPose.rot.GetYAxis();    
-            gazebo::math::Vector3 lZ=collWorldPose.rot.GetZAxis();    
+            gazebo::math::Vector3 lX=linkWorldPose.rot.GetXAxis();    
+            gazebo::math::Vector3 lY=linkWorldPose.rot.GetYAxis();    
+            gazebo::math::Vector3 lZ=linkWorldPose.rot.GetZAxis();    
     
-            std::cout<<"World ori: "<<collWorldPose.rot.x<<","<<collWorldPose.rot.y<<","<<collWorldPose.rot.z<<","<<collWorldPose.rot.w<<std::endl;
+            std::cout<<"World ori: "<<linkWorldPose.rot.x<<","<<linkWorldPose.rot.y<<","<<linkWorldPose.rot.z<<","<<linkWorldPose.rot.w<<std::endl;
             std::cout<<"x axis: "<<lX.x<<","<<lX.y<<","<<lX.z<<std::endl;
             std::cout<<"y axis: "<<lY.x<<","<<lY.y<<","<<lY.z<<std::endl;
             std::cout<<"z axis: "<<lZ.x<<","<<lZ.y<<","<<lZ.z<<std::endl;*/
 
-            //now, get the pose of the object and compute it's relative position to the collision surface.
-            gazebo::math::Pose objWorldPose=objCollision->GetLink()->GetWorldPose();
-            gazebo::math::Matrix4 worldToObj=objWorldPose.rot.GetAsMatrix4();
+            // now, get the pose of the object and compute it's relative position to the collision surface.
+            gazebo::math::Pose objWorldPose = objCollision->GetLink()->GetWorldPose();
+            gazebo::math::Matrix4 worldToObj = objWorldPose.rot.GetAsMatrix4();
             worldToObj.SetTranslate(objWorldPose.pos);
     
-            gazebo::math::Matrix4 objInLocal = worldToCollisionInv * worldToObj;
+            gazebo::math::Matrix4 objInLocal = worldToLinkInv * worldToObj;
             gazebo::math::Vector3 objPosInLocal = objInLocal.GetTranslation();
 
             {
                 boost::mutex::scoped_lock lock(this->mutexContacts);
-                CollidingPoint& p=contacts[collidingObjName][collidingLink];
+                CollidingPoint& p = this->contacts[collidingObjName][collidingLink];  // inserts new entry if doesn't exist
                 p.collLink=linkCollision;
                 p.collObj=objCollision;
-                p.force+=sumForce;
+                p.force+=avgForce;
                 p.pos+=contactPosInLocal;
                 p.objPos+=objPosInLocal;
                 p.sum++;
             }
-            //std::cout<<"Average force of contact= "<<sumForce.x<<", "<<sumForce.y<<", "<<sumForce.z<<" out of "<<force.size()<<" vectors."<<std::endl;
-                    
+            //std::cout<<"Average force of contact= "<<avgForce.x<<", "<<avgForce.y<<", "<<avgForce.z<<" out of "<<force.size()<<" vectors."<<std::endl;
         }
     }
 }
 
 
-bool GazeboGraspFix::HandleAttach(const std::string& objName) {
+// #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 GazeboGraspFix::HandleAttach(const std::string& objName)
+{
     if (!this->palmLink) {
         gzwarn << "palm link not found, not enforcing grasp hack!\n";
         return false;
     }
+#ifdef USE_MODEL_ATTACH
+    physics::ModelPtr obj = this->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>(this->world->GetEntity(objName));
     if (!obj.get()){
-        gzerr<<"Object "<<objName<<" not found in world, can't attach it"<<std::endl;
+        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);
-
-    //we should 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");
+    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
     return true;
 }
 
-void GazeboGraspFix::HandleDetach(const std::string& objName){
-    
+void GazeboGraspFix::HandleDetach(const std::string& objName)
+{
+#ifdef USE_MODEL_ATTACH
+     physics::ModelPtr obj = this->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>(this->world->GetEntity(objName));
     if (!obj.get()){
-        gzerr<<"Object "<<objName<<" not found in world, can't attach it"<<std::endl;
+        std::cerr<<"ERROR: Object "<<objName<<" not found in world, can't attach it"<<std::endl;
         return;
-    }else{
+    }
+    else if (this->disableCollisionsOnAttach)
+    {
         obj->GetLink()->SetCollideMode("all");
     }
-    
     this->fixedJoint->Detach();
-
+#endif  // USE_MODEL_ATTACH
 }
-
-
-
-
-