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
      // 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
557
558
    physics::CollisionPtr objColl =
      boost::dynamic_pointer_cast<physics::Collision>(GetEntityByName(world, objName));
559
560
    if (objColl && objColl->GetLink())
    {
561
562
563
564
      auto linVel = GetWorldVelocity(objColl->GetLink());
      gzmsg << "Velocity for link " << objColl->GetLink()->GetName()
        << " (collision name " << objName << "): " << linVel
        << ", absolute val " << GetLength(linVel) << std::endl;
565
566
567
568
569
570
571
    }
#endif
    // ------------------- 

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

573
574
    // add to "gripped objects"
    grippedObjects.insert(objName);
575

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

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

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

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

587
588
589
590
591
592
593
594
595
596
597
598
    // 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;
    }
599

600
601
602
603
604
    // 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())
605
    {
606
607
608
609
      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
610
    }
611
612
613
    GazeboGraspGripper &graspingGripper = gIt->second;

    if (graspingGripper.isObjectAttached())
614
    {
615
616
617
618
619
      gzerr << "GazeboGraspFix has found that object " <<
            graspingGripper.attachedObject() << " is already attached to gripper " <<
            graspingGripperName << ", so can't grasp '" << objName << "'!" << std::endl;
      continue;
    }
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
646
647
    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;
      }
    }
648

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


Jennifer Buehler's avatar
Jennifer Buehler committed
658

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

662
663
664
665
666
667
  // 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
668

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

671
672
673
674
675
676
    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
677

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

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

682
683
684
685
686
687
688
689
690
    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
691

692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
    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
717
    }
718
719
720
721
722
723
724
725
726
727

    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
728
729
730
      GzVector3 relContactPos = cpInfo.pos / cpInfo.sum;
      // Initial distance from link to object (relative to link)
      GzVector3 relObjPos = cpInfo.objPos / cpInfo.sum;
731

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

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

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

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

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

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

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

      //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
765
766
      float diff =
        fabs(gazebo::GetLength(oldObjDist) - gazebo::GetLength(newObjDist));
767
768
769
770
771
772
      //gzmsg<<"Diff for link "<<cpInfo.collLink->GetName()<<": "<<diff<<std::endl;

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

    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
795
    }
796
797
798
  }

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

801
802
803
804
805
806
807
808
809
////////////////////////////////////////////////////////////////////////////////
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
810
        gazebo::GetEntityByName(this->world, _msg->contact(i).collision1()));
811
812
    physics::CollisionPtr collision2 =
      boost::dynamic_pointer_cast<physics::Collision>(
Jennifer Buehler's avatar
Jennifer Buehler committed
813
        gazebo::GetEntityByName(this->world, _msg->contact(i).collision2()));
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
847
848

    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
849
      std::vector<GzVector3> force;
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
877
878

      // 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
879
      GzVector3 avgForce;
880
881
882
883
884
885
886
      // 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
887
      GzVector3 avgPos;
888
889
890
891
892
      // 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
893
      GzPose3 linkWorldPose = gazebo::GetWorldPose(linkCollision->GetLink());
894
895

      // 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
896
      GzMatrix4 worldToLink = gazebo::GetMatrix(linkWorldPose);
897

Jennifer Buehler's avatar
Jennifer Buehler committed
898
899
900
      // 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);
901
902
903

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

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

Jennifer Buehler's avatar
Jennifer Buehler committed
911
      /*GzVector3 sDiff=avgPos-linkWorldPose.pos;
912
913
914
915
      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
916
917
918
      GzVector3 lX=linkWorldPose.rot.GetXAxis();
      GzVector3 lY=linkWorldPose.rot.GetYAxis();
      GzVector3 lZ=linkWorldPose.rot.GetZAxis();
919
920
921
922
923
924

      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
925
926
927
928
      // 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);
929

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

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