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 } - - - - -