GazeboGraspFix.cpp 36 KB
Newer Older
Jennifer Buehler's avatar
Jennifer Buehler committed
1
2
3
4
5
6
7
8
9
10
11
12
#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>
 
#include <gazebo_grasp_plugin/GazeboGraspFix.h>

using gazebo::GazeboGraspFix;

13
14
15
16
17
18
19

#define DEFAULT_FORCES_ANGLE_TOLERANCE 120
#define DEFAULT_UPDATE_RATE 5
#define DEFAULT_MAX_GRIP_COUNT 10    
#define DEFAULT_RELEASE_TOLERANCE 0.005
#define DEFAULT_DISABLE_COLLISIONS_ON_ATTACH false

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


GazeboGraspFix::GazeboGraspFix(){
    InitValues();
}

28
29
GazeboGraspFix::GazeboGraspFix(physics::ModelPtr _model)
{
Jennifer Buehler's avatar
Jennifer Buehler committed
30
31
32
    InitValues();
}

33
34
GazeboGraspFix::~GazeboGraspFix()
{
Jennifer Buehler's avatar
Jennifer Buehler committed
35
36
37
38
39
    this->update_connection.reset();
    if (this->node) this->node->Fini();
    this->node.reset();
}

40
41
void GazeboGraspFix::Init()
{
Jennifer Buehler's avatar
Jennifer Buehler committed
42
43
44
    this->prevUpdateTime = common::Time::GetWallTime();
}

