GazeboGraspFix.cpp 35.7 KB
Newer Older
Jennifer Buehler's avatar
Jennifer Buehler committed
1
2
3
4
5
6
7
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/physics/ContactManager.hh>
#include <gazebo/physics/Contact.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>
8

Jennifer Buehler's avatar
Jennifer Buehler committed
9
#include <gazebo_grasp_plugin/GazeboGraspFix.h>
Jennifer Buehler's avatar
Jennifer Buehler committed
10
#include <gazebo_version_helpers/GazeboVersionHelpers.h>
Jennifer Buehler's avatar
Jennifer Buehler committed
11
12

using gazebo::GazeboGraspFix;
Jennifer Buehler's avatar
Jennifer Buehler committed
13
using gazebo::GzVector3;
14
15
16

#define DEFAULT_FORCES_ANGLE_TOLERANCE 120
#define DEFAULT_UPDATE_RATE 5
17
#define DEFAULT_MAX_GRIP_COUNT 10
18
19
20
#define DEFAULT_RELEASE_TOLERANCE 0.005
#define DEFAULT_DISABLE_COLLISIONS_ON_ATTACH false

Jennifer Buehler's avatar
Jennifer Buehler committed
21
22
23
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(GazeboGraspFix)

24
25
26
27
////////////////////////////////////////////////////////////////////////////////
GazeboGraspFix::GazeboGraspFix()
{
  InitValues();
Jennifer Buehler's avatar
Jennifer Buehler committed
28
29
}

30
////////////////////////////////////////////////////////////////////////////////
31
32
GazeboGraspFix::GazeboGraspFix(physics::ModelPtr _model)
{
33
  InitValues();
Jennifer Buehler's avatar
Jennifer Buehler committed
34
35
}

36
////////////////////////////////////////////////////////////////////////////////
37
38
GazeboGraspFix::~GazeboGraspFix()
{
39
40
41
  this->update_connection.reset();
  if (this->node) this->node->Fini();
  this->node.reset();
Jennifer Buehler's avatar
Jennifer Buehler committed
42
43
}

44
////////////////////////////////////////////////////////////////////////////////
45
46
void GazeboGraspFix::Init()
{
47
  this->prevUpdateTime = common::Time::GetWallTime();
Jennifer Buehler's avatar
Jennifer Buehler committed
48
49
}

50
////////////////////////////////////////////////////////////////////////////////
51
52
void GazeboGraspFix::InitValues()
{
53
#if GAZEBO_MAJOR_VERSION > 2
54
  gazebo::common::Console::SetQuiet(false);
55
56
#endif

57
58
59
60
61
62
63
64
  // float timeDiff=0.25;
  // this->releaseTolerance=0.005;
  // this->updateRate = common::Time(0, common::Time::SecToNano(timeDiff));
  this->prevUpdateTime = common::Time::GetWallTime();
  //float graspedSecs=2;
  //this->maxGripCount=floor(graspedSecs/timeDiff);
  //this->gripCountThreshold=floor(this->maxGripCount/2);
  this->node = transport::NodePtr(new transport::Node());
Jennifer Buehler's avatar
Jennifer Buehler committed
65
66
}

