Go to the documentation of this file.00001
00002
00003
00004
00005 #ifndef ROBOTICAN_COMMON_GAZEBOGRASPFIX_H
00006 #define ROBOTICAN_COMMON_GAZEBOGRASPFIX_H
00007
00008 #include <boost/bind.hpp>
00009 #include <gazebo/gazebo.hh>
00010 #include <gazebo/physics/physics.hh>
00011 #include <gazebo/common/common.hh>
00012 #include <gazebo/transport/TransportTypes.hh>
00013 #include <stdio.h>
00014 #include <robotican_common/GazeboGraspGripper.h>
00015
00016
00017 namespace gazebo {
00018
00096 class GazeboGraspFix : public ModelPlugin {
00097 public:
00098 GazeboGraspFix();
00099 GazeboGraspFix(physics::ModelPtr _model);
00100 virtual ~GazeboGraspFix();
00101
00105 virtual void OnAttach(const std::string& objectName, const std::string& armName){}
00109 virtual void OnDetach(const std::string& objectName, const std::string& armName){}
00110
00111 private:
00112 virtual void Init();
00113 virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00136 void OnUpdate();
00137
00138 void InitValues();
00139
00140
00153 void OnContact(const ConstContactsPtr& ptr);
00154
00160 bool checkGrip(const std::vector<math::Vector3>& forces, float minAngleDiff, float lengthRatio);
00161
00162 bool isGripperLink(const std::string& linkName, std::string& gripperName) const;
00163
00167 std::map<std::string, std::string> getAttachedObjects() const;
00168
00173 class ObjectContactInfo;
00174
00178 bool objectAttachedToGripper(const ObjectContactInfo& objContInfo, std::string& attachedToGripper) const;
00179
00183 bool objectAttachedToGripper(const std::string& gripperName, std::string& attachedToGripper) const;
00184
00185
00186
00187
00188 physics::WorldPtr world;
00189
00190
00191 std::map<std::string, GazeboGraspGripper> grippers;
00192
00193 event::ConnectionPtr update_connection;
00194 transport::NodePtr node;
00195 transport::SubscriberPtr contactSub;
00196
00197
00198
00199 float forcesAngleTolerance;
00200
00201
00202
00203 bool disableCollisionsOnAttach;
00204
00205
00206
00207
00208
00209 std::map<std::string, std::string> collisions;
00210
00211
00215 class CollidingPoint;
00216
00217
00218
00219 std::map<std::string, std::map<std::string, CollidingPoint> > contacts;
00220 boost::mutex mutexContacts;
00221
00222
00223
00224
00225
00226
00227
00228 std::map<std::string, std::map<std::string, CollidingPoint> > attachGripContacts;
00229
00230
00231
00232
00233
00234
00235 std::map<std::string, int> gripCounts;
00236
00237
00238 int maxGripCount;
00239
00240
00241
00242 int gripCountThreshold;
00243
00244
00245
00246
00247 float releaseTolerance;
00248
00249
00250 common::Time updateRate;
00251
00252
00253 common::Time prevUpdateTime;
00254 };
00255
00256 }
00257
00258
00259 #endif //ROBOTICAN_COMMON_GAZEBOGRASPFIX_H