45
46
void GazeboGraspFix::InitValues()
{
47
#if GAZEBO_MAJOR_VERSION > 2
48
    gazebo::common::Console::SetQuiet(false);
49
50
#endif

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


62
63
64
void GazeboGraspFix::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
    std::cout<<"Loading grasp-fix plugin"<<std::endl;
65
66
67
68
69
70
71
    
    // ++++++++++++ Read parameters and initialize fields  +++++++++++++++
    
    physics::ModelPtr model = _parent;
    this->world = model->GetWorld();
    
    sdf::ElementPtr disableCollisionsOnAttachElem = _sdf->GetElement("disable_collisions_on_attach");
72
73
    if (!disableCollisionsOnAttachElem){
        std::cout<<"GazeboGraspFix: Using default "<<DEFAULT_DISABLE_COLLISIONS_ON_ATTACH<<" because no <disable_collisions_on_attach> element specified."<<std::endl;
74
75
        this->disableCollisionsOnAttach = DEFAULT_DISABLE_COLLISIONS_ON_ATTACH;
    } else {
76
77
78
79
        std::string str = disableCollisionsOnAttachElem->Get<std::string>();
        bool bVal = false;
        if ((str=="true") || (str=="1"))  bVal = true;
        this->disableCollisionsOnAttach = bVal;
80
        std::cout<<"GazeboGraspFix: Using disable_collisions_on_attach "<<this->disableCollisionsOnAttach<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
81
82
    }

83
    sdf::ElementPtr forcesAngleToleranceElem = _sdf->GetElement("forces_angle_tolerance");
84
    if (!forcesAngleToleranceElem){
85
86
87
88
89
90
91
92
        std::cout<<"GazeboGraspFix: Using default tolerance of "<<DEFAULT_FORCES_ANGLE_TOLERANCE<<" because no <forces_angle_tolerance> element specified."<<std::endl;
        this->forcesAngleTolerance = DEFAULT_FORCES_ANGLE_TOLERANCE * M_PI/180;
    } else {
        this->forcesAngleTolerance = forcesAngleToleranceElem->Get<float>() * M_PI/180;
    }

    sdf::ElementPtr updateRateElem = _sdf->GetElement("update_rate");
    double _updateSecs;
93
    if (!updateRateElem){
94
95
96
97
98
99
100
101
102
103
104
        std::cout<<"GazeboGraspFix: Using  "<<DEFAULT_UPDATE_RATE<<" because no <updateRate_tag> element specified."<<std::endl;
        _updateSecs = 1.0 / DEFAULT_UPDATE_RATE;
    } else {
        int _rate = updateRateElem->Get<int>();
        double _updateRate = _rate;
        _updateSecs = 1.0/_updateRate;
        std::cout<<"GazeboGraspFix: Using update rate "<<_rate<<std::endl;
    }
    this->updateRate = common::Time(0, common::Time::SecToNano(_updateSecs));

    sdf::ElementPtr maxGripCountElem = _sdf->GetElement("max_grip_count");
105
    if (!maxGripCountElem){
106
107
108
109
110
111
112
113
        std::cout<<"GazeboGraspFix: Using  "<<DEFAULT_MAX_GRIP_COUNT<<" because no <max_grip_count> element specified."<<std::endl;
        this->maxGripCount = DEFAULT_MAX_GRIP_COUNT;
    } else {
        this->maxGripCount = maxGripCountElem->Get<int>();
        std::cout<<"GazeboGraspFix: Using max_grip_count "<<this->maxGripCount<<std::endl;
    }

    sdf::ElementPtr gripCountThresholdElem = _sdf->GetElement("grip_count_threshold");
114
    if (!gripCountThresholdElem){
115
116
117
118
119
120
121
122
        this->gripCountThreshold=floor(this->maxGripCount/2.0);
        std::cout<<"GazeboGraspFix: Using  "<<this->gripCountThreshold<<" because no <grip_count_threshold> element specified."<<std::endl;
    } else {
        this->gripCountThreshold = gripCountThresholdElem->Get<int>();
        std::cout<<"GazeboGraspFix: Using grip_count_threshold "<<this->gripCountThreshold<<std::endl;
    }

    sdf::ElementPtr releaseToleranceElem = _sdf->GetElement("release_tolerance");
123
    if (!releaseToleranceElem){
124
125
126
127
128
129
130
        std::cout<<"GazeboGraspFix: Using  "<<DEFAULT_RELEASE_TOLERANCE<<" because no <release_tolerance> element specified."<<std::endl;
        this->releaseTolerance = DEFAULT_RELEASE_TOLERANCE;
    } else {
        this->releaseTolerance = releaseToleranceElem->Get<float>();
        std::cout<<"GazeboGraspFix: Using release_tolerance "<<this->releaseTolerance<<std::endl;
    }

131
132
133
134
    // will contain all names of collision entities involved from all arms
    std::vector<std::string> collisionNames;
    
    sdf::ElementPtr armElem=_sdf->GetElement("arm");
135
    if (!armElem)
136
137
138
    {
        gzerr <<"GazeboGraspFix: Cannot load the GazeboGraspFix without any <arm> declarations"<<std::endl;
        return;
139
    }
140
141
142
143
144
145
    // 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");
146

147
        if (!handLinkElem || !fingerLinkElem || !armNameElem) {
148
149
            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;
Jennifer Buehler's avatar
Jennifer Buehler committed
150
151
            continue;
        }
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
            
        std::string armName = armNameElem->Get<std::string>();
        std::string palmName = handLinkElem->Get<std::string>();
        
        // 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);
        }
            
        // add new gripper
        if (grippers.find(armName)!=grippers.end())
        {
            gzerr<<"GazeboGraspFix: Arm named "<<armName<<" was already added, cannot add it twice."<<std::endl;
        }
        GazeboGraspGripper& gripper = grippers[armName];
        std::map<std::string, physics::CollisionPtr> _collisions;
        if (!gripper.Init(_parent, armName, palmName, fingerLinkNames, disableCollisionsOnAttach, _collisions))
        {
            gzerr<<"GazeboGraspFix: Could not initialize arm "<<armName<<". Skipping."<<std::endl;
            grippers.erase(armName);
            continue;
        }
        // add all the grippers's collision elements
        for (std::map<std::string, physics::CollisionPtr>::iterator collIt = _collisions.begin();
            collIt != _collisions.end(); ++collIt)
        {
            const std::string& collName=collIt->first;
            //physics::CollisionPtr& coll=collIt->second;
            std::map<std::string, std::string>::iterator collIter = this->collisions.find(collName);
Jennifer Buehler's avatar
Jennifer Buehler committed
184
            if (collIter != this->collisions.end()) { //this collision was already added before
185
                gzwarn <<"GazeboGraspFix: Adding Gazebo collision link element "<<collName<<" multiple times, the grasp plugin may not work properly"<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
186
187
                continue;
            }
188
189
190
            std::cout<<"GazeboGraspFix: Adding collision scoped name "<<collName<<std::endl;
            this->collisions[collName] = armName;
            collisionNames.push_back(collName);
Jennifer Buehler's avatar
Jennifer Buehler committed
191
192
193
        }
    }

194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
    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 +++++++++++++++

    physics::PhysicsEnginePtr physics = this->world->GetPhysicsEngine();
    this->node->Init(this->world->GetName());
    physics::ContactManager * contactManager = physics->GetContactManager();
    contactManager->PublishContacts(); //XXX not sure I need this?

    std::string topic = contactManager->CreateFilter(model->GetScopedName(), collisionNames);
209
    if (!this->contactSub) {
Jennifer Buehler's avatar
Jennifer Buehler committed
210
211
212
213
214
215
216
217
        std::cout<<"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));
}

218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239