67
////////////////////////////////////////////////////////////////////////////////
68
69
void GazeboGraspFix::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
  gzmsg << "Loading grasp-fix plugin" << std::endl;

  // ++++++++++++ Read parameters and initialize fields  +++++++++++++++

  physics::ModelPtr model = _parent;
  this->world = model->GetWorld();

  sdf::ElementPtr disableCollisionsOnAttachElem =
    _sdf->GetElement("disable_collisions_on_attach");
  if (!disableCollisionsOnAttachElem)
  {
    gzmsg << "GazeboGraspFix: Using default " <<
          DEFAULT_DISABLE_COLLISIONS_ON_ATTACH <<
          " because no <disable_collisions_on_attach> element specified." <<
          std::endl;
    this->disableCollisionsOnAttach = DEFAULT_DISABLE_COLLISIONS_ON_ATTACH;
  }
  else
  {
    std::string str = disableCollisionsOnAttachElem->Get<std::string>();
    bool bVal = false;
    if ((str == "true") || (str == "1"))  bVal = true;
    this->disableCollisionsOnAttach = bVal;
    gzmsg << "GazeboGraspFix: Using disable_collisions_on_attach " <<
          this->disableCollisionsOnAttach << std::endl;
  }

  sdf::ElementPtr forcesAngleToleranceElem =
    _sdf->GetElement("forces_angle_tolerance");
  if (!forcesAngleToleranceElem)
  {
    gzmsg << "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)
  {
    gzmsg << "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;
    gzmsg << "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)
  {
    gzmsg << "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>();
    gzmsg << "GazeboGraspFix: Using max_grip_count "
          << this->maxGripCount << std::endl;
  }

  sdf::ElementPtr gripCountThresholdElem =
    _sdf->GetElement("grip_count_threshold");
  if (!gripCountThresholdElem)
  {
    this->gripCountThreshold = floor(this->maxGripCount / 2.0);
    gzmsg << "GazeboGraspFix: Using  " << this->gripCountThreshold <<
          " because no <grip_count_threshold> element specified." <<
          std::endl;
  }
  else
  {
    this->gripCountThreshold = gripCountThresholdElem->Get<int>();
    gzmsg << "GazeboGraspFix: Using grip_count_threshold " <<
          this->gripCountThreshold << std::endl;
  }

  sdf::ElementPtr releaseToleranceElem = _sdf->GetElement("release_tolerance");
  if (!releaseToleranceElem)
  {
    gzmsg << "GazeboGraspFix: Using  " << DEFAULT_RELEASE_TOLERANCE <<
          " because no <release_tolerance> element specified." << std::endl;
    this->releaseTolerance = DEFAULT_RELEASE_TOLERANCE;
  }
  else
  {
    this->releaseTolerance = releaseToleranceElem->Get<float>();
    gzmsg << "GazeboGraspFix: Using release_tolerance " <<
          this->releaseTolerance << std::endl;
  }

  // will contain all names of collision entities involved from all arms
  std::vector<std::string> collisionNames;

  sdf::ElementPtr armElem = _sdf->GetElement("arm");
  if (!armElem)
  {
    gzerr << "GazeboGraspFix: Cannot load the GazeboGraspFix without any <arm> declarations"
          << std::endl;
    return;
  }
  // add all arms:
  for (; armElem != NULL; armElem = armElem->GetNextElement("arm"))
  {
    sdf::ElementPtr armNameElem = armElem->GetElement("arm_name");
    sdf::ElementPtr handLinkElem = armElem->GetElement("palm_link");
    sdf::ElementPtr fingerLinkElem = armElem->GetElement("gripper_link");

    if (!handLinkElem || !fingerLinkElem || !armNameElem)
    {
      gzerr << "ERROR: GazeboGraspFix: Cannot use a GazeboGraspFix arm because "
            << "not all of <arm_name>, <palm_link> and <gripper_link> elements specified in URDF/SDF. Skipping."
            << std::endl;
      continue;
197
198
    }

199
200
    std::string armName = armNameElem->Get<std::string>();
    std::string palmName = handLinkElem->Get<std::string>();
201

202
203
204
205
206
207
208
    // collect all finger names:
    std::vector<std::string> fingerLinkNames;
    for (; fingerLinkElem != NULL;
         fingerLinkElem = fingerLinkElem->GetNextElement("gripper_link"))
    {
      std::string linkName = fingerLinkElem->Get<std::string>();
      fingerLinkNames.push_back(linkName);
209
210
    }

211
212
    // add new gripper
    if (grippers.find(armName) != grippers.end())
213
    {
214
215
      gzerr << "GazeboGraspFix: Arm named " << armName <<
            " was already added, cannot add it twice." << std::endl;
216
    }
217
218
219
220
    GazeboGraspGripper &gripper = grippers[armName];
    std::map<std::string, physics::CollisionPtr> _collisions;
    if (!gripper.Init(_parent, armName, palmName, fingerLinkNames,
                      disableCollisionsOnAttach, _collisions))
221
    {
222
223
224
225
      gzerr << "GazeboGraspFix: Could not initialize arm " << armName << ". Skipping."
            << std::endl;
      grippers.erase(armName);
      continue;
Jennifer Buehler's avatar
Jennifer Buehler committed
226
    }
227
228
229
230
    // add all the grippers's collision elements
    for (std::map<std::string, physics::CollisionPtr>::iterator collIt =
           _collisions.begin();
         collIt != _collisions.end(); ++collIt)
231
    {
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
      const std::string &collName = collIt->first;
      //physics::CollisionPtr& coll=collIt->second;
      std::map<std::string, std::string>::iterator collIter = this->collisions.find(
            collName);
      if (collIter !=
          this->collisions.end())   //this collision was already added before
      {
        gzwarn << "GazeboGraspFix: Adding Gazebo collision link element " << collName <<
               " multiple times, the grasp plugin may not work properly" << std::endl;
        continue;
      }
      gzmsg << "GazeboGraspFix: Adding collision scoped name " << collName <<
            std::endl;
      this->collisions[collName] = armName;
      collisionNames.push_back(collName);
247
    }
248
249
250
251
252
253
254
255
256
257
258
  }

  if (grippers.empty())
  {
    gzerr << "ERROR: GazeboGraspFix: Cannot use a GazeboGraspFix because "
          << "no arms were configured successfully. Plugin will not work." << std::endl;
    return;
  }

  // ++++++++++++ start up things +++++++++++++++

Jennifer Buehler's avatar
Jennifer Buehler committed
259
260
  physics::PhysicsEnginePtr physics = GetPhysics(this->world);
  this->node->Init(gazebo::GetName(*(this->world)));
261
  physics::ContactManager *contactManager = physics->GetContactManager();
Jennifer Buehler's avatar
Jennifer Buehler committed
262
  contactManager->PublishContacts();  // TODO not sure this is required?
263
264
265
266
267
268
269
270
271
272
273
274
275

  std::string topic = contactManager->CreateFilter(model->GetScopedName(),
                      collisionNames);
  if (!this->contactSub)
  {
    gzmsg << "Subscribing contact manager to topic " << topic << std::endl;
    bool latching = false;
    this->contactSub = this->node->Subscribe(topic, &GazeboGraspFix::OnContact,
                       this, latching);
  }

  update_connection = event::Events::ConnectWorldUpdateEnd(boost::bind(
                        &GazeboGraspFix::OnUpdate, this));
Jennifer Buehler's avatar
Jennifer Buehler committed
276
277
}

278
////////////////////////////////////////////////////////////////////////////////
279
280
class GazeboGraspFix::ObjectContactInfo
{
281
282
  public:

283
    // all forces effecting on the object
Jennifer Buehler's avatar
Jennifer Buehler committed
284
    std::vector<GzVector3> appliedForces;
285
286
287
288
289

    // all grippers involved in the process, along with
    // a number counting the number of contact points with the
    // object per gripper
    std::map<std::string, int> grippersInvolved;
290

291
292
293
    // maximum number of contacts of *any one* gripper
    // (any in grippersInvolved)
    int maxGripperContactCnt;
294

295
296
297
298
    // the gripper for maxGripperContactCnt
    std::string maxContactGripper;
};

299
300
301
////////////////////////////////////////////////////////////////////////////////
bool GazeboGraspFix::IsGripperLink(const std::string &linkName,
                                   std::string &gripperName) const
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
302
{
303
304
305
306
  for (std::map<std::string, GazeboGraspGripper>::const_iterator it =
         grippers.begin(); it != grippers.end(); ++it)
  {
    if (it->second.hasLink(linkName))
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
307
    {
308
309
      gripperName = it->first;
      return true;
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
310
    }
311
312
313
  }
  return false;
}
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
314

315
316
////////////////////////////////////////////////////////////////////////////////
std::map<std::string, std::string> GazeboGraspFix::GetAttachedObjects() const
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
317
{
318
319
320
321
322
323
324
  std::map<std::string, std::string> ret;
  for (std::map<std::string, GazeboGraspGripper>::const_iterator it =
         grippers.begin(); it != grippers.end(); ++it)
  {
    const std::string &gripperName = it->first;
    const GazeboGraspGripper &gripper = it->second;
    if (gripper.isObjectAttached())
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
325
    {
326
      ret[gripper.attachedObject()] = gripperName;
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
327
    }
328
329
330
  }
  return ret;
}
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
331

332
333
334
////////////////////////////////////////////////////////////////////////////////
bool GazeboGraspFix::ObjectAttachedToGripper(const ObjectContactInfo
    &objContInfo, std::string &attachedToGripper) const
335
{
336
337
338
339
340
341
  for (std::map<std::string, int>::const_iterator gripInvIt =
         objContInfo.grippersInvolved.begin();
       gripInvIt != objContInfo.grippersInvolved.end(); ++gripInvIt)
  {
    const std::string &gripperName = gripInvIt->first;
    if (ObjectAttachedToGripper(gripperName, attachedToGripper))
342
    {
343
      return true;
344
    }
345
346
  }
  return false;
347
348
}

349
350
351
////////////////////////////////////////////////////////////////////////////////
bool GazeboGraspFix::ObjectAttachedToGripper(const std::string &gripperName,
    std::string &attachedToGripper) const
352
{
353
354
355
356
357
358
  std::map<std::string, GazeboGraspGripper>::const_iterator gIt = grippers.find(
        gripperName);
  if (gIt == grippers.end())
  {
    gzerr << "GazeboGraspFix: Inconsistency, gripper " << gripperName <<
          " not found in GazeboGraspFix grippers" << std::endl;
359
    return false;
360
361
362
363
364
365
366
367
368
  }
  const GazeboGraspGripper &gripper = gIt->second;
  // gzmsg<<"Gripper "<<gripperName<<" is involved in the grasp"<<std::endl;
  if (gripper.isObjectAttached())
  {
    attachedToGripper = gripperName;
    return true;
  }
  return false;
369
370
371
}


372
////////////////////////////////////////////////////////////////////////////////
373
374
375
376
377
378
379
380
381
382
/**
 * 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.
383
 *
384
385
 * \author Jennifer Buehler
 */
386
387
388
class GazeboGraspFix::CollidingPoint
{
  public:
389
    CollidingPoint(): sum(0) {}
390
391
392
393
394
395
396
397
    CollidingPoint(const CollidingPoint &o):
      gripperName(o.gripperName),
      collLink(o.collLink),
      collObj(o.collObj),
      force(o.force),
      pos(o.pos),
      objPos(o.objPos),
      sum(o.sum) {}
398
399
400
401
402
403

    // Name of the gripper/arm involved in contact point
    // This is not the specific link, but the name of the
    // whole gripper
    std::string gripperName;

404
    // the collision
405
406
407
    physics::CollisionPtr collLink, collObj;

    // average force vector of the colliding point
Jennifer Buehler's avatar
Jennifer Buehler committed
408
    GzVector3 force;
409
410
411

    // position (relative to reference frame of gripper
    // collision surface) where the contact happens on collision surface
Jennifer Buehler's avatar
Jennifer Buehler committed
412
    GzVector3 pos;
413
414

    // position (relative to reference frame of *gripper* collision surface)
415
    // where the object center is located during collision.
Jennifer Buehler's avatar
Jennifer Buehler committed
416
    GzVector3 objPos;
417

418
419
420
421
422
423
    // 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;
};

Jennifer Buehler's avatar
Jennifer Buehler committed
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
////////////////////////////////////////////////////////////////////////////////
double AngularDistance(const GzVector3 &_v1,
                       const GzVector3 &_v2)
{
  GzVector3 v1 = _v1;
  GzVector3 v2 = _v2;
  v1.Normalize();
  v2.Normalize();
  return acos(v1.Dot(v2));
}

////////////////////////////////////////////////////////////////////////////////
// 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<GzVector3> &forces,
               float minAngleDiff, float lengthRatio)
{
  if (((lengthRatio > 1) || (lengthRatio < 0)) && (lengthRatio > 1e-04
      && (fabs(lengthRatio - 1) > 1e-04)))
  {
    std::cerr << "ERROR: CheckGrip: always specify a lengthRatio of [0..1]" <<
              std::endl;
    return false;
  }
  if (minAngleDiff < M_PI_2)
  {
    std::cerr << "ERROR: CheckGrip: min angle must be at least 90 degrees (PI/2)" <<
              std::endl;
    return false;
  }
  std::vector<GzVector3>::const_iterator it1, it2;
  for (it1 = forces.begin(); it1 != forces.end(); ++it1)
  {
    GzVector3 v1 = *it1;
    for (it2 = it1 + 1; it2 != forces.end(); ++it2)
    {
      GzVector3 v2 = *it2;
      float l1 = gazebo::GetLength(v1);
      float l2 = gazebo::GetLength(v2);
      if ((l1 < 1e-04) || (l2 < 1e-04)) continue;
      /*GzVector3 _v1=v1;
      GzVector3 _v2=v2;
      _v1/=l1;
      _v2/=l2;
      float angle=acos(_v1.Dot(_v2));*/
      float angle = AngularDistance(v1, v2);
      // gzmsg<<"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;
        // gzmsg<<"Got angle "<<angle<<", ratio "<<ratio<<std::endl;
        if (ratio >= lengthRatio)
        {
          // gzmsg<<"CheckGrip() is true"<<std::endl;
          return true;
        }
      }
    }
  }
  return false;
}

