GazeboGripper.cpp
Go to the documentation of this file.
00001 #include "../include/GazeboGripper.h"
00002 #include "physics/Model.hh"
00003 #include "physics/Joint.hh"
00004 #include "physics/World.hh"
00005 #include "physics/Link.hh"
00006 #include "physics/PhysicsEngine.hh"
00007 #include "physics/Collision.hh"
00008 #include "physics/Contact.hh"
00009 
00010 using namespace gazebo;
00011 
00012 GazeboGripper::GazeboGripper()
00013     : ModelPlugin()
00014     , attached(false)
00015     , contactDuration(0.)
00016     , nonContactDuration(0.)
00017 {
00018 }
00019 
00020 GazeboGripper::~GazeboGripper()
00021 {
00022 }
00023 
00024 void GazeboGripper::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00025 {
00026     // Store the pointer to the model
00027     modelPtr = _model;
00028 
00029     prevUpdateTime = _model->GetWorld()->GetSimTime();
00030     prevContactUpdateTime = prevUpdateTime;
00031 
00032     // get params
00033     if (!_sdf->HasElement("updateRate"))
00034     {
00035         ROS_INFO("GazeboGripper plugin missing <updateRate>, default to 1000");
00036         updateTime = 1/1000;
00037     }
00038     else
00039     {
00040         updateTime = 1/(_sdf->GetElement("updateRate")->GetValueDouble());
00041     }
00042 
00043     if (!_sdf->HasElement("attachWait"))
00044     {
00045         ROS_FATAL("GazeboGripper plugin missing <attachWait>, cannot proceed");
00046         return;
00047     }
00048     else
00049     {
00050         attachWait = _sdf->GetElement("attachWait")->GetValueDouble();
00051     }
00052 
00053     if (!_sdf->HasElement("detachWait"))
00054     {
00055         ROS_FATAL("GazeboGripper plugin missing <detachWait>, cannot proceed");
00056         return;
00057     }
00058     else
00059     {
00060         detachWait = _sdf->GetElement("detachWait")->GetValueDouble();
00061     }
00062 
00063     if (!_sdf->HasElement("maxRelativeMotionRate"))
00064     {
00065         ROS_FATAL("GazeboGripper plugin missing <maxRelativeMotionRate>, cannot proceed");
00066         return;
00067     }
00068     else
00069     {
00070         maxRelativeMotionRate = _sdf->GetElement("maxRelativeMotionRate")->GetValueDouble();
00071     }
00072 
00073     if (!_sdf->HasElement("gripperAttachLink"))
00074     {
00075         ROS_FATAL("GazeboGripper plugin missing <gripperAttachLink>, cannot proceed");
00076         return;
00077     }
00078     else
00079     {
00080         gripperAttachLink = _sdf->GetElement("gripperAttachLink")->GetValueString();
00081         if (!modelPtr->GetLink(gripperAttachLink))
00082         {
00083             ROS_FATAL((std::string("GazeboGripper plugin couldn't find gripperAttachLink (%s) in model, cannot proceed.\n")
00084                                    + "\tLinks connected with static joints may be combined into one by the URDF parser.").c_str(),
00085                        gripperAttachLink.c_str());
00086             return;
00087         }
00088         ROS_DEBUG("GazeboGripper plugin gripperAttachLink: %s", gripperAttachLink.c_str());
00089     }
00090 
00091     if (!_sdf->HasElement("gripperLink"))
00092     {
00093         ROS_FATAL("GazeboGripper plugin missing <gripperLink>, cannot proceed");
00094         return;
00095     }
00096 
00097     std::vector<physics::LinkPtr> links;
00098     sdf::ElementPtr linkElem = _sdf->GetElement("gripperLink");
00099     while (linkElem)
00100     {
00101         physics::LinkPtr link;
00102         if (link = modelPtr->GetLink(linkElem->GetValueString()))
00103         {
00104             links.push_back(link);
00105             ROS_DEBUG("GazeboGripper plugin added gripperLink: %s", linkElem->GetValueString().c_str());
00106         }
00107         else
00108         {
00109             ROS_FATAL((std::string("GazeboGripper plugin couldn't find gripperLink (%s) in model, cannot proceed.\n")
00110                                    + "\tLinks connected with static joints may be combined into one by the URDF parser.").c_str(),
00111                        linkElem->GetValueString().c_str());
00112             return;
00113         }
00114 
00115         linkElem = linkElem->GetNextElement("gripperLink");
00116     }
00117 
00118     if (links.size() < 2)
00119     {
00120         ROS_FATAL("GazeboGripper plugin requires at least two gripperLinks, cannot proceed");
00121         return;
00122     }
00123 
00124     fixedJointPtr = modelPtr->GetWorld()->GetPhysicsEngine()->CreateJoint("revolute");
00125 
00126     physics::CollisionPtr collision;
00127     std::map<std::string, physics::CollisionPtr>::iterator collIter;
00128 
00129     for (std::vector<physics::LinkPtr>::iterator linkIt = links.begin(); linkIt != links.end(); ++linkIt)
00130     {
00131         for (unsigned int j = 0; j < (*linkIt)->GetChildCount(); ++j)
00132         {
00133             collision = (*linkIt)->GetCollision(j);
00134             collIter = collisionPtrs.find(collision->GetScopedName());
00135             if (collIter != collisionPtrs.end())
00136             {
00137                 continue;
00138             }
00139 
00140             collision->SetContactsEnabled(true);
00141             connectionPtrs.push_back(collision->ConnectContact(boost::bind(&GazeboGripper::onContact, this, _1, _2)));
00142             collisionPtrs[collision->GetScopedName()] = collision;
00143             ROS_DEBUG("GazeboGripper plugin added collision: %s", collision->GetScopedName().c_str());
00144         }
00145     }
00146 
00147     this->connectionPtrs.push_back(event::Events::ConnectWorldUpdateEnd(boost::bind(&GazeboGripper::onUpdate, this)));
00148 
00149     ROS_INFO("Gazebo Gripper plugin loaded");
00150 }
00151 
00152 void GazeboGripper::Init()
00153 {
00154     ROS_INFO("Gazebo Gripper plugin initialized");
00155 }
00156 
00157 void GazeboGripper::onUpdate()
00158 {
00159     common::Time currTime = modelPtr->GetWorld()->GetSimTime();
00160 
00161     if ((currTime - prevUpdateTime).Double() < updateTime)
00162     {
00163         return;
00164     }
00165 
00166     handleContact();
00167 
00168     prevUpdateTime = currTime;
00169 }
00170 
00171 void GazeboGripper::handleContact()
00172 {
00173     common::Time currTime = modelPtr->GetWorld()->GetSimTime();
00174     double elapsedTime = (currTime - prevContactUpdateTime).Double();
00175     prevContactUpdateTime = currTime;
00176 
00177     if (contacts.size() >= 2)
00178     {
00179         nonContactDuration = 0;
00180         contactDuration += elapsedTime;
00181         ROS_DEBUG("GazeboGripper plugin contact detected. Contacts: %d, duration: %f", (int)contacts.size(), contactDuration);
00182 
00183         if (!attached)
00184         {
00185             // check to make sure there is no relative movement for attachWait time
00186             std::map<std::string, physics::Collision*> cc;
00187             std::map<std::string, int> contactCounts;
00188             std::map<std::string, int>::iterator iter;
00189 
00190             // identify gripper links colliding with each external part
00191             for (unsigned int i = 0; i < contacts.size(); ++i)
00192             {
00193                 std::string name1 = contacts[i].collision1->GetScopedName();
00194                 std::string name2 = contacts[i].collision2->GetScopedName();
00195 
00196                 if (collisionPtrs.find(name1) == collisionPtrs.end())
00197                 {
00198                     cc[name1] = contacts[i].collision1;
00199                     contactCounts[name1] += 1;
00200                 }
00201                 if (collisionPtrs.find(name2) == collisionPtrs.end())
00202                 {
00203                     cc[name2] = contacts[i].collision2;
00204                     contactCounts[name2] += 1;
00205                 }
00206             }
00207 
00208             // verify 2 gripper links touching part before attaching
00209             iter = contactCounts.begin();
00210             while (iter != contactCounts.end())
00211             {
00212                 if (iter->second < 2)
00213                 {
00214                     contactCounts.erase(iter++);
00215                 }
00216                 else
00217                 {
00218                     math::Pose diff = cc[iter->first]->GetLink()->GetWorldPose() - modelPtr->GetLink(gripperAttachLink)->GetWorldPose();
00219 
00220                     double relMotionRate; // find relative motion rate
00221                     if (contactDuration <= elapsedTime)
00222                     {
00223                         // first time in
00224                         relMotionRate = 0;
00225                     }
00226                     else
00227                     {
00228                         relMotionRate = (diff - prevDiff).pos.GetSquaredLength() / elapsedTime;
00229                     }
00230                     prevDiff = diff;
00231 
00232                     if (relMotionRate > maxRelativeMotionRate)
00233                     {
00234                         // moving too fast
00235                         ROS_DEBUG("GazeboGripper plugin gripper contact relative motion too great (%f).", relMotionRate);
00236                         contactDuration = 0;
00237                     }
00238                     else if (contactDuration >= attachWait)
00239                     {
00240                         handleAttach(cc[iter->first]->GetLink());
00241                         break; // only handle one attachment
00242                     }
00243 
00244                     ++iter;
00245                 }
00246             }
00247         }
00248     }
00249     else
00250     {
00251         contactDuration = 0;
00252 
00253         if (attached)
00254         {
00255             // track time before detach
00256             nonContactDuration += elapsedTime;
00257 
00258             if (nonContactDuration >= detachWait)
00259             {
00260                 handleDetach();
00261             }
00262         }
00263     }
00264 
00265     contacts.clear();
00266 }
00267 
00268 void GazeboGripper::handleAttach(physics::LinkPtr linkPtr)
00269 {
00270     attached = true;
00271 
00272     fixedJointPtr->Load(modelPtr->GetLink(gripperAttachLink), linkPtr, math::Pose(0, 0, 0, 0, 0, 0));
00273     fixedJointPtr->Init();
00274     fixedJointPtr->SetHighStop(0, 0);
00275     fixedJointPtr->SetLowStop(0, 0);
00276 
00277     ROS_INFO("GazeboGripper attached to %s", linkPtr->GetName().c_str());
00278 }
00279 
00280 void GazeboGripper::handleDetach()
00281 {
00282     attached = false;
00283     fixedJointPtr->Detach();
00284     ROS_INFO("GazeboGripper detached");
00285 }
00286 
00287 void GazeboGripper::onContact(const std::string& _collisionName, const physics::Contact& _contact)
00288 {
00289     contacts.push_back(_contact);
00290 }
00291 
00292 GZ_REGISTER_MODEL_PLUGIN(GazeboGripper)


gazebo_gripper
Author(s): rctaylo2
autogenerated on Thu Jan 2 2014 11:31:47