class GazeboGraspFix::ObjectContactInfo
{
    public:
     
    // all forces effecting on the object
    std::vector<gazebo::math::Vector3> appliedForces;

    // 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;
    
    // maximum number of contacts of *any one* gripper
    // (any in grippersInvolved)
    int maxGripperContactCnt;
    
    // the gripper for maxGripperContactCnt
    std::string maxContactGripper;
};


Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272

bool GazeboGraspFix::isGripperLink(const std::string& linkName, std::string& gripperName) const
{
    for (std::map<std::string, GazeboGraspGripper>::const_iterator it=grippers.begin(); it!=grippers.end(); ++it)
    {
        if (it->second.hasLink(linkName))
        {
            gripperName=it->first;
            return true;
        }
    }
    return false;
}   

std::map<std::string, std::string> GazeboGraspFix::getAttachedObjects() const
{
    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())
        {
            ret[gripper.attachedObject()]=gripperName;
        }
    }
    return ret;
}   





273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
bool GazeboGraspFix::objectAttachedToGripper(const ObjectContactInfo& objContInfo, std::string& attachedToGripper) const
{
    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))
        {
            return true; 
        }
    }
    return false;
}

bool GazeboGraspFix::objectAttachedToGripper(const std::string& gripperName, std::string& attachedToGripper) const
{
    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;
        return false; 
    }
    const GazeboGraspGripper& gripper = gIt->second;
    // std::cout<<"Gripper "<<gripperName<<" is involved in the grasp"<<std::endl;
    if (gripper.isObjectAttached())
    {
        attachedToGripper = gripperName;
        return true;
    }
    return false;
}


/**
 * Helper class to encapsulate a collision information.
 * One contact has two bodies, and only
 * the ones where one of the bodies is a gripper link are considered.
 * Each contact consists of a *list* of forces with their own origin/position each
 * (e.g. when the object and gripper are colliding at several places).
 * The averages of each contact's force vectors along with their origins are
 * *accumulated* in the given Vector3 \e pos and \eforce objects.
 * The number of additions is stored in \e sum.
 * This is to get the average force application over time between link and object.
 * 
 * \author Jennifer Buehler
 */
class GazeboGraspFix::CollidingPoint{
public:
    CollidingPoint(): sum(0) {}
    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){}

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

    // the collision        
    physics::CollisionPtr collLink, collObj;

    // average force vector of the colliding point
    gazebo::math::Vector3 force;

    // position (relative to reference frame of gripper
    // collision surface) where the contact happens on collision surface
    gazebo::math::Vector3 pos;

    // position (relative to reference frame of *gripper* collision surface)
    // where the object center is located during collision. 
    gazebo::math::Vector3 objPos;
     
    // sum of force and pose (they are actually summed
    // up from several contact points).
    // Divide both \e force and \e pos by this to obtain average
    int sum;
};



Jennifer Buehler's avatar
Jennifer Buehler committed
358
void GazeboGraspFix::OnUpdate() {
359
    if ((common::Time::GetWallTime() - this->prevUpdateTime) < this->updateRate)
Jennifer Buehler's avatar
Jennifer Buehler committed
360
361
        return;

362
363
    // first, copy all contact data into local struct. Don't do the complex grip check (checkGrip)
    // within the mutex, because that slows down OnContact().
Jennifer Buehler's avatar
Jennifer Buehler committed
364
    this->mutexContacts.lock();
365
366
    std::map<std::string, std::map<std::string, CollidingPoint> > contPoints(this->contacts);
    this->contacts.clear(); // clear so it can be filled anew by OnContact().
Jennifer Buehler's avatar
Jennifer Buehler committed
367
368
    this->mutexContacts.unlock();

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

371
372
373

    // 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). 
Jennifer Buehler's avatar
Jennifer Buehler committed
374
    std::map<std::string, std::map<std::string, CollidingPoint> >::iterator objIt;
375
376
    std::map<std::string, ObjectContactInfo> objectContactInfo;

377
378
    for (objIt=contPoints.begin(); objIt!=contPoints.end(); ++objIt)
    {
Jennifer Buehler's avatar
Jennifer Buehler committed
379
380
        std::string objName=objIt->first;
        //std::cout<<"Examining object collisions with "<<objName<<std::endl;
381
382
383
            
        // create new entry in accumulated results map and get reference to fill in:
        ObjectContactInfo& objContInfo = objectContactInfo[objName];
384
385
       
        // for all links colliding with this object... 
386
        std::map<std::string, CollidingPoint>::iterator lIt;
Jennifer Buehler's avatar
Jennifer Buehler committed
387
388
389
390
        for (lIt=objIt->second.begin(); lIt!=objIt->second.end(); ++lIt){
            std::string linkName=lIt->first;
            CollidingPoint& collP=lIt->second;
            gazebo::math::Vector3 avgForce=collP.force/collP.sum;
391
            // std::cout << "Found collision with "<<linkName<<": "<<avgForce.x<<", "<<avgForce.y<<", "<<avgForce.z<<" (avg over "<<collP.sum<<")"<<std::endl;
392
393
394
395
396
397
398
399
400
401
            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;
            }
Jennifer Buehler's avatar
Jennifer Buehler committed
402
403
        }
    }
