Go to the documentation of this file.00001
00002
00003
00004
00005 #include <boost/bind.hpp>
00006 #include <gazebo/gazebo.hh>
00007 #include <gazebo/physics/physics.hh>
00008 #include <gazebo/physics/ContactManager.hh>
00009 #include <gazebo/physics/Contact.hh>
00010 #include <gazebo/common/common.hh>
00011 #include <stdio.h>
00012
00013 #include <robotican_common/GazeboGraspGripper.h>
00014
00015 using gazebo::GazeboGraspGripper;
00016
00017 #define DEFAULT_FORCES_ANGLE_TOLERANCE 120
00018 #define DEFAULT_UPDATE_RATE 5
00019 #define DEFAULT_MAX_GRIP_COUNT 10
00020 #define DEFAULT_RELEASE_TOLERANCE 0.005
00021 #define DEFAULT_DISABLE_COLLISIONS_ON_ATTACH false
00022
00023 GazeboGraspGripper::GazeboGraspGripper():
00024 attached(false)
00025 {
00026 }
00027
00028 GazeboGraspGripper::GazeboGraspGripper(const GazeboGraspGripper& o):
00029 model(o.model),
00030 gripperName(o.gripperName),
00031 linkNames(o.linkNames),
00032 collisionElems(o.collisionElems),
00033 fixedJoint(o.fixedJoint),
00034 palmLink(o.palmLink),
00035 disableCollisionsOnAttach(o.disableCollisionsOnAttach),
00036 attached(o.attached),
00037 attachedObjName(o.attachedObjName)
00038 {}
00039
00040 GazeboGraspGripper::~GazeboGraspGripper() {
00041 this->model.reset();
00042 }
00043
00044
00045
00046 bool GazeboGraspGripper::Init(physics::ModelPtr& _model,
00047 const std::string& _gripperName,
00048 const std::string& palmLinkName,
00049 const std::vector<std::string>& fingerLinkNames,
00050 bool _disableCollisionsOnAttach,
00051 std::map<std::string, physics::CollisionPtr>& _collisionElems)
00052 {
00053 this->gripperName=_gripperName;
00054 this->attached = false;
00055 this->disableCollisionsOnAttach = _disableCollisionsOnAttach;
00056 this->model = _model;
00057 physics::PhysicsEnginePtr physics = this->model->GetWorld()->GetPhysicsEngine();
00058 this->fixedJoint = physics->CreateJoint("revolute");
00059
00060 this->palmLink = this->model->GetLink(palmLinkName);
00061 if (!this->palmLink)
00062 {
00063 gzerr << "GazeboGraspGripper: Palm link "<<palmLinkName<<" not found. The gazebo grasp plugin will not work."<<std::endl;
00064 return false;
00065 }
00066 for (std::vector<std::string>::const_iterator fingerIt=fingerLinkNames.begin();
00067 fingerIt!=fingerLinkNames.end(); ++fingerIt)
00068 {
00069 physics::LinkPtr link = this->model->GetLink(*fingerIt);
00070
00071 if (!link.get()){
00072 gzerr << "GazeboGraspGripper ERROR: Link "<<*fingerIt<<" can't be found in gazebo for GazeboGraspGripper model plugin. Skipping."<<std::endl;
00073 continue;
00074 }
00075 for (unsigned int j = 0; j < link->GetChildCount(); ++j)
00076 {
00077 physics::CollisionPtr collision = link->GetCollision(j);
00078 std::string collName = collision->GetScopedName();
00079
00080 std::map<std::string, physics::CollisionPtr>::iterator collIter = collisionElems.find(collName);
00081 if (collIter != this->collisionElems.end()) {
00082 gzwarn <<"GazeboGraspGripper: Adding Gazebo collision link element "<<collName<<" multiple times, the gazebo grasp handler may not work properly"<<std::endl;
00083 continue;
00084 }
00085 this->collisionElems[collName] = collision;
00086 _collisionElems[collName] = collision;
00087 }
00088 }
00089 return !this->collisionElems.empty();
00090 }
00091
00092
00093
00094 const std::string& GazeboGraspGripper::getGripperName() const
00095 {
00096 return gripperName;
00097 }
00098
00099 bool GazeboGraspGripper::hasLink(const std::string& linkName) const
00100 {
00101 for (std::vector<std::string>::const_iterator it=linkNames.begin(); it!=linkNames.end(); ++it)
00102 {
00103 if (*it==linkName) return true;
00104 }
00105 return false;
00106 }
00107
00108 bool GazeboGraspGripper::hasCollisionLink(const std::string& linkName) const
00109 {
00110 return collisionElems.find(linkName) != collisionElems.end();
00111 }
00112
00113
00114 bool GazeboGraspGripper::isObjectAttached() const
00115 {
00116 return attached;
00117 }
00118
00119 const std::string& GazeboGraspGripper::attachedObject() const
00120 {
00121 return attachedObjName;
00122 }
00123
00124
00125
00126
00127
00128
00129 bool GazeboGraspGripper::HandleAttach(const std::string& objName)
00130 {
00131 if (!this->palmLink) {
00132 gzwarn << "GazeboGraspGripper: palm link not found, not enforcing grasp hack!\n"<<std::endl;
00133 return false;
00134 }
00135 physics::WorldPtr world = this->model->GetWorld();
00136 if (!world.get())
00137 {
00138 gzerr << "GazeboGraspGripper: world is NULL"<<std::endl;
00139 return false;
00140 }
00141 #ifdef USE_MODEL_ATTACH
00142 physics::ModelPtr obj = world->GetModel(objName);
00143 if (!obj.get()){
00144 std::cerr<<"ERROR: Object ModelPtr "<<objName<<" not found in world, can't attach it"<<std::endl;
00145 return false;
00146 }
00147 gazebo::math::Pose diff = obj->GetLink()->GetWorldPose() - this->palmLink->GetWorldPose();
00148 this->palmLink->AttachStaticModel(obj,diff);
00149 #else
00150 physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>(world->GetEntity(objName));
00151 if (!obj.get()){
00152 std::cerr<<"ERROR: Object "<<objName<<" not found in world, can't attach it"<<std::endl;
00153 return false;
00154 }
00155 gazebo::math::Pose diff = obj->GetLink()->GetWorldPose() - this->palmLink->GetWorldPose();
00156 this->fixedJoint->Load(this->palmLink,obj->GetLink(), diff);
00157 this->fixedJoint->Init();
00158 this->fixedJoint->SetHighStop(0, 0);
00159 this->fixedJoint->SetLowStop(0, 0);
00160 if (this->disableCollisionsOnAttach) {
00161
00162
00163 obj->GetLink()->SetCollideMode("none");
00164 }
00165 #endif // USE_MODEL_ATTACH
00166 this->attached=true;
00167 this->attachedObjName=objName;
00168 return true;
00169 }
00170
00171 void GazeboGraspGripper::HandleDetach(const std::string& objName)
00172 {
00173 physics::WorldPtr world = this->model->GetWorld();
00174 if (!world.get())
00175 {
00176 gzerr << "GazeboGraspGripper: world is NULL"<<std::endl<<std::endl;
00177 return;
00178 }
00179 #ifdef USE_MODEL_ATTACH
00180 physics::ModelPtr obj = world->GetModel(objName);
00181 if (!obj.get()){
00182 std::cerr<<"ERROR: Object ModelPtr "<<objName<<" not found in world, can't detach it"<<std::endl;
00183 return;
00184 }
00185 this->palmLink->DetachStaticModel(objName);
00186 #else
00187 physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>(world->GetEntity(objName));
00188 if (!obj.get()){
00189 std::cerr<<"ERROR: Object "<<objName<<" not found in world, can't attach it"<<std::endl;
00190 return;
00191 }
00192 else if (this->disableCollisionsOnAttach)
00193 {
00194 obj->GetLink()->SetCollideMode("all");
00195 }
00196 this->fixedJoint->Detach();
00197 #endif // USE_MODEL_ATTACH
00198 this->attached=false;
00199 this->attachedObjName="";
00200 }