added sources

README.md

 # gazebo-pkgs
+
 A collection of tools and plugins for Gazebo

gazebo_grasp_plugin/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(gazebo_grasp_plugin)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  gazebo_ros
+  geometry_msgs
+  roscpp
+  std_msgs
+)
+
+find_package(gazebo REQUIRED)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES gazebo_grasp_fix
+  CATKIN_DEPENDS gazebo_ros geometry_msgs roscpp std_msgs
+  DEPENDS gazebo 
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${GAZEBO_INCLUDE_DIRS}
+)
+
+link_directories(
+  ${GAZEBO_LIBRARY_DIRS}
+  ${catkin_LIBRARY_DIRS}
+)
+
+## Declare a cpp library
+add_library(gazebo_grasp_fix SHARED src/GazeboGraspFix.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+# add_dependencies(gazebo_grasp_plugin_node gazebo_grasp_plugin_generate_messages_cpp)
+add_dependencies(gazebo_grasp_fix ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+
+target_link_libraries(gazebo_grasp_fix 
+	${GAZEBO_LIBRARIES} 
+	${Boost_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS gazebo_grasp_plugin gazebo_grasp_plugin_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_gazebo_grasp_plugin.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

gazebo_grasp_plugin/include/gazebo_grasp_plugin/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>
+
+namespace gazebo {
+
+/**
+ * Inspired by gazebo::physics::Gripper, this is our own implementation which does not use deprecated functions any more.
+ *
+ * This is a *model* pluing, so you have to load the model plugin from the robot URDF:
+ *
+ * ```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>
+ *       </plugin>
+ *   </gazebo>
+ * ```
+ *
+ * \author Jennifer Buehler
+ */ 
+class GazeboGraspFix : public ModelPlugin {
+public:
+    GazeboGraspFix();
+    GazeboGraspFix(physics::ModelPtr _model);
+    virtual ~GazeboGraspFix();
+    virtual void Init(); 
+    virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+    void OnUpdate();
+private: 
+
+    void InitValues();
+    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);
+    
+    physics::ModelPtr model;
+
+    physics::PhysicsEnginePtr physics;
+    physics::WorldPtr world;
+    physics::JointPtr fixedJoint;
+    
+    physics::LinkPtr palmLink;
+    event::ConnectionPtr update_connection;
+    transport::NodePtr node;
+    transport::SubscriberPtr contactSub; //subscriber to contact updates
+    
+    std::map<std::string, physics::CollisionPtr> collisions;
+
+    /**
+     * Helper class to encapsulate a collision information.
+     * Forward declaration here.
+     */
+    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.
+    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.
+    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.
+    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;
+
+    bool attached;
+    std::string attachedObjName;    
+
+    //nano seconds between two updates
+    common::Time updateRate;
+
+    //last time OnUpdate() was called
+    common::Time prevUpdateTime;
+};
+
+}

gazebo_grasp_plugin/package.xml

+<?xml version="1.0"?>
+<package>
+  <name>gazebo_grasp_plugin</name>
+  <version>0.0.0</version>
+  <description>
+        Gazebo Model plugin(s) which handle/help grasping in Gazebo.
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <maintainer email="jennifer.e.buehler@gmail.com">Jennifer Buehler</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>GPLv3</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/gazebo_grasp_plugin</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>gazebo_ros</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

gazebo_grasp_plugin/src/GazeboGraspFix.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/GazeboGraspFix.h>
+
+using gazebo::GazeboGraspFix;
+
+// Register this plugin with the simulator
+GZ_REGISTER_MODEL_PLUGIN(GazeboGraspFix)
+
+/**
+ * Helper class to encapsulate a collision information
+ */
+class GazeboGraspFix::CollidingPoint{
+public:
+    CollidingPoint(): sum(0) {}
+    CollidingPoint(const CollidingPoint& o):
+        collLink(o.collLink),
+        collObj(o.collObj),
+        force(o.force),
+        pos(o.pos),
+        objPos(o.objPos),
+        sum(o.sum){}
+
+    //the collision        
+    physics::CollisionPtr collLink, collObj;
+
+    //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
+    gazebo::math::Vector3 pos;
+    //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
+    int sum;
+};
+
+
+
+
+GazeboGraspFix::GazeboGraspFix(){
+    InitValues();
+}
+
+GazeboGraspFix::GazeboGraspFix(physics::ModelPtr _model){
+    this->model = _model;
+    InitValues();
+}
+
+GazeboGraspFix::~GazeboGraspFix() {
+    /**
+    //XXX this is for gazebo 4.0
+      if (this->world && this->model && this->world->GetRunning()) {
+        physics::ContactManager *mgr = this->world->GetPhysicsEngine()->GetContactManager();
+        mgr->RemoveFilter(this->model->GetScopedName());
+    }
+    */
+
+    this->model.reset();
+    this->physics.reset();
+    this->update_connection.reset();
+    if (this->node) this->node->Fini();
+    this->node.reset();
+}
+
+void GazeboGraspFix::Init(){
+    this->prevUpdateTime = common::Time::GetWallTime();
+}
+
+void GazeboGraspFix::InitValues(){
+
+    float timeDiff=0.25;
+    this->release_tolerance=0.005;
+    this->attached = false;
+    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);
+
+    this->attached = false;
+
+    this->node = transport::NodePtr(new transport::Node());
+}
+
+
+void GazeboGraspFix::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
+    this->model = _parent;
+    this->world = this->model->GetWorld();
+    this->physics = this->model->GetWorld()->GetPhysicsEngine();
+    this->fixedJoint = this->physics->CreateJoint("revolute");
+
+    this->node->Init(this->model->GetWorld()->GetName());
+        
+    physics::ContactManager * contactManager = this->physics->GetContactManager();
+    contactManager->PublishContacts(); //XXX not sure I need this?
+
+    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;
+        return;
+    }
+
+      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;
+            fingerLinkElem = fingerLinkElem->GetNextElement("gripper_link");
+            continue;
+        }
+        for (unsigned int j = 0; j < link->GetChildCount(); ++j) {
+            physics::CollisionPtr collision = link->GetCollision(j);
+            std::string scopedName = collision->GetScopedName();
+
+            std::map<std::string, physics::CollisionPtr>::iterator collIter = this->collisions.find(scopedName);
+            if (collIter != this->collisions.end()) { //this collision was already added before
+                continue;
+            }
+
+            /*collision->SetContactsEnabled(true);
+            this->connections.push_back(collision->ConnectContact(boost::bind(&GazeboGraspFix::OnContact, this, _1, _2)));*/
+            std::cout<<"GazeboGraspFix: Adding collision scoped name "<<scopedName<<std::endl;
+            
+            this->collisions[scopedName] = collision;
+            collisionScopedNames.push_back(scopedName);
+        }
+        fingerLinkElem = fingerLinkElem->GetNextElement("gripper_link");
+    }
+
+    std::string topic = contactManager->CreateFilter(this->model->GetScopedName(), collisionScopedNames);
+    if (!this->contactSub.get()) {
+        std::cout<<"Subscribing contact manager to topic "<<topic<<std::endl;
+        bool latching=false;
+        this->contactSub = this->node->Subscribe(topic,&GazeboGraspFix::OnContact, this, latching);
+    }
+
+    update_connection=event::Events::ConnectWorldUpdateEnd(boost::bind(&GazeboGraspFix::OnUpdate, this));
+}
+
+void GazeboGraspFix::OnUpdate() {
+
+    //std::cout<<"UPDATE!"<<std::endl;
+
+    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().
+    this->mutexContacts.lock();
+    std::map<std::string, std::map<std::string, CollidingPoint> > contPoints(contacts);
+    this->contacts.clear();
+    this->mutexContacts.unlock();
+
+    //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){
+        std::string objName=objIt->first;
+        //std::cout<<"Examining object collisions with "<<objName<<std::endl;
+        std::map<std::string, CollidingPoint>::iterator lIt;
+        
+        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;
+            forces.push_back(avgForce);
+        }
+        appliedForces[objName]=forces;
+    }
+
+
+    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;
+    
+        //std::cout<<"Number applied forces on "<<objName<<": "<<it->second.size()<<std::endl;
+        
+        if (checkGrip(it->second,minAngleDiff,0.3)) {
+            int& counts=gripCounts[objName];
+            
+            if (counts < max_grip_count) ++counts;                
+            
+            //std::cout<<"GRIPPING "<<objName<<", grip count "<<counts<<" (threshold "<<grip_count_threshold<<") !!!!!!! "<<std::endl;
+        
+
+            if (attached && (attachedObjName != objName)) {
+                //if (counts > grip_count_threshold) 
+                    gzwarn<<"GazeboGraspFix has found that object "<<
+                        attachedObjName<<" is already attached, can't grasp "<<objName<<" as well !"<<std::endl;
+                continue;
+            }
+            
+            if (!attached && (counts > grip_count_threshold))  {
+                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];
+                if (this->HandleAttach(objName)){
+                    attached=true;
+                    attachedObjName=objName;
+                }
+            }
+            grippedObjects.insert(objName);
+        }
+    }
+    
+
+
+    //now, for all objects that are not gripped, decrease grip counter, and possibly release object.
+    std::map<std::string, int>::iterator gripCntIt;
+    for (gripCntIt=gripCounts.begin(); gripCntIt!=gripCounts.end(); ++gripCntIt){
+
+        std::string objName=gripCntIt->first;
+
+        if (grippedObjects.find(objName)!=grippedObjects.end()) continue;
+        
+        //std::cout<<"NOT-GRIPPING "<<objName<<", grip count "<<gripCntIt->second<<" (threshold "<<grip_count_threshold<<") !!!!!!! "<<std::endl;
+
+        if (gripCntIt->second > 0) --(gripCntIt->second);
+
+        if (!attached || (gripCntIt->second > grip_count_threshold)) continue;
+
+        //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.
+
+        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;
+            continue;
+        }
+
+        std::map<std::string, CollidingPoint>& colls=collIt->second;
+        int cntRelease=0;
+        
+        std::map<std::string, CollidingPoint>::iterator pointIt;
+        for (pointIt=colls.begin(); pointIt!=colls.end(); ++pointIt) {
+
+            CollidingPoint& cpInfo=pointIt->second;
+            gazebo::math::Vector3 relContactPos=cpInfo.pos/cpInfo.sum;
+            gazebo::math::Vector3 relObjPos=cpInfo.objPos/cpInfo.sum;
+            
+            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);
+
+            //transform from collision link to contact point
+            gazebo::math::Matrix4 collToContact=gazebo::math::Matrix4::IDENTITY;
+            collToContact.SetTranslate(relContactPos);
+                    
+            //now, current world position of the contact point is:
+            gazebo::math::Matrix4 _currContactWorldPose=worldToCollision*collToContact;
+            gazebo::math::Vector3 currContactWorldPose=_currContactWorldPose.GetTranslation();
+
+            gazebo::math::Vector3 oldObjDist=relContactPos-relObjPos;
+            gazebo::math::Vector3 newObjDist=currContactWorldPose-currObjWorldPose.pos;
+            
+            //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;
+
+            float diff=fabs(oldObjDist.GetLength() - newObjDist.GetLength());
+            //std::cout<<"Diff for link "<<cpInfo.collLink->GetName()<<": "<<diff<<std::endl;
+
+            if (diff > release_tolerance) {
+                ++cntRelease;
+            }
+        }
+
+        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="";
+            gripCntIt->second=0;
+            attachGripContacts.erase(collIt);
+        }
+    }
+
+
+
+    this->prevUpdateTime = common::Time::GetWallTime();
+
+}
+
+    
+double angularDistance(const gazebo::math::Vector3& _v1, const gazebo::math::Vector3& _v2)