404
405
 
    // ++++++++++++++++++++ Handle Attachment  +++++++++++++++++++++++++++++++
Jennifer Buehler's avatar
Jennifer Buehler committed
406

407
408
409
410
    // 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.
Jennifer Buehler's avatar
Jennifer Buehler committed
411
    std::set<std::string> grippedObjects;    
412
413
414
415
416
417
418
    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;
    
        // std::cout<<"Number applied forces on "<<objName<<": "<<objContInfo.appliedForces.size()<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
419
        
420
        float minAngleDiff= this->forcesAngleTolerance; //120 * M_PI/180;
421
422
        if (!checkGrip(objContInfo.appliedForces, minAngleDiff, 0.3)) 
            continue;
Jennifer Buehler's avatar
Jennifer Buehler committed
423
        
424
425
426
427
        // add to "gripped objects" 
        grippedObjects.insert(objName);
        
        //std::cout<<"Grasp Held: "<<objName<<" grip count: "<<this->gripCounts[objName]<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
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
        int& counts = this->gripCounts[objName];
        if (counts < this->maxGripCount) ++counts;                

        // only need to attach object if the grip count threshold is exceeded        
        if (counts <= this->gripCountThreshold)
            continue;
        
        //std::cout<<"GRIPPING "<<objName<<", grip count "<<counts<<" (threshold "<<this->gripCountThreshold<<")"<<std::endl;
        
        // 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.
            // std::cout << "GazeboGraspFix has found that object "<<
            //     gripper.attachedObject()<<" is already attached to gripper "<<gripperName;
            continue;       
        }

        // 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())
        {
            gzerr << "GazeboGraspFix: Inconsistency, gripper '"<<graspingGripperName
                  << "' not found in GazeboGraspFix grippers, so cannot do attachment of object " << objName << std::endl;
            continue;
        }
        GazeboGraspGripper& graspingGripper = gIt->second;
                
        if (graspingGripper.isObjectAttached())
        {
           gzerr<<"GazeboGraspFix has found that object "<<
                 graspingGripper.attachedObject()<<" is already attached to gripper "<<
                 graspingGripperName<<", so can't grasp '"<<objName<<"'!"<<std::endl;
            continue;
        }

        std::cout<<"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;
            // std::cout<<"Checking initial contact with "<<collidingLink<<" and "<<graspingGripperName<<std::endl;
            if (graspingGripper.hasCollisionLink(collidingLink))
            {
                // std::cout<<"Insert initial contact with "<<collidingLink<<std::endl;
                attGripConts[collidingLink] = collidingPoint;
Jennifer Buehler's avatar
Jennifer Buehler committed
489
            }
490
491
492
493
        } 

        if (!graspingGripper.HandleAttach(objName)){
            gzerr<<"GazeboGraspFix: Could not attach object "<<objName<<" to gripper "<<graspingGripperName<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
494
        }
495
496
497
498
        this->OnAttach(objName, graspingGripperName);
    }  // for all objects


Jennifer Buehler's avatar
Jennifer Buehler committed
499

500
501
    // ++++++++++++++++++++ Handle Detachment  +++++++++++++++++++++++++++++++
    std::map<std::string, std::string> attachedObjects = getAttachedObjects();
Jennifer Buehler's avatar
Jennifer Buehler committed
502

503
504
    // now, for all objects that are not currently gripped,
    // decrease grip counter, and possibly release object.
Jennifer Buehler's avatar
Jennifer Buehler committed
505
    std::map<std::string, int>::iterator gripCntIt;
