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
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
  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())
  {
    auto linVel = obj->GetLink()->GetWorldLinearVel();
    gzmsg << "PRE-DETACH Velocity for link " << obj->GetLink()->GetName()
          << " (collision name " << objName << "): "
          << linVel << ", absolute val " << linVel.GetLength() << std::endl;
  }
#endif
  // ------------------- 

  this->fixedJoint->Detach();

  // TODO: remove this test print, for issue #26 ------------------- 
#if 0
  if (obj && obj->GetLink())
  {
    auto linVel = obj->GetLink()->GetWorldLinearVel();
    gzmsg << "POST-DETACH Velocity for link " << obj->GetLink()->GetName()
          << " (collision name " << objName << "): "
          << linVel << ", absolute val " << linVel.GetLength() << std::endl;
  }
#endif
  // ------------------- 

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