GazeboGraspGripper.cpp 8.66 KB
Newer Older
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

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

using gazebo::GazeboGraspGripper;

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

20
///////////////////////////////////////////////////////////////////////////////
21
GazeboGraspGripper::GazeboGraspGripper():
22
  attached(false)
23
24
25
{
}

26
27
28
29
30
31
32
33
34
35
36
///////////////////////////////////////////////////////////////////////////////
GazeboGraspGripper::GazeboGraspGripper(const GazeboGraspGripper &o):
  model(o.model),
  gripperName(o.gripperName),
  linkNames(o.linkNames),
  collisionElems(o.collisionElems),
  fixedJoint(o.fixedJoint),
  palmLink(o.palmLink),
  disableCollisionsOnAttach(o.disableCollisionsOnAttach),
  attached(o.attached),
  attachedObjName(o.attachedObjName)
37
38
{}

39
40
41
42
///////////////////////////////////////////////////////////////////////////////
GazeboGraspGripper::~GazeboGraspGripper()
{
  this->model.reset();
43
44
}

45
46
47
48
49
50
51
///////////////////////////////////////////////////////////////////////////////
bool GazeboGraspGripper::Init(physics::ModelPtr &_model,
                              const std::string &_gripperName,
                              const std::string &palmLinkName,
                              const std::vector<std::string> &fingerLinkNames,
                              bool _disableCollisionsOnAttach,
                              std::map<std::string, physics::CollisionPtr> &_collisionElems)
52
{
53
54
55
56
  this->gripperName = _gripperName;
  this->attached = false;
  this->disableCollisionsOnAttach = _disableCollisionsOnAttach;
  this->model = _model;
Jennifer Buehler's avatar
Jennifer Buehler committed
57
58
  physics::PhysicsEnginePtr physics =
    gazebo::GetPhysics(this->model->GetWorld());
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
  this->fixedJoint = physics->CreateJoint("revolute");

  this->palmLink = this->model->GetLink(palmLinkName);
  if (!this->palmLink)
  {
    gzerr << "GazeboGraspGripper: Palm link " << palmLinkName <<
          " not found. The gazebo grasp plugin will not work." << std::endl;
    return false;
  }
  for (std::vector<std::string>::const_iterator fingerIt =
         fingerLinkNames.begin();
       fingerIt != fingerLinkNames.end(); ++fingerIt)
  {
    physics::LinkPtr link = this->model->GetLink(*fingerIt);
    //gzmsg<<"Got link "<<fingerLinkElem->Get<std::string>()<<std::endl;
    if (!link.get())
75
    {
76
77
78
79
      gzerr << "GazeboGraspGripper ERROR: Link " << *fingerIt <<
            " can't be found in gazebo for GazeboGraspGripper model plugin. Skipping." <<
            std::endl;
      continue;
80
    }
81
    for (unsigned int j = 0; j < link->GetChildCount(); ++j)
82
    {
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
      physics::CollisionPtr collision = link->GetCollision(j);
      std::string collName = collision->GetScopedName();
      //collision->SetContactsEnabled(true);
      std::map<std::string, physics::CollisionPtr>::iterator collIter =
        collisionElems.find(collName);
      if (collIter !=
          this->collisionElems.end())   //this collision was already added before
      {
        gzwarn << "GazeboGraspGripper: Adding Gazebo collision link element " <<
               collName << " multiple times, the gazebo grasp handler may not work properly" <<
               std::endl;
        continue;
      }
      this->collisionElems[collName] = collision;
      _collisionElems[collName] = collision;
98
    }
99
100
  }
  return !this->collisionElems.empty();
101
102
}

103
104
///////////////////////////////////////////////////////////////////////////////
const std::string &GazeboGraspGripper::getGripperName() const
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
105
{
106
  return gripperName;
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
107
108
}

109
110
///////////////////////////////////////////////////////////////////////////////
bool GazeboGraspGripper::hasLink(const std::string &linkName) const
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
111
{
112
113
114
115
116
117
  for (std::vector<std::string>::const_iterator it = linkNames.begin();
       it != linkNames.end(); ++it)
  {
    if (*it == linkName) return true;
  }
  return false;
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
118
119
}

120
121
///////////////////////////////////////////////////////////////////////////////
bool GazeboGraspGripper::hasCollisionLink(const std::string &linkName) const
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
122
{
123
  return collisionElems.find(linkName) != collisionElems.end();
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
124
125
126
}


127
///////////////////////////////////////////////////////////////////////////////
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
128
129
bool GazeboGraspGripper::isObjectAttached() const
{
130
  return attached;
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
131
132
}

133
134
///////////////////////////////////////////////////////////////////////////////
const std::string &GazeboGraspGripper::attachedObject() const
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
135
{
136
  return attachedObjName;
Jennifer Buehler's avatar
tidy up    
Jennifer Buehler committed
137
138
139
140
}