489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
////////////////////////////////////////////////////////////////////////////////
void GazeboGraspFix::OnUpdate()
{
  if ((common::Time::GetWallTime() - this->prevUpdateTime) < this->updateRate)
    return;

  // first, copy all contact data into local struct. Don't do the complex grip check (CheckGrip)
  // within the mutex, because that slows down OnContact().
  this->mutexContacts.lock();
  std::map<std::string, std::map<std::string, CollidingPoint> > contPoints(
    this->contacts);
  this->contacts.clear(); // clear so it can be filled anew by OnContact().
  this->mutexContacts.unlock();

  // contPoints now contains CollidingPoint objects for each *object* and *link*.

  // Iterate through all contact points to gather all summed forces
  // (and other useful information) for all the objects (so we have all forces on one object).
  std::map<std::string, std::map<std::string, CollidingPoint> >::iterator objIt;
  std::map<std::string, ObjectContactInfo> objectContactInfo;

  for (objIt = contPoints.begin(); objIt != contPoints.end(); ++objIt)
  {
    std::string objName = objIt->first;
    //gzmsg<<"Examining object collisions with "<<objName<<std::endl;

    // create new entry in accumulated results map and get reference to fill in:
    ObjectContactInfo &objContInfo = objectContactInfo[objName];

    // for all links colliding with this object...
    std::map<std::string, CollidingPoint>::iterator lIt;
    for (lIt = objIt->second.begin(); lIt != objIt->second.end(); ++lIt)
    {
      std::string linkName = lIt->first;
      CollidingPoint &collP = lIt->second;
Jennifer Buehler's avatar
Jennifer Buehler committed
524
      GzVector3 avgForce = collP.force / collP.sum;
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
      // gzmsg << "Found collision with "<<linkName<<": "<<avgForce.x<<", "<<avgForce.y<<", "<<avgForce.z<<" (avg over "<<collP.sum<<")"<<std::endl;
      objContInfo.appliedForces.push_back(avgForce);
      // insert the gripper (if it doesn't exist yet) and increase contact counter
      int &gContactCnt = objContInfo.grippersInvolved[collP.gripperName];
      gContactCnt++;
      int &_maxGripperContactCnt = objContInfo.maxGripperContactCnt;
      if (gContactCnt > _maxGripperContactCnt)
      {
        _maxGripperContactCnt = gContactCnt;
        objContInfo.maxContactGripper = collP.gripperName;
      }
    }
  }

  // ++++++++++++++++++++ Handle Attachment  +++++++++++++++++++++++++++++++

  // collect of all objects which are found to be "gripped" at the current stage.
  // if they are gripped, increase the grip counter. If the grip count exceeds the
  // threshold, attach the object to the gripper which has most contact points with the
  // object.
  std::set<std::string> grippedObjects;
  for (std::map<std::string, ObjectContactInfo>::iterator ocIt =
         objectContactInfo.begin();
       ocIt != objectContactInfo.end(); ++ocIt)
  {
    const std::string &objName = ocIt->first;
    const ObjectContactInfo &objContInfo = ocIt->second;

    // gzmsg<<"Number applied forces on "<<objName<<": "<<objContInfo.appliedForces.size()<<std::endl;
  
    // TODO: remove this test print, for issue #26 ------------------- 
#if 0
    physics::CollisionPtr objColl = boost::dynamic_pointer_cast<physics::Collision>
                              (world->GetEntity(objName));
    if (objColl && objColl->GetLink())
    {
      auto linVel = objColl->GetLink()->GetWorldLinearVel();
      gzmsg << "Velocity for link " << objColl->GetLink()->GetName() << " (collision name " << objName << "): " << linVel << ", absolute val " << linVel.GetLength() << std::endl;
    }
#endif
    // ------------------- 

    float minAngleDiff = this->forcesAngleTolerance; //120 * M_PI/180;
    if (!CheckGrip(objContInfo.appliedForces, minAngleDiff, 0.3))
      continue;
570

571
572
    // add to "gripped objects"
    grippedObjects.insert(objName);
573

574
    //gzmsg<<"Grasp Held: "<<objName<<" grip count: "<<this->gripCounts[objName]<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
575

576
577
    int &counts = this->gripCounts[objName];
    if (counts < this->maxGripCount) ++counts;
Jennifer Buehler's avatar
Jennifer Buehler committed
578

579
580
581
    // only need to attach object if the grip count threshold is exceeded
    if (counts <= this->gripCountThreshold)
      continue;
582

583
    //gzmsg<<"GRIPPING "<<objName<<", grip count "<<counts<<" (threshold "<<this->gripCountThreshold<<")"<<std::endl;
584

585
586
587
588
589
590
591
592
593
594
595
596
    // find out if any of the grippers involved in the grasp is already grasping the object.
    // If there is no such gripper, attach it to the gripper which has most contact points.
    std::string attachedToGripper;
    bool isAttachedToGripper = ObjectAttachedToGripper(objContInfo,
                               attachedToGripper);
    if (isAttachedToGripper)
    {
      // the object is already attached to a gripper, so it does not need to be attached.
      // gzmsg << "GazeboGraspFix has found that object "<<
      //     gripper.attachedObject()<<" is already attached to gripper "<<gripperName;
      continue;
    }
597

598
599
600
601
602
    // attach the object to the gripper with most contact counts
    const std::string &graspingGripperName = objContInfo.maxContactGripper;
    std::map<std::string, GazeboGraspGripper>::iterator gIt = grippers.find(
          graspingGripperName);
    if (gIt == grippers.end())
603
    {
604
605
606
607
      gzerr << "GazeboGraspFix: Inconsistency, gripper '" << graspingGripperName
            << "' not found in GazeboGraspFix grippers, so cannot do attachment of object "
            << objName << std::endl;
      continue;
Jennifer Buehler's avatar
Jennifer Buehler committed
608
    }
609
610
611
    GazeboGraspGripper &graspingGripper = gIt->second;

    if (graspingGripper.isObjectAttached())
612
    {
613
614
615
616
617
      gzerr << "GazeboGraspFix has found that object " <<
            graspingGripper.attachedObject() << " is already attached to gripper " <<
            graspingGripperName << ", so can't grasp '" << objName << "'!" << std::endl;
      continue;
    }
618

619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
    gzmsg << "GazeboGraspFix: Attaching " << objName << " to gripper " <<
          graspingGripperName << "." << std::endl;

    // Store the array of contact poses which played part in the grip, sorted by colliding link.
    // Filter out all link names of other grippers, otherwise if the other gripper moves
    // away, this is going to trigger the release condition.
    // XXX this does not consider full support for an object being gripped by two grippers (e.g.
    // one left, one right).
    // this->attachGripContacts[objName]=contPoints[objName];
    const std::map<std::string, CollidingPoint> &contPointsTmp =
      contPoints[objName];
    std::map<std::string, CollidingPoint> &attGripConts =
      this->attachGripContacts[objName];
    attGripConts.clear();
    std::map<std::string, CollidingPoint>::const_iterator contPointsIt;
    for (contPointsIt = contPointsTmp.begin(); contPointsIt != contPointsTmp.end();
         ++contPointsIt)
    {
      const std::string &collidingLink = contPointsIt->first;
      const CollidingPoint &collidingPoint = contPointsIt->second;
      // gzmsg<<"Checking initial contact with "<<collidingLink<<" and "<<graspingGripperName<<std::endl;
      if (graspingGripper.hasCollisionLink(collidingLink))
      {
        // gzmsg<<"Insert initial contact with "<<collidingLink<<std::endl;
        attGripConts[collidingLink] = collidingPoint;
      }
    }
646

647
648
649
650
651
652
653
    if (!graspingGripper.HandleAttach(objName))
    {
      gzerr << "GazeboGraspFix: Could not attach object " << objName << " to gripper "
            << graspingGripperName << std::endl;
    }
    this->OnAttach(objName, graspingGripperName);
  }  // for all objects
654
655


Jennifer Buehler's avatar
Jennifer Buehler committed
656

657
658
  // ++++++++++++++++++++ Handle Detachment  +++++++++++++++++++++++++++++++
  std::map<std::string, std::string> attachedObjects = GetAttachedObjects();
Jennifer Buehler's avatar
Jennifer Buehler committed
659

660
661
662
663
664
665
  // 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 = this->gripCounts.begin(); gripCntIt != this->gripCounts.end();
       ++gripCntIt)
  {
Jennifer Buehler's avatar
Jennifer Buehler committed
666

667
    const std::string &objName = gripCntIt->first;
Jennifer Buehler's avatar
Jennifer Buehler committed
668

669
670
671
672
673
674
    if (grippedObjects.find(objName) != grippedObjects.end())
    {
      // this object is one we just detected as "gripped", so no need to check for releasing it...
      // gzmsg<<"NOT considering "<<objName<<" for detachment."<<std::endl;
      continue;
    }
Jennifer Buehler's avatar
Jennifer Buehler committed
675

676
    // the object does not satisfy "gripped" criteria, so potentially has to be released.
Jennifer Buehler's avatar
Jennifer Buehler committed
677

678
    // gzmsg<<"NOT-GRIPPING "<<objName<<", grip count "<<gripCntIt->second<<" (threshold "<<this->gripCountThreshold<<")"<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
679

680
681
682
683
684
685
686
687
688
    if (gripCntIt->second > 0) --(gripCntIt->second);

    std::map<std::string, std::string>::iterator attIt = attachedObjects.find(
          objName);
    bool isAttached = (attIt != attachedObjects.end());

    // gzmsg<<"is attached: "<<isAttached<<std::endl;

    if (!isAttached || (gripCntIt->second > this->gripCountThreshold)) continue;
Jennifer Buehler's avatar
Jennifer Buehler committed
689

690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
    const std::string &graspingGripperName = attIt->second;

    // gzmsg<<"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;
Jennifer Buehler's avatar
Jennifer Buehler committed
715
    }
