Commit 0b900eee authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

added parameters and more control, tested the issue #1 reported on github

parent e4b18b27
......@@ -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
......
......@@ -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) {
++cntRele