506
    for (gripCntIt = this->gripCounts.begin(); gripCntIt != this->gripCounts.end(); ++gripCntIt){
Jennifer Buehler's avatar
Jennifer Buehler committed
507

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

510
511
512
513
514
        if (grippedObjects.find(objName) != grippedObjects.end())
        {   // this object is one we just detected as "gripped", so no need to check for releasing it...
            // std::cout<<"NOT considering "<<objName<<" for detachment."<<std::endl;
            continue;
        }
Jennifer Buehler's avatar
Jennifer Buehler committed
515
        
516
        // the object does not satisfy "gripped" criteria, so potentially has to be released.
Jennifer Buehler's avatar
Jennifer Buehler committed
517

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

520
        if (gripCntIt->second > 0) --(gripCntIt->second);
521
522
523
    
        std::map<std::string, std::string>::iterator attIt = attachedObjects.find(objName);
        bool isAttached = (attIt != attachedObjects.end());
524
        
525
526
527
528
529
530
        // std::cout<<"is attached: "<<isAttached<<std::endl;

        if (!isAttached || (gripCntIt->second > this->gripCountThreshold)) continue;

        const std::string& graspingGripperName=attIt->second;

531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
        // std::cout<<"Considering "<<objName<<" for detachment."<<std::endl;

        // Object should potentially be detached now.
        // However, this happens too easily when just considering the count, as the fingers
        // in gazebo start wobbling as the arm moves around, and although they are still
        // close to the object, the grip is not detected any more. So to be sure, we will
        // check that the collision point (the place on the link where the contact originally
        // was detected) has not moved too far from where it originally was, relative to the object.

        // get the initial set of CollidingPoints for this object
        // note that it is enough to use the initial contact points, because the object won't
        // have "slipped" after being attached, and the location of the original contact point
        // on the link itself is considered as a fixed point on the link, regardless whether this
        // point is currently still colliding with the object or not. We only want to know whether
        // this fixed point on the link has increased in distance from the corresponding fixed
        // point (where the contact originally happened) on the object.
        std::map<std::string, std::map<std::string, CollidingPoint> >::iterator initCollIt=this->attachGripContacts.find(objName);
        if (initCollIt == this->attachGripContacts.end()) {
            std::cerr<<"ERROR: Consistency: Could not find attachGripContacts for "<<objName<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
550
551
552
            continue;
        }

553
        std::map<std::string, CollidingPoint>& initColls=initCollIt->second;
Jennifer Buehler's avatar
Jennifer Buehler committed
554
        int cntRelease=0;
555
556
       
        // for all links which have initially been detected to collide: 
Jennifer Buehler's avatar
Jennifer Buehler committed
557
        std::map<std::string, CollidingPoint>::iterator pointIt;
558
559
        for (pointIt=initColls.begin(); pointIt!=initColls.end(); ++pointIt)
        {
Jennifer Buehler's avatar
Jennifer Buehler committed
560
            CollidingPoint& cpInfo=pointIt->second;
561
            // initial distance from link to contact point (relative to link)
Jennifer Buehler's avatar
Jennifer Buehler committed
562
            gazebo::math::Vector3 relContactPos=cpInfo.pos/cpInfo.sum;
563
            // initial distance from link to object (relative to link)
Jennifer Buehler's avatar
Jennifer Buehler committed
564
            gazebo::math::Vector3 relObjPos=cpInfo.objPos/cpInfo.sum;
565
566
           
            // get current world pose of object 
Jennifer Buehler's avatar
Jennifer Buehler committed
567
568
            gazebo::math::Pose currObjWorldPose=cpInfo.collObj->GetLink()->GetWorldPose();

569
570
571
572
573
574
            // get world pose of link
            gazebo::math::Pose currLinkWorldPose=cpInfo.collLink->GetLink()->GetWorldPose();

            // Get transform for currLinkWorldPose as matrix
            gazebo::math::Matrix4 worldToLink=currLinkWorldPose.rot.GetAsMatrix4();
            worldToLink.SetTranslate(currLinkWorldPose.pos);
Jennifer Buehler's avatar
Jennifer Buehler committed
575

576
577
578
            // Get the transform from collision link to contact point
            gazebo::math::Matrix4 linkToContact=gazebo::math::Matrix4::IDENTITY;
            linkToContact.SetTranslate(relContactPos);
Jennifer Buehler's avatar
Jennifer Buehler committed
579
                    
580
581
            // the current world position of the contact point right now is:
            gazebo::math::Matrix4 _currContactWorldPose=worldToLink*linkToContact;
Jennifer Buehler's avatar
Jennifer Buehler committed
582
583
            gazebo::math::Vector3 currContactWorldPose=_currContactWorldPose.GetTranslation();

584
585
586
587
588
589
590
591
592
            // the initial contact point location on the link should still correspond
            // to the initial contact point location on the object.

            // initial vector from object center to contact point (relative to link,
            // because relObjPos and relContactPos are from center of link)
            gazebo::math::Vector3 oldObjDist= relContactPos - relObjPos;
            // the same vector as \e oldObjDist, but calculated by the current world pose
            // of object and the current location of the initial contact location on the link.
            gazebo::math::Vector3 newObjDist= currContactWorldPose - currObjWorldPose.pos; // new distance from contact to object
Jennifer Buehler's avatar
Jennifer Buehler committed
593
594
595
            
            //std::cout<<"Obj Trans "<<cpInfo.collLink->GetName()<<": "<<relObjPos.x<<", "<<relObjPos.y<<", "<<relObjPos.z<<std::endl;
            //std::cout<<"Cont Trans "<<cpInfo.collLink->GetName()<<": "<<relContactPos.x<<", "<<relContactPos.y<<", "<<relContactPos.z<<std::endl;
596
597
        
            // the difference between these vectors should not be too large...
Jennifer Buehler's avatar
Jennifer Buehler committed
598
599
600
            float diff=fabs(oldObjDist.GetLength() - newObjDist.GetLength());
            //std::cout<<"Diff for link "<<cpInfo.collLink->GetName()<<": "<<diff<<std::endl;

601
            if (diff > releaseTolerance) {
Jennifer Buehler's avatar
Jennifer Buehler committed
602
603
604
605
                ++cntRelease;
            }
        }

606
        if (cntRelease > 0)
607
608
609
610
611
612
613
614
615
616
617
618
619
        {   // 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:
            std::cout<<"GazeboGraspFix: Detaching "<<objName<<" from gripper "<<graspingGripperName<<"!!!!!!!"<<std::endl;
            graspingGripper.HandleDetach(objName);
            this->OnDetach(objName,graspingGripperName);
Jennifer Buehler's avatar
Jennifer Buehler committed
620
            gripCntIt->second=0;
621
            this->attachGripContacts.erase(initCollIt);
Jennifer Buehler's avatar
Jennifer Buehler committed
622
623
624
625
626
627
628
629
630
631
        }
    }

    this->prevUpdateTime = common::Time::GetWallTime();
}
    