141
///////////////////////////////////////////////////////////////////////////////
142
// #define USE_MODEL_ATTACH // this only works if the object is a model in itself, which is usually not
143
144
145
// the case. Leaving this in here anyway for documentation of what has been
// tried, and for and later re-evaluation.
bool GazeboGraspGripper::HandleAttach(const std::string &objName)
146
{
147
148
149
150
151
152
153
154
155
156
157
158
  if (!this->palmLink)
  {
    gzwarn << "GazeboGraspGripper: palm link not found, not enforcing grasp hack!\n"
           << std::endl;
    return false;
  }
  physics::WorldPtr world = this->model->GetWorld();
  if (!world.get())
  {
    gzerr << "GazeboGraspGripper: world is NULL" << std::endl;
    return false;
  }
159
#ifdef USE_MODEL_ATTACH
160
161
162
163
164
165
166
167
168
169
  physics::ModelPtr obj = world->GetModel(objName);
  if (!obj.get())
  {
    std::cerr << "ERROR: Object ModelPtr " << objName <<
              " not found in world, can't attach it" << std::endl;
    return false;
  }
  gazebo::math::Pose diff = obj->GetLink()->GetWorldPose() -
                            this->palmLink->GetWorldPose();
  this->palmLink->AttachStaticModel(obj, diff);
170
#else
Jennifer Buehler's avatar
Jennifer Buehler committed
171
172
  physics::CollisionPtr obj =
    boost::dynamic_pointer_cast<physics::Collision>(gazebo::GetEntityByName(world, objName));
173
174
175
176
177
178
  if (!obj.get())
  {
    std::cerr << "ERROR: Object " << objName <<
              " not found in world, can't attach it" << std::endl;
    return false;
  }
Jennifer Buehler's avatar
Jennifer Buehler committed
179
180
  gazebo::GzPose3 diff = gazebo::GetWorldPose(obj->GetLink()) -
                         gazebo::GetWorldPose(this->palmLink);
181
182
  this->fixedJoint->Load(this->palmLink, obj->GetLink(), diff);
  this->fixedJoint->Init();
Jennifer Buehler's avatar
Jennifer Buehler committed
183
184
185
186
#if GAZEBO_MAJOR_VERSION >= 8
  this->fixedJoint->SetUpperLimit(0, 0);
  this->fixedJoint->SetLowerLimit(0, 0);
#else
187
188
  this->fixedJoint->SetHighStop(0, 0);
  this->fixedJoint->SetLowStop(0, 0);
Jennifer Buehler's avatar
Jennifer Buehler committed
189
190
#endif

191
192
193
194
195
196
  if (this->disableCollisionsOnAttach)
  {
    // we can disable collisions of the grasped object, because when the fingers keep colliding with
    // it, the fingers keep wobbling, which can create difficulties when moving the arm.
    obj->GetLink()->SetCollideMode("none");
  }
197
#endif  // USE_MODEL_ATTACH
198
199
200
  this->attached = true;
  this->attachedObjName = objName;
  return true;
201
202
}

203
204
///////////////////////////////////////////////////////////////////////////////
void GazeboGraspGripper::HandleDetach(const std::string &objName)
205
{
206
207
208
209
210
211
  physics::WorldPtr world = this->model->GetWorld();
  if (!world.get())
  {
    gzerr << "GazeboGraspGripper: world is NULL" << std::endl << std::endl;
    return;
  }
212
#ifdef USE_MODEL_ATTACH
213
214
215
216
217
218
219
220
  physics::ModelPtr obj = world->GetModel(objName);
  if (!obj.get())
  {
    std::cerr << "ERROR: Object ModelPtr " << objName <<
              " not found in world, can't detach it" << std::endl;
    return;
  }
  this->palmLink->DetachStaticModel(objName);
221
#else
222
  physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>
Jennifer Buehler's avatar
Jennifer Buehler committed
223
                              (gazebo::GetEntityByName(world, objName));
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
  if (!obj.get())
  {
    std::cerr << "ERROR: Object " << objName <<
              " not found in world, can't attach it" << std::endl;
    return;
  }
  else if (this->disableCollisionsOnAttach)
  {
    obj->GetLink()->SetCollideMode("all");
  }

  // TODO: remove this test print, for issue #26 ------------------- 
#if 0
  if (obj && obj->GetLink())
  {
239
    auto linVel = GetWorldVelocity(obj->GetLink());
240
    gzmsg << "PRE-DETACH Velocity for link " << obj->GetLink()->GetName()
241
242
          << " (collision name " << objName << "): " << linVel
          << ", absolute val " << GetLength(linVel) << std::endl;
243
244
245
246
247
248
249
250
251
252
  }
#endif
  // ------------------- 

  this->fixedJoint->Detach();

  // TODO: remove this test print, for issue #26 ------------------- 
#if 0
  if (obj && obj->GetLink())
  {
253
    auto linVel = GetWorldVelocity(obj->GetLink());
Jennifer Buehler's avatar
Jennifer Buehler committed
254
    gzmsg << "POST-DETACH Velocity for link " << obj->GetLink()->GetName()
255
256
257
          << " (collision name " << objName << "): " << linVel
          << ", absolute val " << GetLength(linVel) << std::endl;

258
259
260
261
  }
#endif
  // ------------------- 

262
#endif  // USE_MODEL_ATTACH
263
264
  this->attached = false;
  this->attachedObjName = "";
265
}