716
717
718
719
720
721
722
723
724
725

    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 = initColls.begin(); pointIt != initColls.end(); ++pointIt)
    {
      CollidingPoint &cpInfo = pointIt->second;
      // initial distance from link to contact point (relative to link)
Jennifer Buehler's avatar
Jennifer Buehler committed
726
727
728
      GzVector3 relContactPos = cpInfo.pos / cpInfo.sum;
      // Initial distance from link to object (relative to link)
      GzVector3 relObjPos = cpInfo.objPos / cpInfo.sum;
729

Jennifer Buehler's avatar
Jennifer Buehler committed
730
731
732
      // Get current world pose of object
      GzPose3 currObjWorldPose =
        gazebo::GetWorldPose(cpInfo.collObj->GetLink());
733

Jennifer Buehler's avatar
Jennifer Buehler committed
734
735
736
      // Get world pose of link
      GzPose3 currLinkWorldPose =
        gazebo::GetWorldPose(cpInfo.collLink->GetLink());
737
738

      // Get transform for currLinkWorldPose as matrix
Jennifer Buehler's avatar
Jennifer Buehler committed
739
      GzMatrix4 worldToLink = gazebo::GetMatrix(currLinkWorldPose);
740
741

      // Get the transform from collision link to contact point
Jennifer Buehler's avatar
Jennifer Buehler committed
742
      GzMatrix4 linkToContact = gazebo::GetMatrix(relContactPos);
743

Jennifer Buehler's avatar
Jennifer Buehler committed
744
745
746
      // The current world position of the contact point right now is:
      GzMatrix4 _currContactWorldPose = worldToLink * linkToContact;
      GzVector3 currContactWorldPose = gazebo::GetPos(_currContactWorldPose);
747

Jennifer Buehler's avatar
Jennifer Buehler committed
748
      // The initial contact point location on the link should still correspond
749
750
      // to the initial contact point location on the object.

Jennifer Buehler's avatar
Jennifer Buehler committed
751
      // Initial vector from object center to contact point (relative to link,
752
      // because relObjPos and relContactPos are from center of link)
Jennifer Buehler's avatar
Jennifer Buehler committed
753
754
      GzVector3 oldObjDist = relContactPos - relObjPos;
      // The same vector as \e oldObjDist, but calculated by the current world pose
755
      // of object and the current location of the initial contact location on the link.
Jennifer Buehler's avatar
Jennifer Buehler committed
756
757
      // This is the new distance from contact to object.
      GzVector3 newObjDist = currContactWorldPose - gazebo::GetPos(currObjWorldPose);
758
759
760
761
762

      //gzmsg<<"Obj Trans "<<cpInfo.collLink->GetName()<<": "<<relObjPos.x<<", "<<relObjPos.y<<", "<<relObjPos.z<<std::endl;
      //gzmsg<<"Cont Trans "<<cpInfo.collLink->GetName()<<": "<<relContactPos.x<<", "<<relContactPos.y<<", "<<relContactPos.z<<std::endl;

      // the difference between these vectors should not be too large...
Jennifer Buehler's avatar
Jennifer Buehler committed
763
764
      float diff =
        fabs(gazebo::GetLength(oldObjDist) - gazebo::GetLength(newObjDist));
765
766
767
768
769
770
      //gzmsg<<"Diff for link "<<cpInfo.collLink->GetName()<<": "<<diff<<std::endl;

      if (diff > releaseTolerance)
      {
        ++cntRelease;
      }
Jennifer Buehler's avatar
Jennifer Buehler committed
771
    }
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792

    if (cntRelease > 0)
    {
      // sufficient links did not meet the criteria to be close enough to the object.
      // First, get the grasping gripper:
      std::map<std::string, GazeboGraspGripper>::iterator gggIt = grippers.find(
            graspingGripperName);
      if (gggIt == grippers.end())
      {
        gzerr << "GazeboGraspFix: Inconsistency: Gazebo gripper '" <<
              graspingGripperName << "' not found when checking for detachment" << std::endl;
        continue;
      }
      GazeboGraspGripper &graspingGripper = gggIt->second;
      // Now, detach the object:
      gzmsg << "GazeboGraspFix: Detaching " << objName << " from gripper " <<
            graspingGripperName << "." << std::endl;
      graspingGripper.HandleDetach(objName);
      this->OnDetach(objName, graspingGripperName);
      gripCntIt->second = 0;
      this->attachGripContacts.erase(initCollIt);
Jennifer Buehler's avatar
Jennifer Buehler committed
793
    }