double angularDistance(const gazebo::math::Vector3& _v1, const gazebo::math::Vector3& _v2) {
    gazebo::math::Vector3 v1=_v1;        
    gazebo::math::Vector3 v2=_v2;
    v1.Normalize();
632
    v2.Normalize();
Jennifer Buehler's avatar
Jennifer Buehler committed
633
634
635
636
637
    return acos(v1.Dot(v2));    
}

bool GazeboGraspFix::checkGrip(const std::vector<gazebo::math::Vector3>& forces, float minAngleDiff, float lengthRatio){
    if (((lengthRatio > 1) || (lengthRatio < 0)) && (lengthRatio > 1e-04 && (fabs(lengthRatio-1) > 1e-04)))  {
638
        std::cerr<<"ERROR: checkGrip: always specify a lengthRatio of [0..1]"<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
639
640
641
        return false;
    }
    if (minAngleDiff < M_PI_2){
642
        std::cerr<<"ERROR: checkGrip: min angle must be at least 90 degrees (PI/2)"<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
643
644
645
646
647
648
649
650
651
652
        return false;
    }
    std::vector<gazebo::math::Vector3>::const_iterator it1, it2;
    for (it1=forces.begin(); it1!=forces.end(); ++it1){
        gazebo::math::Vector3 v1=*it1;
        for (it2=it1+1; it2!=forces.end(); ++it2){
            gazebo::math::Vector3 v2=*it2;
            float l1=v1.GetLength();
            float l2=v2.GetLength();
            if ((l1<1e-04) || (l2<1e-04)) continue;
653
            /*gazebo::math::Vector3 _v1=v1;
Jennifer Buehler's avatar
Jennifer Buehler committed
654
655
656
            gazebo::math::Vector3 _v2=v2;
            _v1/=l1;
            _v2/=l2;
657
658
659
            float angle=acos(_v1.Dot(_v2));*/
            float angle=angularDistance(v1, v2);
            // std::cout<<"Angular distance between v1.len="<<v1.GetLength()<<" and v2.len="<<v2.GetLength()<<": "<<angle*180/M_PI<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
660
661
662
663
            if (angle > minAngleDiff) {
                float ratio;
                if (l1>l2) ratio=l2/l1;
                else ratio=l1/l2;
664
665
666
667
668
669
                // std::cout<<"Got angle "<<angle<<", ratio "<<ratio<<std::endl;
                if (ratio >= lengthRatio)
                {
                    // std::cout<<"checkGrip() is true"<<std::endl;
                    return true;
                }
Jennifer Buehler's avatar
Jennifer Buehler committed
670
671
672
            }            
        }
    }
673
    return false;
Jennifer Buehler's avatar
Jennifer Buehler committed
674
675
}

