GazeboGraspFix.cpp 36.1 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
  // Release filter to make it safe to reload the model with plugin
40
  if (!filter_name.empty() && this->world)
41
42
43
  {
    physics::PhysicsEnginePtr physics = GetPhysics(this->world);
    physics::ContactManager *contactManager = physics->GetContactManager();
44
45
    if (contactManager)
        contactManager->RemoveFilter(filter_name);
46
  }
47
48
49
  this->update_connection.reset();
  if (this->node) this->node->Fini();
  this->node.reset();
Jennifer Buehler's avatar
Jennifer Buehler committed
50
51
}

52
////////////////////////////////////////////////////////////////////////////////
53
54
void GazeboGraspFix::Init()
{
55
  this->prevUpdateTime = common::Time::GetWallTime();
Jennifer Buehler's avatar
Jennifer Buehler committed
56
57
}

58
////////////////////////////////////////////////////////////////////////////////
59
60
void GazeboGraspFix::InitValues()
{
61
#if GAZEBO_MAJOR_VERSION > 2
62
  gazebo::common::Console::SetQuiet(false);
63
64
#endif

65
66
67
68
69
70
71
72
  // 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
73
74
}

75
////////////////////////////////////////////////////////////////////////////////
76
77
void GazeboGraspFix::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
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
197
198
199
200
201
202
203
204
  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;
205
206
    }

207
208
    std::string armName = armNameElem->Get<std::string>();
    std::string palmName = handLinkElem->Get<std::string>();
209

210
211
212
213
214
215
216
    // 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);
217
218
    }

219
220
    // add new gripper
    if (grippers.find(armName) != grippers.end())
221
    {
222
223
      gzerr << "GazeboGraspFix: Arm named " << armName <<
            " was already added, cannot add it twice." << std::endl;
224
    }
225
226
227
228
    GazeboGraspGripper &gripper = grippers[armName];
    std::map<std::string, physics::CollisionPtr> _collisions;
    if (!gripper.Init(_parent, armName, palmName, fingerLinkNames,
                      disableCollisionsOnAttach, _collisions))
229
    {
230
231
232
233
      gzerr << "GazeboGraspFix: Could not initialize arm " << armName << ". Skipping."
            << std::endl;
      grippers.erase(armName);
      continue;
Jennifer Buehler's avatar
Jennifer Buehler committed
234
    }
235
236
237
238
    // add all the grippers's collision elements
    for (std::map<std::string, physics::CollisionPtr>::iterator collIt =
           _collisions.begin();
         collIt != _collisions.end(); ++collIt)
239
    {
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
      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);
255
    }
256
257
258
259
260
261
262
263
264
265
266
  }

  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
267
268
  physics::PhysicsEnginePtr physics = GetPhysics(this->world);
  this->node->Init(gazebo::GetName(*(this->world)));
269
  physics::ContactManager *contactManager = physics->GetContactManager();
Jennifer Buehler's avatar
Jennifer Buehler committed
270
  contactManager->PublishContacts();  // TODO not sure this is required?
271

272
273
  filter_name = model->GetScopedName();
  std::string topic = contactManager->CreateFilter(filter_name,
274
275
276
277
278
279
280
281
282
283
284
                      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
285
286
}

287
////////////////////////////////////////////////////////////////////////////////
288
289
class GazeboGraspFix::ObjectContactInfo
{
290
291
  public:

292
    // all forces effecting on the object
Jennifer Buehler's avatar
Jennifer Buehler committed
293
    std::vector<GzVector3> appliedForces;
294
295
296
297
298

    // 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;
299

300
301
302
    // maximum number of contacts of *any one* gripper
    // (any in grippersInvolved)
    int maxGripperContactCnt;
303

304
305
306
307
    // the gripper for maxGripperContactCnt
    std::string maxContactGripper;
};

308
309
310
////////////////////////////////////////////////////////////////////////////////
bool GazeboGraspFix::IsGripperLink(const std::string &linkName,
                                   std::string &gripperName) const
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
311
{
312
313
314
315
  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
316
    {
317
318
      gripperName = it->first;
      return true;
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
319
    }
320
321
322
  }
  return false;
}
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
323

324
325
////////////////////////////////////////////////////////////////////////////////
std::map<std::string, std::string> GazeboGraspFix::GetAttachedObjects() const
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
326
{
327
328
329
330
331
332
333
  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
334
    {
335
      ret[gripper.attachedObject()] = gripperName;
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
336
    }
337
338
339
  }
  return ret;
}
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
340