794
795
796
  }

  this->prevUpdateTime = common::Time::GetWallTime();
Jennifer Buehler's avatar
Jennifer Buehler committed
797
798
}

799
800
801
802
803
804
805
806
807
////////////////////////////////////////////////////////////////////////////////
void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg)
{
  //gzmsg<<"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>(
Jennifer Buehler's avatar
Jennifer Buehler committed
808
        gazebo::GetEntityByName(this->world, _msg->contact(i).collision1()));
809
810
    physics::CollisionPtr collision2 =
      boost::dynamic_pointer_cast<physics::Collision>(
Jennifer Buehler's avatar
Jennifer Buehler committed
811
        gazebo::GetEntityByName(this->world, _msg->contact(i).collision2()));
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846

    if ((collision1 && !collision1->IsStatic()) && (collision2
        && !collision2->IsStatic()))
    {
      std::string name1 = collision1->GetScopedName();
      std::string name2 = collision2->GetScopedName();

      //gzmsg<<"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()))
      {
        gzerr << "GazeboGraspFix: Contact message has invalid array sizes\n" <<
              std::endl;
        continue;
      }

      std::string collidingObjName, collidingLink, gripperOfCollidingLink;
      physics::CollisionPtr linkCollision;
      physics::CollisionPtr objCollision;

      physics::Contact contact;
      contact = _msg->contact(i);

      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
Jennifer Buehler's avatar
Jennifer Buehler committed
847
      std::vector<GzVector3> force;
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876

      // 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'.
      std::map<std::string, std::string>::const_iterator gripperCollIt =
        this->collisions.find(name2);
      if (gripperCollIt != this->collisions.end())
      {
        // collision 1 is the object
        collidingObjName = name1;
        collidingLink = name2;
        linkCollision = collision2;
        objCollision = collision1;
        gripperOfCollidingLink = gripperCollIt->second;
        for (int k = 0; k < contact.count; ++k)
          force.push_back(contact.wrench[k].body1Force);
      }
      else if ((gripperCollIt = this->collisions.find(name1)) !=
               this->collisions.end())
      {
        // collision 2 is the object
        collidingObjName = name2;
        collidingLink = name1;
        linkCollision = collision1;
        objCollision = collision2;
        gripperOfCollidingLink = gripperCollIt->second;
        for (int k = 0; k < contact.count; ++k)
          force.push_back(contact.wrench[k].body2Force);
      }

Jennifer Buehler's avatar
Jennifer Buehler committed
877
      GzVector3 avgForce;
878
879
880
881
882
883
884
      // compute average/sum of the forces applied on the object
      for (int k = 0; k < force.size(); ++k)
      {
        avgForce += force[k];
      }
      avgForce /= force.size();

Jennifer Buehler's avatar
Jennifer Buehler committed
885
      GzVector3 avgPos;
886
887
888
889
890
      // 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
Jennifer Buehler's avatar
Jennifer Buehler committed
891
      GzPose3 linkWorldPose = gazebo::GetWorldPose(linkCollision->GetLink());
892
893

      // To find out the collision point relative to the Link's local coordinate system, first get the Poses as 4x4 matrices
Jennifer Buehler's avatar
Jennifer Buehler committed
894
      GzMatrix4 worldToLink = gazebo::GetMatrix(linkWorldPose);
895

Jennifer Buehler's avatar
Jennifer Buehler committed
896
897
898
      // 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.
      GzMatrix4 worldToContact = gazebo::GetMatrix(avgPos);
899
900
901

      // now, worldToLink * contactInLocal = worldToContact
      // hence, contactInLocal = worldToLink.Inv * worldToContact
Jennifer Buehler's avatar
Jennifer Buehler committed
902
903
904
      GzMatrix4 worldToLinkInv = worldToLink.Inverse();
      GzMatrix4 contactInLocal = worldToLinkInv * worldToContact;
      GzVector3 contactPosInLocal = gazebo::GetPos(contactInLocal);
905
906
907
908

      //gzmsg<<"---------"<<std::endl;
      //gzmsg<<"CNT in loc: "<<contactPosInLocal.x<<","<<contactPosInLocal.y<<","<<contactPosInLocal.z<<std::endl;

Jennifer Buehler's avatar
Jennifer Buehler committed
909
      /*GzVector3 sDiff=avgPos-linkWorldPose.pos;
910
911
912
913
      gzmsg<<"SIMPLE trans: "<<sDiff.x<<","<<sDiff.y<<","<<sDiff.z<<std::endl;
      gzmsg<<"coll world pose: "<<linkWorldPose.pos.x<<", "<<linkWorldPose.pos.y<<", "<<linkWorldPose.pos.z<<std::endl;
      gzmsg<<"contact avg pose: "<<avgPos.x<<", "<<avgPos.y<<", "<<avgPos.z<<std::endl;

Jennifer Buehler's avatar
Jennifer Buehler committed
914
915
916
      GzVector3 lX=linkWorldPose.rot.GetXAxis();
      GzVector3 lY=linkWorldPose.rot.GetYAxis();
      GzVector3 lZ=linkWorldPose.rot.GetZAxis();
917
918
919
920
921
922

      gzmsg<<"World ori: "<<linkWorldPose.rot.x<<","<<linkWorldPose.rot.y<<","<<linkWorldPose.rot.z<<","<<linkWorldPose.rot.w<<std::endl;
      gzmsg<<"x axis: "<<lX.x<<","<<lX.y<<","<<lX.z<<std::endl;
      gzmsg<<"y axis: "<<lY.x<<","<<lY.y<<","<<lY.z<<std::endl;
      gzmsg<<"z axis: "<<lZ.x<<","<<lZ.y<<","<<lZ.z<<std::endl;*/

Jennifer Buehler's avatar
Jennifer Buehler committed
923
924
925
926
      // Get the pose of the object and compute it's relative position to
      // the collision surface.
      GzPose3 objWorldPose = gazebo::GetWorldPose(objCollision->GetLink());
      GzMatrix4 worldToObj = gazebo::GetMatrix(objWorldPose);
927

Jennifer Buehler's avatar
Jennifer Buehler committed
928
929
      GzMatrix4 objInLocal = worldToLinkInv * worldToObj;
      GzVector3 objPosInLocal = gazebo::GetPos(objInLocal);
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946

      {
        boost::mutex::scoped_lock lock(this->mutexContacts);
        CollidingPoint &p =
          this->contacts[collidingObjName][collidingLink];  // inserts new entry if doesn't exist
        p.gripperName = gripperOfCollidingLink;
        p.collLink = linkCollision;
        p.collObj = objCollision;
        p.force += avgForce;
        p.pos += contactPosInLocal;
        p.objPos += objPosInLocal;
        p.sum++;
      }
      //gzmsg<<"Average force of contact= "<<avgForce.x<<", "<<avgForce.y<<", "<<avgForce.z<<" out of "<<force.size()<<" vectors."<<std::endl;
    }
  }
}