676
677
void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg)
{
Jennifer Buehler's avatar
Jennifer Buehler committed
678
    //std::cout<<"CONTACT! "<<std::endl;//<<_contact<<std::endl;
679
    // for all contacts...
Jennifer Buehler's avatar
Jennifer Buehler committed
680
681
682
683
684
685
    for (int i = 0; i < _msg->contact_size(); ++i) {
        physics::CollisionPtr collision1 = boost::dynamic_pointer_cast<physics::Collision>(
                this->world->GetEntity(_msg->contact(i).collision1()));
        physics::CollisionPtr collision2 = boost::dynamic_pointer_cast<physics::Collision>(
                this->world->GetEntity(_msg->contact(i).collision2()));

686
687
        if ((collision1 && !collision1->IsStatic()) && (collision2 && !collision2->IsStatic()))
        {
Jennifer Buehler's avatar
Jennifer Buehler committed
688
689
690
691
692
693
694
695
696
            std::string name1 = collision1->GetScopedName();
            std::string name2 = collision2->GetScopedName();

            //std::cout<<"OBJ CONTACT! "<<name1<<" / "<<name2<<std::endl;
            int count = _msg->contact(i).position_size();

            // Check to see if the contact arrays all have the same size.
            if ((count != _msg->contact(i).normal_size()) ||
                (count != _msg->contact(i).wrench_size()) ||
697
698
                (count != _msg->contact(i).depth_size()))
            {
699
                gzerr << "GazeboGraspFix: Contact message has invalid array sizes\n"<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
700
701
702
                continue;
            }

703
            std::string collidingObjName, collidingLink, gripperOfCollidingLink;
Jennifer Buehler's avatar
Jennifer Buehler committed
704
705
            physics::CollisionPtr linkCollision;
            physics::CollisionPtr objCollision;
706

Jennifer Buehler's avatar
Jennifer Buehler committed
707
708
709
            physics::Contact contact;
            contact = _msg->contact(i);

710
711
712
            if (contact.count<1)
            {
                std::cerr<<"ERROR: GazeboGraspFix: Not enough forces given for contact of ."<<name1<<" / "<<name2<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
713
714
715
                continue;
            }

716
            // all force vectors which are part of this contact
Jennifer Buehler's avatar
Jennifer Buehler committed
717
718
            std::vector<gazebo::math::Vector3> force;
            
719
720
            // 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'.
721
722
            std::map<std::string,std::string>::const_iterator gripperCollIt = this->collisions.find(name2);
            if (gripperCollIt != this->collisions.end())
723
            {   // collision 1 is the object
Jennifer Buehler's avatar
Jennifer Buehler committed
724
725
726
727
                collidingObjName=name1;
                collidingLink=name2;
                linkCollision=collision2;
                objCollision=collision1;
728
                gripperOfCollidingLink=gripperCollIt->second;
729
730
731
                for (int k=0; k<contact.count; ++k)
                    force.push_back(contact.wrench[k].body1Force);
            }
732
            else if ((gripperCollIt=this->collisions.find(name1)) != this->collisions.end())
733
            {   // collision 2 is the object
Jennifer Buehler's avatar
Jennifer Buehler committed
734
735
736
737
                collidingObjName=name2;
                collidingLink=name1;
                linkCollision=collision1;
                objCollision=collision2;
738
                gripperOfCollidingLink=gripperCollIt->second;
739
740
                for (int k=0; k<contact.count; ++k)
                    force.push_back(contact.wrench[k].body2Force);
Jennifer Buehler's avatar
Jennifer Buehler committed
741
742
            }

743
744
            gazebo::math::Vector3 avgForce;
            // compute average/sum of the forces applied on the object
Jennifer Buehler's avatar
Jennifer Buehler committed
745
            for (int k=0; k<force.size(); ++k){
746
                avgForce+=force[k];
Jennifer Buehler's avatar
Jennifer Buehler committed
747
            }    
748
            avgForce/=force.size();
Jennifer Buehler's avatar
Jennifer Buehler committed
749
750

            gazebo::math::Vector3 avgPos;
751
            // compute center point (average pose) of all the origin positions of the forces appied
Jennifer Buehler's avatar
Jennifer Buehler committed
752
753
754
            for (int k=0; k<contact.count; ++k) avgPos+=contact.positions[k];
            avgPos/=contact.count;

755
756
            // now, get average pose relative to the colliding link
            gazebo::math::Pose linkWorldPose=linkCollision->GetLink()->GetWorldPose();
Jennifer Buehler's avatar
Jennifer Buehler committed
757

758
759
760
            // To find out the collision point relative to the Link's local coordinate system, first get the Poses as 4x4 matrices
            gazebo::math::Matrix4 worldToLink=linkWorldPose.rot.GetAsMatrix4();
            worldToLink.SetTranslate(linkWorldPose.pos);
Jennifer Buehler's avatar
Jennifer Buehler committed
761
762
763
764
765
766
            
            gazebo::math::Matrix4 worldToContact=gazebo::math::Matrix4::IDENTITY;
            //we can assume that the contact has identity rotation because we don't care about its orientation.
            //We could always set another rotation here too.
            worldToContact.SetTranslate(avgPos);

767
768
769
770
            // now, worldToLink * contactInLocal = worldToContact
            // hence, contactInLocal = worldToLink.Inv * worldToContact
            gazebo::math::Matrix4 worldToLinkInv = worldToLink.Inverse();
            gazebo::math::Matrix4 contactInLocal = worldToLinkInv * worldToContact;
Jennifer Buehler's avatar
Jennifer Buehler committed
771
772
773
774
775
            gazebo::math::Vector3 contactPosInLocal = contactInLocal.GetTranslation();
            
            //std::cout<<"---------"<<std::endl;    
            //std::cout<<"CNT in loc: "<<contactPosInLocal.x<<","<<contactPosInLocal.y<<","<<contactPosInLocal.z<<std::endl;

776
            /*gazebo::math::Vector3 sDiff=avgPos-linkWorldPose.pos;
Jennifer Buehler's avatar
Jennifer Buehler committed
777
            std::cout<<"SIMPLE trans: "<<sDiff.x<<","<<sDiff.y<<","<<sDiff.z<<std::endl;
778
            std::cout<<"coll world pose: "<<linkWorldPose.pos.x<<", "<<linkWorldPose.pos.y<<", "<<linkWorldPose.pos.z<<std::endl; 
Jennifer Buehler's avatar
Jennifer Buehler committed
779
780
            std::cout<<"contact avg pose: "<<avgPos.x<<", "<<avgPos.y<<", "<<avgPos.z<<std::endl; 

781
782
783
            gazebo::math::Vector3 lX=linkWorldPose.rot.GetXAxis();    
            gazebo::math::Vector3 lY=linkWorldPose.rot.GetYAxis();    
            gazebo::math::Vector3 lZ=linkWorldPose.rot.GetZAxis();    
Jennifer Buehler's avatar
Jennifer Buehler committed
784
    
785
            std::cout<<"World ori: "<<linkWorldPose.rot.x<<","<<linkWorldPose.rot.y<<","<<linkWorldPose.rot.z<<","<<linkWorldPose.rot.w<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
786
787
788
789
            std::cout<<"x axis: "<<lX.x<<","<<lX.y<<","<<lX.z<<std::endl;
            std::cout<<"y axis: "<<lY.x<<","<<lY.y<<","<<lY.z<<std::endl;
            std::cout<<"z axis: "<<lZ.x<<","<<lZ.y<<","<<lZ.z<<std::endl;*/

790
791
792
            // now, get the pose of the object and compute it's relative position to the collision surface.
            gazebo::math::Pose objWorldPose = objCollision->GetLink()->GetWorldPose();
            gazebo::math::Matrix4 worldToObj = objWorldPose.rot.GetAsMatrix4();
Jennifer Buehler's avatar
Jennifer Buehler committed
793
794
            worldToObj.SetTranslate(objWorldPose.pos);
    
795
            gazebo::math::Matrix4 objInLocal = worldToLinkInv * worldToObj;
Jennifer Buehler's avatar
Jennifer Buehler committed
796
797
798
799
            gazebo::math::Vector3 objPosInLocal = objInLocal.GetTranslation();

            {
                boost::mutex::scoped_lock lock(this->mutexContacts);
800
                CollidingPoint& p = this->contacts[collidingObjName][collidingLink];  // inserts new entry if doesn't exist
801
                p.gripperName=gripperOfCollidingLink;
Jennifer Buehler's avatar
Jennifer Buehler committed
802
803
                p.collLink=linkCollision;
                p.collObj=objCollision;
804
                p.force+=avgForce;
Jennifer Buehler's avatar
Jennifer Buehler committed
805
806
807
808
                p.pos+=contactPosInLocal;
                p.objPos+=objPosInLocal;
                p.sum++;
            }
809
            //std::cout<<"Average force of contact= "<<avgForce.x<<", "<<avgForce.y<<", "<<avgForce.z<<" out of "<<force.size()<<" vectors."<<std::endl;
Jennifer Buehler's avatar
Jennifer Buehler committed
810
811
812
813
814
        }
    }
}