00001
00002
00003
00004
00005 #include <boost/bind.hpp>
00006 #include <gazebo/gazebo.hh>
00007 #include <gazebo/physics/physics.hh>
00008 #include <gazebo/physics/ContactManager.hh>
00009 #include <gazebo/physics/Contact.hh>
00010 #include <gazebo/common/common.hh>
00011 #include <stdio.h>
00012
00013 #include <robotican_common/GazeboGraspFix.h>
00014
00015 using gazebo::GazeboGraspFix;
00016
00017
00018 #define DEFAULT_FORCES_ANGLE_TOLERANCE 120
00019 #define DEFAULT_UPDATE_RATE 5
00020 #define DEFAULT_MAX_GRIP_COUNT 10
00021 #define DEFAULT_RELEASE_TOLERANCE 0.005
00022 #define DEFAULT_DISABLE_COLLISIONS_ON_ATTACH false
00023
00024
00025 GZ_REGISTER_MODEL_PLUGIN(GazeboGraspFix)
00026
00027
00028 GazeboGraspFix::GazeboGraspFix(){
00029 InitValues();
00030 }
00031
00032 GazeboGraspFix::GazeboGraspFix(physics::ModelPtr _model)
00033 {
00034 InitValues();
00035 }
00036
00037 GazeboGraspFix::~GazeboGraspFix()
00038 {
00039 this->update_connection.reset();
00040 if (this->node) this->node->Fini();
00041 this->node.reset();
00042 }
00043
00044 void GazeboGraspFix::Init()
00045 {
00046 this->prevUpdateTime = common::Time::GetWallTime();
00047 }
00048
00049 void GazeboGraspFix::InitValues()
00050 {
00051 #if GAZEBO_MAJOR_VERSION > 2
00052 gazebo::common::Console::SetQuiet(false);
00053 #endif
00054
00055
00056
00057
00058 this->prevUpdateTime = common::Time::GetWallTime();
00059
00060
00061
00062 this->node = transport::NodePtr(new transport::Node());
00063 }
00064
00065
00066 void GazeboGraspFix::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00067 {
00068 std::cout<<"Loading grasp-fix plugin"<<std::endl;
00069
00070
00071
00072 physics::ModelPtr model = _parent;
00073 this->world = model->GetWorld();
00074
00075 sdf::ElementPtr disableCollisionsOnAttachElem = _sdf->GetElement("disable_collisions_on_attach");
00076 if (!disableCollisionsOnAttachElem){
00077 std::cout<<"GazeboGraspFix: Using default "<<DEFAULT_DISABLE_COLLISIONS_ON_ATTACH<<" because no <disable_collisions_on_attach> element specified."<<std::endl;
00078 this->disableCollisionsOnAttach = DEFAULT_DISABLE_COLLISIONS_ON_ATTACH;
00079 } else {
00080 std::string str = disableCollisionsOnAttachElem->Get<std::string>();
00081 bool bVal = false;
00082 if ((str=="true") || (str=="1")) bVal = true;
00083 this->disableCollisionsOnAttach = bVal;
00084 std::cout<<"GazeboGraspFix: Using disable_collisions_on_attach "<<this->disableCollisionsOnAttach<<std::endl;
00085 }
00086
00087 sdf::ElementPtr forcesAngleToleranceElem = _sdf->GetElement("forces_angle_tolerance");
00088 if (!forcesAngleToleranceElem){
00089 std::cout<<"GazeboGraspFix: Using default tolerance of "<<DEFAULT_FORCES_ANGLE_TOLERANCE<<" because no <forces_angle_tolerance> element specified."<<std::endl;
00090 this->forcesAngleTolerance = DEFAULT_FORCES_ANGLE_TOLERANCE * M_PI/180;
00091 } else {
00092 this->forcesAngleTolerance = forcesAngleToleranceElem->Get<float>() * M_PI/180;
00093 }
00094
00095 sdf::ElementPtr updateRateElem = _sdf->GetElement("update_rate");
00096 double _updateSecs;
00097 if (!updateRateElem){
00098 std::cout<<"GazeboGraspFix: Using "<<DEFAULT_UPDATE_RATE<<" because no <updateRate_tag> element specified."<<std::endl;
00099 _updateSecs = 1.0 / DEFAULT_UPDATE_RATE;
00100 } else {
00101 int _rate = updateRateElem->Get<int>();
00102 double _updateRate = _rate;
00103 _updateSecs = 1.0/_updateRate;
00104 std::cout<<"GazeboGraspFix: Using update rate "<<_rate<<std::endl;
00105 }
00106 this->updateRate = common::Time(0, common::Time::SecToNano(_updateSecs));
00107
00108 sdf::ElementPtr maxGripCountElem = _sdf->GetElement("max_grip_count");
00109 if (!maxGripCountElem){
00110 std::cout<<"GazeboGraspFix: Using "<<DEFAULT_MAX_GRIP_COUNT<<" because no <max_grip_count> element specified."<<std::endl;
00111 this->maxGripCount = DEFAULT_MAX_GRIP_COUNT;
00112 } else {
00113 this->maxGripCount = maxGripCountElem->Get<int>();
00114 std::cout<<"GazeboGraspFix: Using max_grip_count "<<this->maxGripCount<<std::endl;
00115 }
00116
00117 sdf::ElementPtr gripCountThresholdElem = _sdf->GetElement("grip_count_threshold");
00118 if (!gripCountThresholdElem){
00119 this->gripCountThreshold=floor(this->maxGripCount/2.0);
00120 std::cout<<"GazeboGraspFix: Using "<<this->gripCountThreshold<<" because no <grip_count_threshold> element specified."<<std::endl;
00121 } else {
00122 this->gripCountThreshold = gripCountThresholdElem->Get<int>();
00123 std::cout<<"GazeboGraspFix: Using grip_count_threshold "<<this->gripCountThreshold<<std::endl;
00124 }
00125
00126 sdf::ElementPtr releaseToleranceElem = _sdf->GetElement("release_tolerance");
00127 if (!releaseToleranceElem){
00128 std::cout<<"GazeboGraspFix: Using "<<DEFAULT_RELEASE_TOLERANCE<<" because no <release_tolerance> element specified."<<std::endl;
00129 this->releaseTolerance = DEFAULT_RELEASE_TOLERANCE;
00130 } else {
00131 this->releaseTolerance = releaseToleranceElem->Get<float>();
00132 std::cout<<"GazeboGraspFix: Using release_tolerance "<<this->releaseTolerance<<std::endl;
00133 }
00134
00135
00136 std::vector<std::string> collisionNames;
00137
00138 sdf::ElementPtr armElem=_sdf->GetElement("arm");
00139 if (!armElem)
00140 {
00141 gzerr <<"GazeboGraspFix: Cannot load the GazeboGraspFix without any <arm> declarations"<<std::endl;
00142 return;
00143 }
00144
00145 for (; armElem != NULL; armElem = armElem->GetNextElement("arm"))
00146 {
00147 sdf::ElementPtr armNameElem = armElem->GetElement("arm_name");
00148 sdf::ElementPtr handLinkElem = armElem->GetElement("palm_link");
00149 sdf::ElementPtr fingerLinkElem = armElem->GetElement("gripper_link");
00150
00151 if (!handLinkElem || !fingerLinkElem || !armNameElem) {
00152 gzerr << "ERROR: GazeboGraspFix: Cannot use a GazeboGraspFix arm because "
00153 << "not all of <arm_name>, <palm_link> and <gripper_link> elements specified in URDF/SDF. Skipping."<<std::endl;
00154 continue;
00155 }
00156
00157 std::string armName = armNameElem->Get<std::string>();
00158 std::string palmName = handLinkElem->Get<std::string>();
00159
00160
00161 std::vector<std::string> fingerLinkNames;
00162 for (; fingerLinkElem != NULL; fingerLinkElem = fingerLinkElem->GetNextElement("gripper_link"))
00163 {
00164 std::string linkName = fingerLinkElem->Get<std::string>();
00165 fingerLinkNames.push_back(linkName);
00166 }
00167
00168
00169 if (grippers.find(armName)!=grippers.end())
00170 {
00171 gzerr<<"GazeboGraspFix: Arm named "<<armName<<" was already added, cannot add it twice."<<std::endl;
00172 }
00173 GazeboGraspGripper& gripper = grippers[armName];
00174 std::map<std::string, physics::CollisionPtr> _collisions;
00175 if (!gripper.Init(_parent, armName, palmName, fingerLinkNames, disableCollisionsOnAttach, _collisions))
00176 {
00177 gzerr<<"GazeboGraspFix: Could not initialize arm "<<armName<<". Skipping."<<std::endl;
00178 grippers.erase(armName);
00179 continue;
00180 }
00181
00182 for (std::map<std::string, physics::CollisionPtr>::iterator collIt = _collisions.begin();
00183 collIt != _collisions.end(); ++collIt)
00184 {
00185 const std::string& collName=collIt->first;
00186
00187 std::map<std::string, std::string>::iterator collIter = this->collisions.find(collName);
00188 if (collIter != this->collisions.end()) {
00189 gzwarn <<"GazeboGraspFix: Adding Gazebo collision link element "<<collName<<" multiple times, the grasp plugin may not work properly"<<std::endl;
00190 continue;
00191 }
00192 std::cout<<"GazeboGraspFix: Adding collision scoped name "<<collName<<std::endl;
00193 this->collisions[collName] = armName;
00194 collisionNames.push_back(collName);
00195 }
00196 }
00197
00198 if (grippers.empty())
00199 {
00200 gzerr << "ERROR: GazeboGraspFix: Cannot use a GazeboGraspFix because "
00201 << "no arms were configured successfully. Plugin will not work."<<std::endl;
00202 return;
00203 }
00204
00205
00206
00207 physics::PhysicsEnginePtr physics = this->world->GetPhysicsEngine();
00208 this->node->Init(this->world->GetName());
00209 physics::ContactManager * contactManager = physics->GetContactManager();
00210 contactManager->PublishContacts();
00211
00212 std::string topic = contactManager->CreateFilter(model->GetScopedName(), collisionNames);
00213 if (!this->contactSub) {
00214 std::cout<<"Subscribing contact manager to topic "<<topic<<std::endl;
00215 bool latching=false;
00216 this->contactSub = this->node->Subscribe(topic,&GazeboGraspFix::OnContact, this, latching);
00217 }
00218
00219 update_connection=event::Events::ConnectWorldUpdateEnd(boost::bind(&GazeboGraspFix::OnUpdate, this));
00220 }
00221
00222
00223 class GazeboGraspFix::ObjectContactInfo
00224 {
00225 public:
00226
00227
00228 std::vector<gazebo::math::Vector3> appliedForces;
00229
00230
00231
00232
00233 std::map<std::string, int> grippersInvolved;
00234
00235
00236
00237 int maxGripperContactCnt;
00238
00239
00240 std::string maxContactGripper;
00241 };
00242
00243
00244
00245 bool GazeboGraspFix::isGripperLink(const std::string& linkName, std::string& gripperName) const
00246 {
00247 for (std::map<std::string, GazeboGraspGripper>::const_iterator it=grippers.begin(); it!=grippers.end(); ++it)
00248 {
00249 if (it->second.hasLink(linkName))
00250 {
00251 gripperName=it->first;
00252 return true;
00253 }
00254 }
00255 return false;
00256 }
00257
00258 std::map<std::string, std::string> GazeboGraspFix::getAttachedObjects() const
00259 {
00260 std::map<std::string, std::string> ret;
00261 for (std::map<std::string, GazeboGraspGripper>::const_iterator it=grippers.begin(); it!=grippers.end(); ++it)
00262 {
00263 const std::string& gripperName = it->first;
00264 const GazeboGraspGripper& gripper = it->second;
00265 if (gripper.isObjectAttached())
00266 {
00267 ret[gripper.attachedObject()]=gripperName;
00268 }
00269 }
00270 return ret;
00271 }
00272
00273
00274
00275
00276
00277 bool GazeboGraspFix::objectAttachedToGripper(const ObjectContactInfo& objContInfo, std::string& attachedToGripper) const
00278 {
00279 for (std::map<std::string, int>::const_iterator gripInvIt=objContInfo.grippersInvolved.begin();
00280 gripInvIt != objContInfo.grippersInvolved.end(); ++gripInvIt)
00281 {
00282 const std::string& gripperName=gripInvIt->first;
00283 if (objectAttachedToGripper(gripperName, attachedToGripper))
00284 {
00285 return true;
00286 }
00287 }
00288 return false;
00289 }
00290
00291 bool GazeboGraspFix::objectAttachedToGripper(const std::string& gripperName, std::string& attachedToGripper) const
00292 {
00293 std::map<std::string, GazeboGraspGripper>::const_iterator gIt=grippers.find(gripperName);
00294 if (gIt == grippers.end())
00295 {
00296 gzerr << "GazeboGraspFix: Inconsistency, gripper "<<gripperName<<" not found in GazeboGraspFix grippers"<<std::endl;
00297 return false;
00298 }
00299 const GazeboGraspGripper& gripper = gIt->second;
00300
00301 if (gripper.isObjectAttached())
00302 {
00303 attachedToGripper = gripperName;
00304 return true;
00305 }
00306 return false;
00307 }
00308
00309
00323 class GazeboGraspFix::CollidingPoint{
00324 public:
00325 CollidingPoint(): sum(0) {}
00326 CollidingPoint(const CollidingPoint& o):
00327 gripperName(o.gripperName),
00328 collLink(o.collLink),
00329 collObj(o.collObj),
00330 force(o.force),
00331 pos(o.pos),
00332 objPos(o.objPos),
00333 sum(o.sum){}
00334
00335
00336
00337
00338 std::string gripperName;
00339
00340
00341 physics::CollisionPtr collLink, collObj;
00342
00343
00344 gazebo::math::Vector3 force;
00345
00346
00347
00348 gazebo::math::Vector3 pos;
00349
00350
00351
00352 gazebo::math::Vector3 objPos;
00353
00354
00355
00356
00357 int sum;
00358 };
00359
00360
00361
00362 void GazeboGraspFix::OnUpdate() {
00363 if ((common::Time::GetWallTime() - this->prevUpdateTime) < this->updateRate)
00364 return;
00365
00366
00367
00368 this->mutexContacts.lock();
00369 std::map<std::string, std::map<std::string, CollidingPoint> > contPoints(this->contacts);
00370 this->contacts.clear();
00371 this->mutexContacts.unlock();
00372
00373
00374
00375
00376
00377
00378 std::map<std::string, std::map<std::string, CollidingPoint> >::iterator objIt;
00379 std::map<std::string, ObjectContactInfo> objectContactInfo;
00380
00381 for (objIt=contPoints.begin(); objIt!=contPoints.end(); ++objIt)
00382 {
00383 std::string objName=objIt->first;
00384
00385
00386
00387 ObjectContactInfo& objContInfo = objectContactInfo[objName];
00388
00389
00390 std::map<std::string, CollidingPoint>::iterator lIt;
00391 for (lIt=objIt->second.begin(); lIt!=objIt->second.end(); ++lIt){
00392 std::string linkName=lIt->first;
00393 CollidingPoint& collP=lIt->second;
00394 gazebo::math::Vector3 avgForce=collP.force/collP.sum;
00395
00396 objContInfo.appliedForces.push_back(avgForce);
00397
00398 int& gContactCnt = objContInfo.grippersInvolved[collP.gripperName];
00399 gContactCnt++;
00400 int& _maxGripperContactCnt=objContInfo.maxGripperContactCnt;
00401 if (gContactCnt > _maxGripperContactCnt)
00402 {
00403 _maxGripperContactCnt = gContactCnt;
00404 objContInfo.maxContactGripper=collP.gripperName;
00405 }
00406 }
00407 }
00408
00409
00410
00411
00412
00413
00414
00415 std::set<std::string> grippedObjects;
00416 for (std::map<std::string, ObjectContactInfo>::iterator ocIt=objectContactInfo.begin();
00417 ocIt!=objectContactInfo.end(); ++ocIt)
00418 {
00419 const std::string& objName=ocIt->first;
00420 const ObjectContactInfo& objContInfo = ocIt->second;
00421
00422
00423
00424 float minAngleDiff= this->forcesAngleTolerance;
00425 if (!checkGrip(objContInfo.appliedForces, minAngleDiff, 0.3))
00426 continue;
00427
00428
00429 grippedObjects.insert(objName);
00430
00431
00432
00433 int& counts = this->gripCounts[objName];
00434 if (counts < this->maxGripCount) ++counts;
00435
00436
00437 if (counts <= this->gripCountThreshold)
00438 continue;
00439
00440
00441
00442
00443
00444 std::string attachedToGripper;
00445 bool isAttachedToGripper=objectAttachedToGripper(objContInfo, attachedToGripper);
00446 if (isAttachedToGripper)
00447 {
00448
00449
00450 continue;
00451 }
00452
00453
00454 const std::string& graspingGripperName = objContInfo.maxContactGripper;
00455 std::map<std::string, GazeboGraspGripper>::iterator gIt=grippers.find(graspingGripperName);
00456 if (gIt == grippers.end())
00457 {
00458 gzerr << "GazeboGraspFix: Inconsistency, gripper '"<<graspingGripperName
00459 << "' not found in GazeboGraspFix grippers, so cannot do attachment of object " << objName << std::endl;
00460 continue;
00461 }
00462 GazeboGraspGripper& graspingGripper = gIt->second;
00463
00464 if (graspingGripper.isObjectAttached())
00465 {
00466 gzerr<<"GazeboGraspFix has found that object "<<
00467 graspingGripper.attachedObject()<<" is already attached to gripper "<<
00468 graspingGripperName<<", so can't grasp '"<<objName<<"'!"<<std::endl;
00469 continue;
00470 }
00471
00472 std::cout<<"GazeboGraspFix: Attaching "<<objName<<" to gripper "<<graspingGripperName<<"!!!!!!!"<<std::endl;
00473
00474
00475
00476
00477
00478
00479
00480 const std::map<std::string, CollidingPoint>& contPointsTmp = contPoints[objName];
00481 std::map<std::string, CollidingPoint>& attGripConts = this->attachGripContacts[objName];
00482 attGripConts.clear();
00483 std::map<std::string, CollidingPoint>::const_iterator contPointsIt;
00484 for (contPointsIt=contPointsTmp.begin(); contPointsIt!=contPointsTmp.end(); ++contPointsIt)
00485 {
00486 const std::string& collidingLink = contPointsIt->first;
00487 const CollidingPoint& collidingPoint = contPointsIt->second;
00488
00489 if (graspingGripper.hasCollisionLink(collidingLink))
00490 {
00491
00492 attGripConts[collidingLink] = collidingPoint;
00493 }
00494 }
00495
00496 if (!graspingGripper.HandleAttach(objName)){
00497 gzerr<<"GazeboGraspFix: Could not attach object "<<objName<<" to gripper "<<graspingGripperName<<std::endl;
00498 }
00499 this->OnAttach(objName, graspingGripperName);
00500 }
00501
00502
00503
00504
00505 std::map<std::string, std::string> attachedObjects = getAttachedObjects();
00506
00507
00508
00509 std::map<std::string, int>::iterator gripCntIt;
00510 for (gripCntIt = this->gripCounts.begin(); gripCntIt != this->gripCounts.end(); ++gripCntIt){
00511
00512 const std::string& objName=gripCntIt->first;
00513
00514 if (grippedObjects.find(objName) != grippedObjects.end())
00515 {
00516
00517 continue;
00518 }
00519
00520
00521
00522
00523
00524 if (gripCntIt->second > 0) --(gripCntIt->second);
00525
00526 std::map<std::string, std::string>::iterator attIt = attachedObjects.find(objName);
00527 bool isAttached = (attIt != attachedObjects.end());
00528
00529
00530
00531 if (!isAttached || (gripCntIt->second > this->gripCountThreshold)) continue;
00532
00533 const std::string& graspingGripperName=attIt->second;
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551 std::map<std::string, std::map<std::string, CollidingPoint> >::iterator initCollIt=this->attachGripContacts.find(objName);
00552 if (initCollIt == this->attachGripContacts.end()) {
00553 std::cerr<<"ERROR: Consistency: Could not find attachGripContacts for "<<objName<<std::endl;
00554 continue;
00555 }
00556
00557 std::map<std::string, CollidingPoint>& initColls=initCollIt->second;
00558 int cntRelease=0;
00559
00560
00561 std::map<std::string, CollidingPoint>::iterator pointIt;
00562 for (pointIt=initColls.begin(); pointIt!=initColls.end(); ++pointIt)
00563 {
00564 CollidingPoint& cpInfo=pointIt->second;
00565
00566 gazebo::math::Vector3 relContactPos=cpInfo.pos/cpInfo.sum;
00567
00568 gazebo::math::Vector3 relObjPos=cpInfo.objPos/cpInfo.sum;
00569
00570
00571 gazebo::math::Pose currObjWorldPose=cpInfo.collObj->GetLink()->GetWorldPose();
00572
00573
00574 gazebo::math::Pose currLinkWorldPose=cpInfo.collLink->GetLink()->GetWorldPose();
00575
00576
00577 gazebo::math::Matrix4 worldToLink=currLinkWorldPose.rot.GetAsMatrix4();
00578 worldToLink.SetTranslate(currLinkWorldPose.pos);
00579
00580
00581 gazebo::math::Matrix4 linkToContact=gazebo::math::Matrix4::IDENTITY;
00582 linkToContact.SetTranslate(relContactPos);
00583
00584
00585 gazebo::math::Matrix4 _currContactWorldPose=worldToLink*linkToContact;
00586 gazebo::math::Vector3 currContactWorldPose=_currContactWorldPose.GetTranslation();
00587
00588
00589
00590
00591
00592
00593 gazebo::math::Vector3 oldObjDist= relContactPos - relObjPos;
00594
00595
00596 gazebo::math::Vector3 newObjDist= currContactWorldPose - currObjWorldPose.pos;
00597
00598
00599
00600
00601
00602 float diff=fabs(oldObjDist.GetLength() - newObjDist.GetLength());
00603
00604
00605 if (diff > releaseTolerance) {
00606 ++cntRelease;
00607 }
00608 }
00609
00610 if (cntRelease > 0)
00611 {
00612
00613 std::map<std::string, GazeboGraspGripper>::iterator gggIt = grippers.find(graspingGripperName);
00614 if (gggIt == grippers.end())
00615 {
00616 gzerr << "GazeboGraspFix: Inconsistency: Gazebo gripper '"<<graspingGripperName<<"' not found when checking for detachment" << std::endl;
00617 continue;
00618 }
00619 GazeboGraspGripper& graspingGripper = gggIt->second;
00620
00621 std::cout<<"GazeboGraspFix: Detaching "<<objName<<" from gripper "<<graspingGripperName<<"!!!!!!!"<<std::endl;
00622 graspingGripper.HandleDetach(objName);
00623 this->OnDetach(objName,graspingGripperName);
00624 gripCntIt->second=0;
00625 this->attachGripContacts.erase(initCollIt);
00626 }
00627 }
00628
00629 this->prevUpdateTime = common::Time::GetWallTime();
00630 }
00631
00632 double angularDistance(const gazebo::math::Vector3& _v1, const gazebo::math::Vector3& _v2) {
00633 gazebo::math::Vector3 v1=_v1;
00634 gazebo::math::Vector3 v2=_v2;
00635 v1.Normalize();
00636 v2.Normalize();
00637 return acos(v1.Dot(v2));
00638 }
00639
00640 bool GazeboGraspFix::checkGrip(const std::vector<gazebo::math::Vector3>& forces, float minAngleDiff, float lengthRatio){
00641 if (((lengthRatio > 1) || (lengthRatio < 0)) && (lengthRatio > 1e-04 && (fabs(lengthRatio-1) > 1e-04))) {
00642 std::cerr<<"ERROR: checkGrip: always specify a lengthRatio of [0..1]"<<std::endl;
00643 return false;
00644 }
00645 if (minAngleDiff < M_PI_2){
00646 std::cerr<<"ERROR: checkGrip: min angle must be at least 90 degrees (PI/2)"<<std::endl;
00647 return false;
00648 }
00649 std::vector<gazebo::math::Vector3>::const_iterator it1, it2;
00650 for (it1=forces.begin(); it1!=forces.end(); ++it1){
00651 gazebo::math::Vector3 v1=*it1;
00652 for (it2=it1+1; it2!=forces.end(); ++it2){
00653 gazebo::math::Vector3 v2=*it2;
00654 float l1=v1.GetLength();
00655 float l2=v2.GetLength();
00656 if ((l1<1e-04) || (l2<1e-04)) continue;
00657
00658
00659
00660
00661
00662 float angle=angularDistance(v1, v2);
00663
00664 if (angle > minAngleDiff) {
00665 float ratio;
00666 if (l1>l2) ratio=l2/l1;
00667 else ratio=l1/l2;
00668
00669 if (ratio >= lengthRatio)
00670 {
00671
00672 return true;
00673 }
00674 }
00675 }
00676 }
00677 return false;
00678 }
00679
00680 void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg)
00681 {
00682
00683
00684 for (int i = 0; i < _msg->contact_size(); ++i) {
00685 physics::CollisionPtr collision1 = boost::dynamic_pointer_cast<physics::Collision>(
00686 this->world->GetEntity(_msg->contact(i).collision1()));
00687 physics::CollisionPtr collision2 = boost::dynamic_pointer_cast<physics::Collision>(
00688 this->world->GetEntity(_msg->contact(i).collision2()));
00689
00690 if ((collision1 && !collision1->IsStatic()) && (collision2 && !collision2->IsStatic()))
00691 {
00692 std::string name1 = collision1->GetScopedName();
00693 std::string name2 = collision2->GetScopedName();
00694
00695
00696 int count = _msg->contact(i).position_size();
00697
00698
00699 if ((count != _msg->contact(i).normal_size()) ||
00700 (count != _msg->contact(i).wrench_size()) ||
00701 (count != _msg->contact(i).depth_size()))
00702 {
00703 gzerr << "GazeboGraspFix: Contact message has invalid array sizes\n"<<std::endl;
00704 continue;
00705 }
00706
00707 std::string collidingObjName, collidingLink, gripperOfCollidingLink;
00708 physics::CollisionPtr linkCollision;
00709 physics::CollisionPtr objCollision;
00710
00711 physics::Contact contact;
00712 contact = _msg->contact(i);
00713
00714 if (contact.count<1)
00715 {
00716 std::cerr<<"ERROR: GazeboGraspFix: Not enough forces given for contact of ."<<name1<<" / "<<name2<<std::endl;
00717 continue;
00718 }
00719
00720
00721 std::vector<gazebo::math::Vector3> force;
00722
00723
00724
00725 std::map<std::string,std::string>::const_iterator gripperCollIt = this->collisions.find(name2);
00726 if (gripperCollIt != this->collisions.end())
00727 {
00728 collidingObjName=name1;
00729 collidingLink=name2;
00730 linkCollision=collision2;
00731 objCollision=collision1;
00732 gripperOfCollidingLink=gripperCollIt->second;
00733 for (int k=0; k<contact.count; ++k)
00734 force.push_back(contact.wrench[k].body1Force);
00735 }
00736 else if ((gripperCollIt=this->collisions.find(name1)) != this->collisions.end())
00737 {
00738 collidingObjName=name2;
00739 collidingLink=name1;
00740 linkCollision=collision1;
00741 objCollision=collision2;
00742 gripperOfCollidingLink=gripperCollIt->second;
00743 for (int k=0; k<contact.count; ++k)
00744 force.push_back(contact.wrench[k].body2Force);
00745 }
00746
00747 gazebo::math::Vector3 avgForce;
00748
00749 for (int k=0; k<force.size(); ++k){
00750 avgForce+=force[k];
00751 }
00752 avgForce/=force.size();
00753
00754 gazebo::math::Vector3 avgPos;
00755
00756 for (int k=0; k<contact.count; ++k) avgPos+=contact.positions[k];
00757 avgPos/=contact.count;
00758
00759
00760 gazebo::math::Pose linkWorldPose=linkCollision->GetLink()->GetWorldPose();
00761
00762
00763 gazebo::math::Matrix4 worldToLink=linkWorldPose.rot.GetAsMatrix4();
00764 worldToLink.SetTranslate(linkWorldPose.pos);
00765
00766 gazebo::math::Matrix4 worldToContact=gazebo::math::Matrix4::IDENTITY;
00767
00768
00769 worldToContact.SetTranslate(avgPos);
00770
00771
00772
00773 gazebo::math::Matrix4 worldToLinkInv = worldToLink.Inverse();
00774 gazebo::math::Matrix4 contactInLocal = worldToLinkInv * worldToContact;
00775 gazebo::math::Vector3 contactPosInLocal = contactInLocal.GetTranslation();
00776
00777
00778
00779
00780
00781
00782
00783
00784
00785
00786
00787
00788
00789
00790
00791
00792
00793
00794
00795 gazebo::math::Pose objWorldPose = objCollision->GetLink()->GetWorldPose();
00796 gazebo::math::Matrix4 worldToObj = objWorldPose.rot.GetAsMatrix4();
00797 worldToObj.SetTranslate(objWorldPose.pos);
00798
00799 gazebo::math::Matrix4 objInLocal = worldToLinkInv * worldToObj;
00800 gazebo::math::Vector3 objPosInLocal = objInLocal.GetTranslation();
00801
00802 {
00803 boost::mutex::scoped_lock lock(this->mutexContacts);
00804 CollidingPoint& p = this->contacts[collidingObjName][collidingLink];
00805 p.gripperName=gripperOfCollidingLink;
00806 p.collLink=linkCollision;
00807 p.collObj=objCollision;
00808 p.force+=avgForce;
00809 p.pos+=contactPosInLocal;
00810 p.objPos+=objPosInLocal;
00811 p.sum++;
00812 }
00813
00814 }
00815 }
00816 }
00817
00818