341
342
343
////////////////////////////////////////////////////////////////////////////////
bool GazeboGraspFix::ObjectAttachedToGripper(const ObjectContactInfo
    &objContInfo, std::string &attachedToGripper) const
344
{
345
346
347
348
349
350
  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))
351
    {
352
      return true;
353
    }
354
355
  }
  return false;
356
357
}

358
359
360
////////////////////////////////////////////////////////////////////////////////
bool GazeboGraspFix::ObjectAttachedToGripper(const std::string &gripperName,
    std::string &attachedToGripper) const
361
{
362
363
364
365
366
367
  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;
368
    return false;
369
370
371
372
373
374
375
376
377
  }
  const GazeboGraspGripper &gripper = gIt->second;
  // gzmsg<<"Gripper "<<gripperName<<" is involved in the grasp"<<std::endl;
  if (gripper.isObjectAttached())
  {
    attachedToGripper = gripperName;
    return true;
  }
  return false;
378
379
380
}


381
////////////////////////////////////////////////////////////////////////////////
382
383
384
385
386
387
388
389
390
391
/**
 * 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.
392
 *
393
394
 * \author Jennifer Buehler
 */
395
396
397
class GazeboGraspFix::CollidingPoint
{
  public:
398
    CollidingPoint(): sum(0) {}
399
400
401
402
403
404
405
406
    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) {}
407
408
409
410
411
412

    // 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;

413
    // the collision
414
415
416
    physics::CollisionPtr collLink, collObj;

    // average force vector of the colliding point
Jennifer Buehler's avatar
Jennifer Buehler committed
417
    GzVector3 force;
418
419
420

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

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

427
428
429
430
431
432
    // 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
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
489
490
491
492
493
494
495
496
497
////////////////////////////////////////////////////////////////////////////////
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;
}

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
524
525
526
527
528
529
530
531
532
////////////////////////////////////////////////////////////////////////////////
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
533
      GzVector3 avgForce = collP.force / collP.sum;
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
      // 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
566
567
    physics::CollisionPtr objColl =
      boost::dynamic_pointer_cast<physics::Collision>(GetEntityByName(world, objName));
568
569
    if (objColl && objColl->GetLink())
    {
570
571
572
573
      auto linVel = GetWorldVelocity(objColl->GetLink());
      gzmsg << "Velocity for link " << objColl->GetLink()->GetName()
        << " (collision name " << objName << "): " << linVel
        << ", absolute val " << GetLength(linVel) << std::endl;
574
575
576
577
578
579
580
    }
#endif
    // ------------------- 

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

582
583
    // add to "gripped objects"
    grippedObjects.insert(objName);
584

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

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

590
591
592
    // only need to attach object if the grip count threshold is exceeded
    if (counts <= this->gripCountThreshold)
      continue;
593

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

596
597
598
599
600
601
602
603
604
605
606
607
    // 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;
    }
608

609
610
611
612
613
    // 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())
614
    {
615
616
617
618
      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
619
    }
620
621
622
    GazeboGraspGripper &graspingGripper = gIt->second;

    if (graspingGripper.isObjectAttached())
623
    {
624
625
626
627
628
      gzerr << "GazeboGraspFix has found that object " <<
            graspingGripper.attachedObject() << " is already attached to gripper " <<
            graspingGripperName << ", so can't grasp '" << objName << "'!" << std::endl;
      continue;
    }
629

630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
    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;
      }
    }
657

658
659
660
661
662
663
664
    if (!graspingGripper.HandleAttach(objName))
    {
      gzerr << "GazeboGraspFix: Could not attach object " << objName << " to gripper "
            << graspingGripperName << std::endl;
    }
    this->OnAttach(objName, graspingGripperName);
  }  // for all objects
665
666


Jennifer Buehler's avatar
Jennifer Buehler committed
667

668
669
  // ++++++++++++++++++++ Handle Detachment  +++++++++++++++++++++++++++++++
  std::map<std::string, std::string> attachedObjects = GetAttachedObjects();
Jennifer Buehler's avatar
Jennifer Buehler committed
670

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

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

680
681
682
683
684
685
    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
686

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

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

691
692
693
694
695
696
697
698
699
    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
700

701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
    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
726
    }
727
728
729
730
731
732
733
734
735
736

    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
737
738
739
      GzVector3 relContactPos = cpInfo.pos / cpInfo.sum;
      // Initial distance from link to object (relative to link)
      GzVector3 relObjPos = cpInfo.objPos / cpInfo.sum;
740

Jennifer Buehler's avatar
Jennifer Buehler committed
741
742
743
      // Get current world pose of object
      GzPose3 currObjWorldPose =
        gazebo::GetWorldPose(cpInfo.collObj->GetLink());
744

Jennifer Buehler's avatar
Jennifer Buehler committed
745
746
747
      // Get world pose of link
      GzPose3 currLinkWorldPose =
        gazebo::GetWorldPose(cpInfo.collLink->GetLink());
748
749

      // Get transform for currLinkWorldPose as matrix
Jennifer Buehler's avatar
Jennifer Buehler committed
750
      GzMatrix4 worldToLink = gazebo::GetMatrix(currLinkWorldPose);
751
752

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

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

Jennifer Buehler's avatar
Jennifer Buehler committed
759
      // The initial contact point location on the link should still correspond
760
761
      // to the initial contact point location on the object.

Jennifer Buehler's avatar
Jennifer Buehler committed
762
      // Initial vector from object center to contact point (relative to link,
763
      // because relObjPos and relContactPos are from center of link)
Jennifer Buehler's avatar
Jennifer Buehler committed
764
765
      GzVector3 oldObjDist = relContactPos - relObjPos;
      // The same vector as \e oldObjDist, but calculated by the current world pose
766
      // of object and the current location of the initial contact location on the link.
Jennifer Buehler's avatar
Jennifer Buehler committed
767
768
      // This is the new distance from contact to object.
      GzVector3 newObjDist = currContactWorldPose - gazebo::GetPos(currObjWorldPose);
769
770
771
772
773

      //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
774
775
      float diff =
        fabs(gazebo::GetLength(oldObjDist) - gazebo::GetLength(newObjDist));
776
777
778
779
780
781
      //gzmsg<<"Diff for link "<<cpInfo.collLink->GetName()<<": "<<diff<<std::endl;

      if (diff > releaseTolerance)
      {
        ++cntRelease;
      }
Jennifer Buehler's avatar
Jennifer Buehler committed
782
    }
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803

    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
804
    }
805
806
807
  }

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

810
811
812
813
814
815
816
817
818
////////////////////////////////////////////////////////////////////////////////
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
819
        gazebo::GetEntityByName(this->world, _msg->contact(i).collision1()));
820
821
    physics::CollisionPtr collision2 =
      boost::dynamic_pointer_cast<physics::Collision>(
Jennifer Buehler's avatar
Jennifer Buehler committed
822
        gazebo::GetEntityByName(this->world, _msg->contact(i).collision2()));
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
849
850
851
852
853
854
855
856
857

    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
858
      std::vector<GzVector3> force;
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887

      // 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
888
      GzVector3 avgForce;
889
890
891
892
893
894
895
      // 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
896
      GzVector3 avgPos;
897
898
899
900
901
      // 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
902
      GzPose3 linkWorldPose = gazebo::GetWorldPose(linkCollision->GetLink());
903
904

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

Jennifer Buehler's avatar
Jennifer Buehler committed
907
908
909
      // 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);
910
911
912

      // now, worldToLink * contactInLocal = worldToContact
      // hence, contactInLocal = worldToLink.Inv * worldToContact
Jennifer Buehler's avatar
Jennifer Buehler committed
913
914
915
      GzMatrix4 worldToLinkInv = worldToLink.Inverse();
      GzMatrix4 contactInLocal = worldToLinkInv * worldToContact;
      GzVector3 contactPosInLocal = gazebo::GetPos(contactInLocal);
916
917
918
919

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

Jennifer Buehler's avatar
Jennifer Buehler committed
920
      /*GzVector3 sDiff=avgPos-linkWorldPose.pos;
921
922
923
924
      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
925
926
927
      GzVector3 lX=linkWorldPose.rot.GetXAxis();
      GzVector3 lY=linkWorldPose.rot.GetYAxis();
      GzVector3 lZ=linkWorldPose.rot.GetZAxis();
928
929
930
931
932
933

      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
934
935
936
937
      // 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);
938

Jennifer Buehler's avatar
Jennifer Buehler committed
939
940
      GzMatrix4 objInLocal = worldToLinkInv * worldToObj;
      GzVector3 objPosInLocal = gazebo::GetPos(objInLocal);
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957

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