diff --git a/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h b/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h index 9ec4757..6b836fb 100644 --- a/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h +++ b/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h @@ -10,7 +10,8 @@ #include // #include -namespace gazebo { +namespace gazebo +{ /** * Inspired by gazebo::physics::Gripper, this plugin fixes an object which is grasped to the @@ -46,7 +47,7 @@ namespace gazebo { * - ```` is the name of this arm. Has to be unique. * - ```` has to be the link to which the finger joints are attached. * - ```` tags have to include -all- link names of the gripper/hand which are used to - * actively grasp objects (these are the links which determine whether a "grasp" exists according to + * actively grasp objects (these are the links which determine whether a "grasp" exists according to * above described criterion). * - ```` is the rate at which all contact points are checked against the "gripping criterion". * Note that in-between such updates, existing contact points may be collected at @@ -86,11 +87,12 @@ namespace gazebo { * - Only one object can be attached per gripper. * - Only partial support for an object cannot be gripped with two grippers (release condition may be * triggered wrongly, or not at all, if two grippers are involved) - * + * * \author Jennifer Buehler - */ -class GazeboGraspFix : public ModelPlugin { -public: + */ +class GazeboGraspFix : public ModelPlugin +{ + public: GazeboGraspFix(); GazeboGraspFix(physics::ModelPtr _model); virtual ~GazeboGraspFix(); @@ -98,21 +100,23 @@ class GazeboGraspFix : public ModelPlugin { /** * Gets called just after the object has been attached to the palm link on \e armName */ - virtual void OnAttach(const std::string& objectName, const std::string& armName){} + virtual void OnAttach(const std::string &objectName, + const std::string &armName) {} /** * Gets called just after the object has been detached to the palm link on \e armName */ - virtual void OnDetach(const std::string& objectName, const std::string& armName){} + virtual void OnDetach(const std::string &objectName, + const std::string &armName) {} -private: - virtual void Init(); + private: + virtual void Init(); virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); /** * Collects for each object all forces which are currently applied on it. * Then, for each object, checks whether of all the forces applied, - * there are opposing forces. This is done by calling checkGrip() with the + * there are opposing forces. This is done by calling CheckGrip() with the * list of all forces applied. - * If checkGrip() returns true, the number of "grip counts" + * If CheckGrip() returns true, the number of "grip counts" * is increased for the holding arm (but grip counts will never exceed \e max_grip_count). * If the number of grip counts for this object exceeds \e grip_count_threshold, * the object is attached by calling GazeboGraspGripper::HandleAttach(object-name), @@ -144,60 +148,63 @@ class GazeboGraspFix : public ModelPlugin { * This "average contact force/origin" for each contact is then added to the \e this->contacts map. * If an entry for this object/link pair already exists, the average force (and its origin) * is *added* to the existing force/origin, and the average count is increased. This is to get - * the average force application over time between link and object. + * the average force application over time between link and object. */ - void OnContact(const ConstContactsPtr& ptr); + void OnContact(const ConstContactsPtr &ptr); /** * Checks whether any two vectors in the set have an angle greater * than minAngleDiff (in rad), and one is at least * lengthRatio (0..1) of the other in it's length. */ - bool checkGrip(const std::vector& forces, float minAngleDiff, float lengthRatio); + bool CheckGrip(const std::vector &forces, float minAngleDiff, + float lengthRatio); - bool isGripperLink(const std::string& linkName, std::string& gripperName) const; + bool IsGripperLink(const std::string &linkName, std::string &gripperName) const; /** * return objects (key) and the gripper (value) to which it is attached */ - std::map getAttachedObjects() const; + std::map GetAttachedObjects() const; /** * Helper class to collect contact info per object. * Forward declaration here. */ class ObjectContactInfo; - + /** * Helper function to determine if object attached to a gripper in ObjectContactInfo. */ - bool objectAttachedToGripper(const ObjectContactInfo& objContInfo, std::string& attachedToGripper) const; + bool ObjectAttachedToGripper(const ObjectContactInfo &objContInfo, + std::string &attachedToGripper) const; /** * Helper function to determine if object attached to this gripper */ - bool objectAttachedToGripper(const std::string& gripperName, std::string& attachedToGripper) const; + bool ObjectAttachedToGripper(const std::string &gripperName, + std::string &attachedToGripper) const; + - // physics::ModelPtr model; // physics::PhysicsEnginePtr physics; physics::WorldPtr world; // sorted by their name, all grippers of the robot std::map grippers; - + event::ConnectionPtr update_connection; transport::NodePtr node; transport::SubscriberPtr contactSub; //subscriber to contact updates - + // tolerance (in degrees) between force vectors to // beconsidered "opposing" float forcesAngleTolerance; - + // when an object is attached, collisions with it may be disabled, in case the // robot still keeps wobbling. bool disableCollisionsOnAttach; - + // all collisions per gazebo collision link (each entry // belongs to a physics::CollisionPtr element). The key // is the collision link name, the value is the gripper name @@ -211,8 +218,8 @@ class GazeboGraspFix : public ModelPlugin { class CollidingPoint; // Contact forces sorted by object name the gripper collides with (first key) - // and the link colliding (second key). - std::map > contacts; + // and the link colliding (second key). + std::map > contacts; boost::mutex mutexContacts; //mutex protects contacts // when an object was first attached, it had these colliding points. @@ -221,18 +228,19 @@ class GazeboGraspFix : public ModelPlugin { // limitation that no two grippers can grasp the object (while it would be // possible, the release condition is tied to only one link, so the object may // not be released properly). - std::map > attachGripContacts; - - - // Records how many subsequent update calls the grip on that object has been recorded - // as "holding". Every loop, if a grip is not recorded, this number decreases. + std::map > + attachGripContacts; + + + // Records how many subsequent update calls the grip on that object has been recorded + // as "holding". Every loop, if a grip is not recorded, this number decreases. // When it reaches \e grip_count_threshold, it will be attached. // The number won't increase above max_grip_count once it has reached that number. - std::map gripCounts; + std::map gripCounts; // *maximum* number in \e gripCounts to be recorded. - int maxGripCount; - + int maxGripCount; + // number of recorded "grips" in the past (in gripCount) which, when it is exceeded, counts // as the object grasped, and when it is lower, as released. int gripCountThreshold; diff --git a/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspGripper.h b/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspGripper.h index 44ec84e..f8fdbb1 100644 --- a/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspGripper.h +++ b/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspGripper.h @@ -8,61 +8,63 @@ #include #include -namespace gazebo { +namespace gazebo +{ /** * \brief Helper class for GazeboGraspFix which holds information for one arm. * Attaches /detaches objects to the palm of this arm. * * \author Jennifer Buehler - */ -class GazeboGraspGripper { -public: + */ +class GazeboGraspGripper +{ + public: GazeboGraspGripper(); - GazeboGraspGripper(const GazeboGraspGripper& o); + GazeboGraspGripper(const GazeboGraspGripper &o); virtual ~GazeboGraspGripper(); - + /** * * \param disableCollisionsOnAttach when an object is attached, collisions with it will be disabled. This is useful * if the robot then still keeps wobbling. - */ - bool Init(physics::ModelPtr& _model, - const std::string& _gripperName, - const std::string& palmLinkName, - const std::vector& fingerLinkNames, - bool _disableCollisionsOnAttach, - std::map& _collisions); + */ + bool Init(physics::ModelPtr &_model, + const std::string &_gripperName, + const std::string &palmLinkName, + const std::vector &fingerLinkNames, + bool _disableCollisionsOnAttach, + std::map &_collisions); - const std::string& getGripperName() const; + const std::string &getGripperName() const; /** * Has the link name (URDF) */ - bool hasLink(const std::string& linkName) const; + bool hasLink(const std::string &linkName) const; /** * Has the collision link name (Gazebo collision element name) */ - bool hasCollisionLink(const std::string& linkName) const; + bool hasCollisionLink(const std::string &linkName) const; - bool isObjectAttached() const; + bool isObjectAttached() const; - const std::string& attachedObject() const; + const std::string &attachedObject() const; /** * \param gripContacts contact forces on the object sorted by the link name colliding. */ - bool HandleAttach(const std::string& objName); - void HandleDetach(const std::string& objName); + bool HandleAttach(const std::string &objName); + void HandleDetach(const std::string &objName); -private: + private: physics::ModelPtr model; // name of the gripper std::string gripperName; - + // names of the gripper links std::vector linkNames; // names and Collision objects of the collision links in Gazebo (scoped names) @@ -80,7 +82,7 @@ class GazeboGraspGripper { // flag holding whether an object is attached. Object name in \e attachedObjName bool attached; // name of the object currently attached. - std::string attachedObjName; + std::string attachedObjName; }; } diff --git a/gazebo_grasp_plugin/src/GazeboGraspFix.cpp b/gazebo_grasp_plugin/src/GazeboGraspFix.cpp index 9c09757..1ed529c 100644 --- a/gazebo_grasp_plugin/src/GazeboGraspFix.cpp +++ b/gazebo_grasp_plugin/src/GazeboGraspFix.cpp @@ -5,7 +5,7 @@ #include #include #include - + #include using gazebo::GazeboGraspFix; @@ -13,213 +13,272 @@ using gazebo::GazeboGraspFix; #define DEFAULT_FORCES_ANGLE_TOLERANCE 120 #define DEFAULT_UPDATE_RATE 5 -#define DEFAULT_MAX_GRIP_COUNT 10 +#define DEFAULT_MAX_GRIP_COUNT 10 #define DEFAULT_RELEASE_TOLERANCE 0.005 #define DEFAULT_DISABLE_COLLISIONS_ON_ATTACH false // Register this plugin with the simulator GZ_REGISTER_MODEL_PLUGIN(GazeboGraspFix) - -GazeboGraspFix::GazeboGraspFix(){ - InitValues(); +//////////////////////////////////////////////////////////////////////////////// +GazeboGraspFix::GazeboGraspFix() +{ + InitValues(); } +//////////////////////////////////////////////////////////////////////////////// GazeboGraspFix::GazeboGraspFix(physics::ModelPtr _model) { - InitValues(); + InitValues(); } +//////////////////////////////////////////////////////////////////////////////// GazeboGraspFix::~GazeboGraspFix() { - this->update_connection.reset(); - if (this->node) this->node->Fini(); - this->node.reset(); + this->update_connection.reset(); + if (this->node) this->node->Fini(); + this->node.reset(); } +//////////////////////////////////////////////////////////////////////////////// void GazeboGraspFix::Init() { - this->prevUpdateTime = common::Time::GetWallTime(); + this->prevUpdateTime = common::Time::GetWallTime(); } +//////////////////////////////////////////////////////////////////////////////// void GazeboGraspFix::InitValues() { #if GAZEBO_MAJOR_VERSION > 2 - gazebo::common::Console::SetQuiet(false); + gazebo::common::Console::SetQuiet(false); #endif - // float timeDiff=0.25; - // this->releaseTolerance=0.005; - // this->updateRate = common::Time(0, common::Time::SecToNano(timeDiff)); - this->prevUpdateTime = common::Time::GetWallTime(); - //float graspedSecs=2; - //this->maxGripCount=floor(graspedSecs/timeDiff); - //this->gripCountThreshold=floor(this->maxGripCount/2); - this->node = transport::NodePtr(new transport::Node()); + // float timeDiff=0.25; + // this->releaseTolerance=0.005; + // this->updateRate = common::Time(0, common::Time::SecToNano(timeDiff)); + this->prevUpdateTime = common::Time::GetWallTime(); + //float graspedSecs=2; + //this->maxGripCount=floor(graspedSecs/timeDiff); + //this->gripCountThreshold=floor(this->maxGripCount/2); + this->node = transport::NodePtr(new transport::Node()); } - +//////////////////////////////////////////////////////////////////////////////// void GazeboGraspFix::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { - std::cout<<"Loading grasp-fix plugin"<world = model->GetWorld(); - - sdf::ElementPtr disableCollisionsOnAttachElem = _sdf->GetElement("disable_collisions_on_attach"); - if (!disableCollisionsOnAttachElem){ - std::cout<<"GazeboGraspFix: Using default "< element specified."<disableCollisionsOnAttach = DEFAULT_DISABLE_COLLISIONS_ON_ATTACH; - } else { - std::string str = disableCollisionsOnAttachElem->Get(); - bool bVal = false; - if ((str=="true") || (str=="1")) bVal = true; - this->disableCollisionsOnAttach = bVal; - std::cout<<"GazeboGraspFix: Using disable_collisions_on_attach "<disableCollisionsOnAttach<GetElement("forces_angle_tolerance"); - if (!forcesAngleToleranceElem){ - std::cout<<"GazeboGraspFix: Using default tolerance of "< element specified."<forcesAngleTolerance = DEFAULT_FORCES_ANGLE_TOLERANCE * M_PI/180; - } else { - this->forcesAngleTolerance = forcesAngleToleranceElem->Get() * M_PI/180; - } - - sdf::ElementPtr updateRateElem = _sdf->GetElement("update_rate"); - double _updateSecs; - if (!updateRateElem){ - std::cout<<"GazeboGraspFix: Using "< element specified."<Get(); - double _updateRate = _rate; - _updateSecs = 1.0/_updateRate; - std::cout<<"GazeboGraspFix: Using update rate "<<_rate<updateRate = common::Time(0, common::Time::SecToNano(_updateSecs)); - - sdf::ElementPtr maxGripCountElem = _sdf->GetElement("max_grip_count"); - if (!maxGripCountElem){ - std::cout<<"GazeboGraspFix: Using "< element specified."<maxGripCount = DEFAULT_MAX_GRIP_COUNT; - } else { - this->maxGripCount = maxGripCountElem->Get(); - std::cout<<"GazeboGraspFix: Using max_grip_count "<maxGripCount<world = model->GetWorld(); + + sdf::ElementPtr disableCollisionsOnAttachElem = + _sdf->GetElement("disable_collisions_on_attach"); + if (!disableCollisionsOnAttachElem) + { + gzmsg << "GazeboGraspFix: Using default " << + DEFAULT_DISABLE_COLLISIONS_ON_ATTACH << + " because no element specified." << + std::endl; + this->disableCollisionsOnAttach = DEFAULT_DISABLE_COLLISIONS_ON_ATTACH; + } + else + { + std::string str = disableCollisionsOnAttachElem->Get(); + bool bVal = false; + if ((str == "true") || (str == "1")) bVal = true; + this->disableCollisionsOnAttach = bVal; + gzmsg << "GazeboGraspFix: Using disable_collisions_on_attach " << + this->disableCollisionsOnAttach << std::endl; + } + + sdf::ElementPtr forcesAngleToleranceElem = + _sdf->GetElement("forces_angle_tolerance"); + if (!forcesAngleToleranceElem) + { + gzmsg << "GazeboGraspFix: Using default tolerance of " << + DEFAULT_FORCES_ANGLE_TOLERANCE << + " because no element specified." << + std::endl; + this->forcesAngleTolerance = DEFAULT_FORCES_ANGLE_TOLERANCE * M_PI / 180; + } + else + { + this->forcesAngleTolerance = + forcesAngleToleranceElem->Get() * M_PI / 180; + } + + sdf::ElementPtr updateRateElem = _sdf->GetElement("update_rate"); + double _updateSecs; + if (!updateRateElem) + { + gzmsg << "GazeboGraspFix: Using " << DEFAULT_UPDATE_RATE << + " because no element specified." << std::endl; + _updateSecs = 1.0 / DEFAULT_UPDATE_RATE; + } + else + { + int _rate = updateRateElem->Get(); + double _updateRate = _rate; + _updateSecs = 1.0 / _updateRate; + gzmsg << "GazeboGraspFix: Using update rate " << _rate << std::endl; + } + this->updateRate = common::Time(0, common::Time::SecToNano(_updateSecs)); + + sdf::ElementPtr maxGripCountElem = _sdf->GetElement("max_grip_count"); + if (!maxGripCountElem) + { + gzmsg << "GazeboGraspFix: Using " << DEFAULT_MAX_GRIP_COUNT << + " because no element specified." << std::endl; + this->maxGripCount = DEFAULT_MAX_GRIP_COUNT; + } + else + { + this->maxGripCount = maxGripCountElem->Get(); + gzmsg << "GazeboGraspFix: Using max_grip_count " + << this->maxGripCount << std::endl; + } + + sdf::ElementPtr gripCountThresholdElem = + _sdf->GetElement("grip_count_threshold"); + if (!gripCountThresholdElem) + { + this->gripCountThreshold = floor(this->maxGripCount / 2.0); + gzmsg << "GazeboGraspFix: Using " << this->gripCountThreshold << + " because no element specified." << + std::endl; + } + else + { + this->gripCountThreshold = gripCountThresholdElem->Get(); + gzmsg << "GazeboGraspFix: Using grip_count_threshold " << + this->gripCountThreshold << std::endl; + } + + sdf::ElementPtr releaseToleranceElem = _sdf->GetElement("release_tolerance"); + if (!releaseToleranceElem) + { + gzmsg << "GazeboGraspFix: Using " << DEFAULT_RELEASE_TOLERANCE << + " because no element specified." << std::endl; + this->releaseTolerance = DEFAULT_RELEASE_TOLERANCE; + } + else + { + this->releaseTolerance = releaseToleranceElem->Get(); + gzmsg << "GazeboGraspFix: Using release_tolerance " << + this->releaseTolerance << std::endl; + } + + // will contain all names of collision entities involved from all arms + std::vector collisionNames; + + sdf::ElementPtr armElem = _sdf->GetElement("arm"); + if (!armElem) + { + gzerr << "GazeboGraspFix: Cannot load the GazeboGraspFix without any declarations" + << std::endl; + return; + } + // add all arms: + for (; armElem != NULL; armElem = armElem->GetNextElement("arm")) + { + sdf::ElementPtr armNameElem = armElem->GetElement("arm_name"); + sdf::ElementPtr handLinkElem = armElem->GetElement("palm_link"); + sdf::ElementPtr fingerLinkElem = armElem->GetElement("gripper_link"); + + if (!handLinkElem || !fingerLinkElem || !armNameElem) + { + gzerr << "ERROR: GazeboGraspFix: Cannot use a GazeboGraspFix arm because " + << "not all of , and elements specified in URDF/SDF. Skipping." + << std::endl; + continue; } - sdf::ElementPtr gripCountThresholdElem = _sdf->GetElement("grip_count_threshold"); - if (!gripCountThresholdElem){ - this->gripCountThreshold=floor(this->maxGripCount/2.0); - std::cout<<"GazeboGraspFix: Using "<gripCountThreshold<<" because no element specified."<gripCountThreshold = gripCountThresholdElem->Get(); - std::cout<<"GazeboGraspFix: Using grip_count_threshold "<gripCountThreshold<Get(); + std::string palmName = handLinkElem->Get(); - sdf::ElementPtr releaseToleranceElem = _sdf->GetElement("release_tolerance"); - if (!releaseToleranceElem){ - std::cout<<"GazeboGraspFix: Using "< element specified."<releaseTolerance = DEFAULT_RELEASE_TOLERANCE; - } else { - this->releaseTolerance = releaseToleranceElem->Get(); - std::cout<<"GazeboGraspFix: Using release_tolerance "<releaseTolerance< fingerLinkNames; + for (; fingerLinkElem != NULL; + fingerLinkElem = fingerLinkElem->GetNextElement("gripper_link")) + { + std::string linkName = fingerLinkElem->Get(); + fingerLinkNames.push_back(linkName); } - // will contain all names of collision entities involved from all arms - std::vector collisionNames; - - sdf::ElementPtr armElem=_sdf->GetElement("arm"); - if (!armElem) + // add new gripper + if (grippers.find(armName) != grippers.end()) { - gzerr <<"GazeboGraspFix: Cannot load the GazeboGraspFix without any declarations"<GetNextElement("arm")) + GazeboGraspGripper &gripper = grippers[armName]; + std::map _collisions; + if (!gripper.Init(_parent, armName, palmName, fingerLinkNames, + disableCollisionsOnAttach, _collisions)) { - sdf::ElementPtr armNameElem = armElem->GetElement("arm_name"); - sdf::ElementPtr handLinkElem = armElem->GetElement("palm_link"); - sdf::ElementPtr fingerLinkElem = armElem->GetElement("gripper_link"); - - if (!handLinkElem || !fingerLinkElem || !armNameElem) { - gzerr << "ERROR: GazeboGraspFix: Cannot use a GazeboGraspFix arm because " - << "not all of , and elements specified in URDF/SDF. Skipping."<Get(); - std::string palmName = handLinkElem->Get(); - - // collect all finger names: - std::vector fingerLinkNames; - for (; fingerLinkElem != NULL; fingerLinkElem = fingerLinkElem->GetNextElement("gripper_link")) - { - std::string linkName = fingerLinkElem->Get(); - fingerLinkNames.push_back(linkName); - } - - // add new gripper - if (grippers.find(armName)!=grippers.end()) - { - gzerr<<"GazeboGraspFix: Arm named "< _collisions; - if (!gripper.Init(_parent, armName, palmName, fingerLinkNames, disableCollisionsOnAttach, _collisions)) - { - gzerr<<"GazeboGraspFix: Could not initialize arm "<::iterator collIt = _collisions.begin(); - collIt != _collisions.end(); ++collIt) - { - const std::string& collName=collIt->first; - //physics::CollisionPtr& coll=collIt->second; - std::map::iterator collIter = this->collisions.find(collName); - if (collIter != this->collisions.end()) { //this collision was already added before - gzwarn <<"GazeboGraspFix: Adding Gazebo collision link element "<collisions[collName] = armName; - collisionNames.push_back(collName); - } + gzerr << "GazeboGraspFix: Could not initialize arm " << armName << ". Skipping." + << std::endl; + grippers.erase(armName); + continue; } - - if (grippers.empty()) + // add all the grippers's collision elements + for (std::map::iterator collIt = + _collisions.begin(); + collIt != _collisions.end(); ++collIt) { - gzerr << "ERROR: GazeboGraspFix: Cannot use a GazeboGraspFix because " - << "no arms were configured successfully. Plugin will not work."<first; + //physics::CollisionPtr& coll=collIt->second; + std::map::iterator collIter = this->collisions.find( + collName); + if (collIter != + this->collisions.end()) //this collision was already added before + { + gzwarn << "GazeboGraspFix: Adding Gazebo collision link element " << collName << + " multiple times, the grasp plugin may not work properly" << std::endl; + continue; + } + gzmsg << "GazeboGraspFix: Adding collision scoped name " << collName << + std::endl; + this->collisions[collName] = armName; + collisionNames.push_back(collName); } - - // ++++++++++++ start up things +++++++++++++++ - - physics::PhysicsEnginePtr physics = this->world->GetPhysicsEngine(); - this->node->Init(this->world->GetName()); - physics::ContactManager * contactManager = physics->GetContactManager(); - contactManager->PublishContacts(); //XXX not sure I need this? - - std::string topic = contactManager->CreateFilter(model->GetScopedName(), collisionNames); - if (!this->contactSub) { - std::cout<<"Subscribing contact manager to topic "<contactSub = this->node->Subscribe(topic,&GazeboGraspFix::OnContact, this, latching); - } - - update_connection=event::Events::ConnectWorldUpdateEnd(boost::bind(&GazeboGraspFix::OnUpdate, this)); + } + + if (grippers.empty()) + { + gzerr << "ERROR: GazeboGraspFix: Cannot use a GazeboGraspFix because " + << "no arms were configured successfully. Plugin will not work." << std::endl; + return; + } + + // ++++++++++++ start up things +++++++++++++++ + + physics::PhysicsEnginePtr physics = this->world->GetPhysicsEngine(); + this->node->Init(this->world->GetName()); + physics::ContactManager *contactManager = physics->GetContactManager(); + contactManager->PublishContacts(); //XXX not sure I need this? + + std::string topic = contactManager->CreateFilter(model->GetScopedName(), + collisionNames); + if (!this->contactSub) + { + gzmsg << "Subscribing contact manager to topic " << topic << std::endl; + bool latching = false; + this->contactSub = this->node->Subscribe(topic, &GazeboGraspFix::OnContact, + this, latching); + } + + update_connection = event::Events::ConnectWorldUpdateEnd(boost::bind( + &GazeboGraspFix::OnUpdate, this)); } - +//////////////////////////////////////////////////////////////////////////////// class GazeboGraspFix::ObjectContactInfo { - public: - + public: + // all forces effecting on the object std::vector appliedForces; @@ -227,82 +286,89 @@ class GazeboGraspFix::ObjectContactInfo // a number counting the number of contact points with the // object per gripper std::map grippersInvolved; - + // maximum number of contacts of *any one* gripper // (any in grippersInvolved) int maxGripperContactCnt; - + // the gripper for maxGripperContactCnt std::string maxContactGripper; }; - - -bool GazeboGraspFix::isGripperLink(const std::string& linkName, std::string& gripperName) const +//////////////////////////////////////////////////////////////////////////////// +bool GazeboGraspFix::IsGripperLink(const std::string &linkName, + std::string &gripperName) const { - for (std::map::const_iterator it=grippers.begin(); it!=grippers.end(); ++it) + for (std::map::const_iterator it = + grippers.begin(); it != grippers.end(); ++it) + { + if (it->second.hasLink(linkName)) { - if (it->second.hasLink(linkName)) - { - gripperName=it->first; - return true; - } + gripperName = it->first; + return true; } - return false; -} + } + return false; +} -std::map GazeboGraspFix::getAttachedObjects() const +//////////////////////////////////////////////////////////////////////////////// +std::map GazeboGraspFix::GetAttachedObjects() const { - std::map ret; - for (std::map::const_iterator it=grippers.begin(); it!=grippers.end(); ++it) + std::map ret; + for (std::map::const_iterator it = + grippers.begin(); it != grippers.end(); ++it) + { + const std::string &gripperName = it->first; + const GazeboGraspGripper &gripper = it->second; + if (gripper.isObjectAttached()) { - const std::string& gripperName = it->first; - const GazeboGraspGripper& gripper = it->second; - if (gripper.isObjectAttached()) - { - ret[gripper.attachedObject()]=gripperName; - } + ret[gripper.attachedObject()] = gripperName; } - return ret; -} - - - - + } + return ret; +} -bool GazeboGraspFix::objectAttachedToGripper(const ObjectContactInfo& objContInfo, std::string& attachedToGripper) const +//////////////////////////////////////////////////////////////////////////////// +bool GazeboGraspFix::ObjectAttachedToGripper(const ObjectContactInfo + &objContInfo, std::string &attachedToGripper) const { - for (std::map::const_iterator gripInvIt=objContInfo.grippersInvolved.begin(); - gripInvIt != objContInfo.grippersInvolved.end(); ++gripInvIt) + for (std::map::const_iterator gripInvIt = + objContInfo.grippersInvolved.begin(); + gripInvIt != objContInfo.grippersInvolved.end(); ++gripInvIt) + { + const std::string &gripperName = gripInvIt->first; + if (ObjectAttachedToGripper(gripperName, attachedToGripper)) { - const std::string& gripperName=gripInvIt->first; - if (objectAttachedToGripper(gripperName, attachedToGripper)) - { - return true; - } + return true; } - return false; + } + return false; } -bool GazeboGraspFix::objectAttachedToGripper(const std::string& gripperName, std::string& attachedToGripper) const +//////////////////////////////////////////////////////////////////////////////// +bool GazeboGraspFix::ObjectAttachedToGripper(const std::string &gripperName, + std::string &attachedToGripper) const { - std::map::const_iterator gIt=grippers.find(gripperName); - if (gIt == grippers.end()) - { - gzerr << "GazeboGraspFix: Inconsistency, gripper "<second; - // std::cout<<"Gripper "<::const_iterator gIt = grippers.find( + gripperName); + if (gIt == grippers.end()) + { + gzerr << "GazeboGraspFix: Inconsistency, gripper " << gripperName << + " not found in GazeboGraspFix grippers" << std::endl; return false; + } + const GazeboGraspGripper &gripper = gIt->second; + // gzmsg<<"Gripper "<prevUpdateTime) < this->updateRate) + return; + + // first, copy all contact data into local struct. Don't do the complex grip check (CheckGrip) + // within the mutex, because that slows down OnContact(). + this->mutexContacts.lock(); + std::map > contPoints( + this->contacts); + this->contacts.clear(); // clear so it can be filled anew by OnContact(). + this->mutexContacts.unlock(); + + // contPoints now contains CollidingPoint objects for each *object* and *link*. + + // Iterate through all contact points to gather all summed forces + // (and other useful information) for all the objects (so we have all forces on one object). + std::map >::iterator objIt; + std::map objectContactInfo; + + for (objIt = contPoints.begin(); objIt != contPoints.end(); ++objIt) + { + std::string objName = objIt->first; + //gzmsg<<"Examining object collisions with "<::iterator lIt; + for (lIt = objIt->second.begin(); lIt != objIt->second.end(); ++lIt) + { + std::string linkName = lIt->first; + CollidingPoint &collP = lIt->second; + gazebo::math::Vector3 avgForce = collP.force / collP.sum; + // gzmsg << "Found collision with "< _maxGripperContactCnt) + { + _maxGripperContactCnt = gContactCnt; + objContInfo.maxContactGripper = collP.gripperName; + } + } + } + + // ++++++++++++++++++++ Handle Attachment +++++++++++++++++++++++++++++++ + + // collect of all objects which are found to be "gripped" at the current stage. + // if they are gripped, increase the grip counter. If the grip count exceeds the + // threshold, attach the object to the gripper which has most contact points with the + // object. + std::set grippedObjects; + for (std::map::iterator ocIt = + objectContactInfo.begin(); + ocIt != objectContactInfo.end(); ++ocIt) + { + const std::string &objName = ocIt->first; + const ObjectContactInfo &objContInfo = ocIt->second; + + // gzmsg<<"Number applied forces on "< + (world->GetEntity(objName)); + if (objColl && objColl->GetLink()) + { + auto linVel = objColl->GetLink()->GetWorldLinearVel(); + gzmsg << "Velocity for link " << objColl->GetLink()->GetName() << " (collision name " << objName << "): " << linVel << ", absolute val " << linVel.GetLength() << std::endl; + } +#endif + // ------------------- + + float minAngleDiff = this->forcesAngleTolerance; //120 * M_PI/180; + if (!CheckGrip(objContInfo.appliedForces, minAngleDiff, 0.3)) + continue; + // add to "gripped objects" + grippedObjects.insert(objName); -void GazeboGraspFix::OnUpdate() { - if ((common::Time::GetWallTime() - this->prevUpdateTime) < this->updateRate) - return; + //gzmsg<<"Grasp Held: "<gripCounts[objName]<mutexContacts.lock(); - std::map > contPoints(this->contacts); - this->contacts.clear(); // clear so it can be filled anew by OnContact(). - this->mutexContacts.unlock(); + int &counts = this->gripCounts[objName]; + if (counts < this->maxGripCount) ++counts; - // contPoints now contains CollidingPoint objects for each *object* and *link*. + // only need to attach object if the grip count threshold is exceeded + if (counts <= this->gripCountThreshold) + continue; + //gzmsg<<"GRIPPING "<gripCountThreshold<<")"< >::iterator objIt; - std::map objectContactInfo; + // find out if any of the grippers involved in the grasp is already grasping the object. + // If there is no such gripper, attach it to the gripper which has most contact points. + std::string attachedToGripper; + bool isAttachedToGripper = ObjectAttachedToGripper(objContInfo, + attachedToGripper); + if (isAttachedToGripper) + { + // the object is already attached to a gripper, so it does not need to be attached. + // gzmsg << "GazeboGraspFix has found that object "<< + // gripper.attachedObject()<<" is already attached to gripper "<::iterator gIt = grippers.find( + graspingGripperName); + if (gIt == grippers.end()) { - std::string objName=objIt->first; - //std::cout<<"Examining object collisions with "<::iterator lIt; - for (lIt=objIt->second.begin(); lIt!=objIt->second.end(); ++lIt){ - std::string linkName=lIt->first; - CollidingPoint& collP=lIt->second; - gazebo::math::Vector3 avgForce=collP.force/collP.sum; - // std::cout << "Found collision with "< _maxGripperContactCnt) - { - _maxGripperContactCnt = gContactCnt; - objContInfo.maxContactGripper=collP.gripperName; - } - } + gzerr << "GazeboGraspFix: Inconsistency, gripper '" << graspingGripperName + << "' not found in GazeboGraspFix grippers, so cannot do attachment of object " + << objName << std::endl; + continue; } - - // ++++++++++++++++++++ Handle Attachment +++++++++++++++++++++++++++++++ - - // collect of all objects which are found to be "gripped" at the current stage. - // if they are gripped, increase the grip counter. If the grip count exceeds the - // threshold, attach the object to the gripper which has most contact points with the - // object. - std::set grippedObjects; - for (std::map::iterator ocIt=objectContactInfo.begin(); - ocIt!=objectContactInfo.end(); ++ocIt) + GazeboGraspGripper &graspingGripper = gIt->second; + + if (graspingGripper.isObjectAttached()) { - const std::string& objName=ocIt->first; - const ObjectContactInfo& objContInfo = ocIt->second; - - // std::cout<<"Number applied forces on "<forcesAngleTolerance; //120 * M_PI/180; - if (!checkGrip(objContInfo.appliedForces, minAngleDiff, 0.3)) - continue; - - // add to "gripped objects" - grippedObjects.insert(objName); - - //std::cout<<"Grasp Held: "<gripCounts[objName]<gripCounts[objName]; - if (counts < this->maxGripCount) ++counts; - - // only need to attach object if the grip count threshold is exceeded - if (counts <= this->gripCountThreshold) - continue; - - //std::cout<<"GRIPPING "<gripCountThreshold<<")"<::iterator gIt=grippers.find(graspingGripperName); - if (gIt == grippers.end()) - { - gzerr << "GazeboGraspFix: Inconsistency, gripper '"<second; - - if (graspingGripper.isObjectAttached()) - { - gzerr<<"GazeboGraspFix has found that object "<< - graspingGripper.attachedObject()<<" is already attached to gripper "<< - graspingGripperName<<", so can't grasp '"<attachGripContacts[objName]=contPoints[objName]; + const std::map &contPointsTmp = + contPoints[objName]; + std::map &attGripConts = + this->attachGripContacts[objName]; + attGripConts.clear(); + std::map::const_iterator contPointsIt; + for (contPointsIt = contPointsTmp.begin(); contPointsIt != contPointsTmp.end(); + ++contPointsIt) + { + const std::string &collidingLink = contPointsIt->first; + const CollidingPoint &collidingPoint = contPointsIt->second; + // gzmsg<<"Checking initial contact with "<attachGripContacts[objName]=contPoints[objName]; - const std::map& contPointsTmp = contPoints[objName]; - std::map& attGripConts = this->attachGripContacts[objName]; - attGripConts.clear(); - std::map::const_iterator contPointsIt; - for (contPointsIt=contPointsTmp.begin(); contPointsIt!=contPointsTmp.end(); ++contPointsIt) - { - const std::string& collidingLink = contPointsIt->first; - const CollidingPoint& collidingPoint = contPointsIt->second; - // std::cout<<"Checking initial contact with "<OnAttach(objName, graspingGripperName); - } // for all objects + if (!graspingGripper.HandleAttach(objName)) + { + gzerr << "GazeboGraspFix: Could not attach object " << objName << " to gripper " + << graspingGripperName << std::endl; + } + this->OnAttach(objName, graspingGripperName); + } // for all objects - // ++++++++++++++++++++ Handle Detachment +++++++++++++++++++++++++++++++ - std::map attachedObjects = getAttachedObjects(); + // ++++++++++++++++++++ Handle Detachment +++++++++++++++++++++++++++++++ + std::map attachedObjects = GetAttachedObjects(); - // now, for all objects that are not currently gripped, - // decrease grip counter, and possibly release object. - std::map::iterator gripCntIt; - for (gripCntIt = this->gripCounts.begin(); gripCntIt != this->gripCounts.end(); ++gripCntIt){ + // now, for all objects that are not currently gripped, + // decrease grip counter, and possibly release object. + std::map::iterator gripCntIt; + for (gripCntIt = this->gripCounts.begin(); gripCntIt != this->gripCounts.end(); + ++gripCntIt) + { - const std::string& objName=gripCntIt->first; + const std::string &objName = gripCntIt->first; - if (grippedObjects.find(objName) != grippedObjects.end()) - { // this object is one we just detected as "gripped", so no need to check for releasing it... - // std::cout<<"NOT considering "<second<<" (threshold "<gripCountThreshold<<")"<second > 0) --(gripCntIt->second); - - std::map::iterator attIt = attachedObjects.find(objName); - bool isAttached = (attIt != attachedObjects.end()); - - // std::cout<<"is attached: "<second > this->gripCountThreshold)) continue; - - const std::string& graspingGripperName=attIt->second; - - // std::cout<<"Considering "< >::iterator initCollIt=this->attachGripContacts.find(objName); - if (initCollIt == this->attachGripContacts.end()) { - std::cerr<<"ERROR: Consistency: Could not find attachGripContacts for "<& initColls=initCollIt->second; - int cntRelease=0; - - // for all links which have initially been detected to collide: - std::map::iterator pointIt; - for (pointIt=initColls.begin(); pointIt!=initColls.end(); ++pointIt) - { - CollidingPoint& cpInfo=pointIt->second; - // initial distance from link to contact point (relative to link) - gazebo::math::Vector3 relContactPos=cpInfo.pos/cpInfo.sum; - // initial distance from link to object (relative to link) - gazebo::math::Vector3 relObjPos=cpInfo.objPos/cpInfo.sum; - - // get current world pose of object - gazebo::math::Pose currObjWorldPose=cpInfo.collObj->GetLink()->GetWorldPose(); - - // get world pose of link - gazebo::math::Pose currLinkWorldPose=cpInfo.collLink->GetLink()->GetWorldPose(); - - // Get transform for currLinkWorldPose as matrix - gazebo::math::Matrix4 worldToLink=currLinkWorldPose.rot.GetAsMatrix4(); - worldToLink.SetTranslate(currLinkWorldPose.pos); - - // Get the transform from collision link to contact point - gazebo::math::Matrix4 linkToContact=gazebo::math::Matrix4::IDENTITY; - linkToContact.SetTranslate(relContactPos); - - // the current world position of the contact point right now is: - gazebo::math::Matrix4 _currContactWorldPose=worldToLink*linkToContact; - gazebo::math::Vector3 currContactWorldPose=_currContactWorldPose.GetTranslation(); - - // the initial contact point location on the link should still correspond - // to the initial contact point location on the object. - - // initial vector from object center to contact point (relative to link, - // because relObjPos and relContactPos are from center of link) - gazebo::math::Vector3 oldObjDist= relContactPos - relObjPos; - // the same vector as \e oldObjDist, but calculated by the current world pose - // of object and the current location of the initial contact location on the link. - gazebo::math::Vector3 newObjDist= currContactWorldPose - currObjWorldPose.pos; // new distance from contact to object - - //std::cout<<"Obj Trans "<GetName()<<": "<GetName()<<": "<GetName()<<": "< releaseTolerance) { - ++cntRelease; - } - } + // the object does not satisfy "gripped" criteria, so potentially has to be released. - if (cntRelease > 0) - { // sufficient links did not meet the criteria to be close enough to the object. - // First, get the grasping gripper: - std::map::iterator gggIt = grippers.find(graspingGripperName); - if (gggIt == grippers.end()) - { - gzerr << "GazeboGraspFix: Inconsistency: Gazebo gripper '"<second; - // Now, detach the object: - std::cout<<"GazeboGraspFix: Detaching "<OnDetach(objName,graspingGripperName); - gripCntIt->second=0; - this->attachGripContacts.erase(initCollIt); - } - } + // gzmsg<<"NOT-GRIPPING "<second<<" (threshold "<gripCountThreshold<<")"<prevUpdateTime = common::Time::GetWallTime(); -} - -double angularDistance(const gazebo::math::Vector3& _v1, const gazebo::math::Vector3& _v2) { - gazebo::math::Vector3 v1=_v1; - gazebo::math::Vector3 v2=_v2; - v1.Normalize(); - v2.Normalize(); - return acos(v1.Dot(v2)); -} + if (gripCntIt->second > 0) --(gripCntIt->second); + + std::map::iterator attIt = attachedObjects.find( + objName); + bool isAttached = (attIt != attachedObjects.end()); + + // gzmsg<<"is attached: "<second > this->gripCountThreshold)) continue; -bool GazeboGraspFix::checkGrip(const std::vector& forces, float minAngleDiff, float lengthRatio){ - if (((lengthRatio > 1) || (lengthRatio < 0)) && (lengthRatio > 1e-04 && (fabs(lengthRatio-1) > 1e-04))) { - std::cerr<<"ERROR: checkGrip: always specify a lengthRatio of [0..1]"<second; + + // gzmsg<<"Considering "< >::iterator + initCollIt = this->attachGripContacts.find(objName); + if (initCollIt == this->attachGripContacts.end()) + { + std::cerr << "ERROR: Consistency: Could not find attachGripContacts for " << + objName << std::endl; + continue; } - if (minAngleDiff < M_PI_2){ - std::cerr<<"ERROR: checkGrip: min angle must be at least 90 degrees (PI/2)"< &initColls = initCollIt->second; + int cntRelease = 0; + + // for all links which have initially been detected to collide: + std::map::iterator pointIt; + for (pointIt = initColls.begin(); pointIt != initColls.end(); ++pointIt) + { + CollidingPoint &cpInfo = pointIt->second; + // initial distance from link to contact point (relative to link) + gazebo::math::Vector3 relContactPos = cpInfo.pos / cpInfo.sum; + // initial distance from link to object (relative to link) + gazebo::math::Vector3 relObjPos = cpInfo.objPos / cpInfo.sum; + + // get current world pose of object + gazebo::math::Pose currObjWorldPose = cpInfo.collObj->GetLink()->GetWorldPose(); + + // get world pose of link + gazebo::math::Pose currLinkWorldPose = + cpInfo.collLink->GetLink()->GetWorldPose(); + + // Get transform for currLinkWorldPose as matrix + gazebo::math::Matrix4 worldToLink = currLinkWorldPose.rot.GetAsMatrix4(); + worldToLink.SetTranslate(currLinkWorldPose.pos); + + // Get the transform from collision link to contact point + gazebo::math::Matrix4 linkToContact = gazebo::math::Matrix4::IDENTITY; + linkToContact.SetTranslate(relContactPos); + + // the current world position of the contact point right now is: + gazebo::math::Matrix4 _currContactWorldPose = worldToLink * linkToContact; + gazebo::math::Vector3 currContactWorldPose = + _currContactWorldPose.GetTranslation(); + + // the initial contact point location on the link should still correspond + // to the initial contact point location on the object. + + // initial vector from object center to contact point (relative to link, + // because relObjPos and relContactPos are from center of link) + gazebo::math::Vector3 oldObjDist = relContactPos - relObjPos; + // the same vector as \e oldObjDist, but calculated by the current world pose + // of object and the current location of the initial contact location on the link. + gazebo::math::Vector3 newObjDist = currContactWorldPose - + currObjWorldPose.pos; // new distance from contact to object + + //gzmsg<<"Obj Trans "<GetName()<<": "<GetName()<<": "<GetName()<<": "< releaseTolerance) + { + ++cntRelease; + } } - std::vector::const_iterator it1, it2; - for (it1=forces.begin(); it1!=forces.end(); ++it1){ - gazebo::math::Vector3 v1=*it1; - for (it2=it1+1; it2!=forces.end(); ++it2){ - gazebo::math::Vector3 v2=*it2; - float l1=v1.GetLength(); - float l2=v2.GetLength(); - if ((l1<1e-04) || (l2<1e-04)) continue; - /*gazebo::math::Vector3 _v1=v1; - gazebo::math::Vector3 _v2=v2; - _v1/=l1; - _v2/=l2; - float angle=acos(_v1.Dot(_v2));*/ - float angle=angularDistance(v1, v2); - // std::cout<<"Angular distance between v1.len="< minAngleDiff) { - float ratio; - if (l1>l2) ratio=l2/l1; - else ratio=l1/l2; - // std::cout<<"Got angle "<= lengthRatio) - { - // std::cout<<"checkGrip() is true"< 0) + { + // sufficient links did not meet the criteria to be close enough to the object. + // First, get the grasping gripper: + std::map::iterator gggIt = grippers.find( + graspingGripperName); + if (gggIt == grippers.end()) + { + gzerr << "GazeboGraspFix: Inconsistency: Gazebo gripper '" << + graspingGripperName << "' not found when checking for detachment" << std::endl; + continue; + } + GazeboGraspGripper &graspingGripper = gggIt->second; + // Now, detach the object: + gzmsg << "GazeboGraspFix: Detaching " << objName << " from gripper " << + graspingGripperName << "." << std::endl; + graspingGripper.HandleDetach(objName); + this->OnDetach(objName, graspingGripperName); + gripCntIt->second = 0; + this->attachGripContacts.erase(initCollIt); } - return false; + } + + this->prevUpdateTime = common::Time::GetWallTime(); } -void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg) +//////////////////////////////////////////////////////////////////////////////// +double angularDistance(const gazebo::math::Vector3 &_v1, + const gazebo::math::Vector3 &_v2) { - //std::cout<<"CONTACT! "<contact_size(); ++i) { - physics::CollisionPtr collision1 = boost::dynamic_pointer_cast( - this->world->GetEntity(_msg->contact(i).collision1())); - physics::CollisionPtr collision2 = boost::dynamic_pointer_cast( - this->world->GetEntity(_msg->contact(i).collision2())); - - if ((collision1 && !collision1->IsStatic()) && (collision2 && !collision2->IsStatic())) + gazebo::math::Vector3 v1 = _v1; + gazebo::math::Vector3 v2 = _v2; + v1.Normalize(); + v2.Normalize(); + return acos(v1.Dot(v2)); +} + +//////////////////////////////////////////////////////////////////////////////// +bool GazeboGraspFix::CheckGrip(const std::vector &forces, + float minAngleDiff, float lengthRatio) +{ + if (((lengthRatio > 1) || (lengthRatio < 0)) && (lengthRatio > 1e-04 + && (fabs(lengthRatio - 1) > 1e-04))) + { + std::cerr << "ERROR: CheckGrip: always specify a lengthRatio of [0..1]" << + std::endl; + return false; + } + if (minAngleDiff < M_PI_2) + { + std::cerr << "ERROR: CheckGrip: min angle must be at least 90 degrees (PI/2)" << + std::endl; + return false; + } + std::vector::const_iterator it1, it2; + for (it1 = forces.begin(); it1 != forces.end(); ++it1) + { + gazebo::math::Vector3 v1 = *it1; + for (it2 = it1 + 1; it2 != forces.end(); ++it2) + { + gazebo::math::Vector3 v2 = *it2; + float l1 = v1.GetLength(); + float l2 = v2.GetLength(); + if ((l1 < 1e-04) || (l2 < 1e-04)) continue; + /*gazebo::math::Vector3 _v1=v1; + gazebo::math::Vector3 _v2=v2; + _v1/=l1; + _v2/=l2; + float angle=acos(_v1.Dot(_v2));*/ + float angle = angularDistance(v1, v2); + // gzmsg<<"Angular distance between v1.len="< minAngleDiff) + { + float ratio; + if (l1 > l2) ratio = l2 / l1; + else ratio = l1 / l2; + // gzmsg<<"Got angle "<= lengthRatio) { - std::string name1 = collision1->GetScopedName(); - std::string name2 = collision2->GetScopedName(); - - //std::cout<<"OBJ CONTACT! "<contact(i).position_size(); - - // Check to see if the contact arrays all have the same size. - if ((count != _msg->contact(i).normal_size()) || - (count != _msg->contact(i).wrench_size()) || - (count != _msg->contact(i).depth_size())) - { - gzerr << "GazeboGraspFix: Contact message has invalid array sizes\n"<contact(i); - - if (contact.count<1) - { - std::cerr<<"ERROR: GazeboGraspFix: Not enough forces given for contact of ."< force; - - // find out which part of the colliding entities is the object, *not* the gripper, - // and copy all the forces applied to it into the vector 'force'. - std::map::const_iterator gripperCollIt = this->collisions.find(name2); - if (gripperCollIt != this->collisions.end()) - { // collision 1 is the object - collidingObjName=name1; - collidingLink=name2; - linkCollision=collision2; - objCollision=collision1; - gripperOfCollidingLink=gripperCollIt->second; - for (int k=0; kcollisions.find(name1)) != this->collisions.end()) - { // collision 2 is the object - collidingObjName=name2; - collidingLink=name1; - linkCollision=collision1; - objCollision=collision2; - gripperOfCollidingLink=gripperCollIt->second; - for (int k=0; kGetLink()->GetWorldPose(); - - // To find out the collision point relative to the Link's local coordinate system, first get the Poses as 4x4 matrices - gazebo::math::Matrix4 worldToLink=linkWorldPose.rot.GetAsMatrix4(); - worldToLink.SetTranslate(linkWorldPose.pos); - - gazebo::math::Matrix4 worldToContact=gazebo::math::Matrix4::IDENTITY; - //we can assume that the contact has identity rotation because we don't care about its orientation. - //We could always set another rotation here too. - worldToContact.SetTranslate(avgPos); - - // now, worldToLink * contactInLocal = worldToContact - // hence, contactInLocal = worldToLink.Inv * worldToContact - gazebo::math::Matrix4 worldToLinkInv = worldToLink.Inverse(); - gazebo::math::Matrix4 contactInLocal = worldToLinkInv * worldToContact; - gazebo::math::Vector3 contactPosInLocal = contactInLocal.GetTranslation(); - - //std::cout<<"---------"<GetLink()->GetWorldPose(); - gazebo::math::Matrix4 worldToObj = objWorldPose.rot.GetAsMatrix4(); - worldToObj.SetTranslate(objWorldPose.pos); - - gazebo::math::Matrix4 objInLocal = worldToLinkInv * worldToObj; - gazebo::math::Vector3 objPosInLocal = objInLocal.GetTranslation(); - - { - boost::mutex::scoped_lock lock(this->mutexContacts); - CollidingPoint& p = this->contacts[collidingObjName][collidingLink]; // inserts new entry if doesn't exist - p.gripperName=gripperOfCollidingLink; - p.collLink=linkCollision; - p.collObj=objCollision; - p.force+=avgForce; - p.pos+=contactPosInLocal; - p.objPos+=objPosInLocal; - p.sum++; - } - //std::cout<<"Average force of contact= "<contact_size(); ++i) + { + physics::CollisionPtr collision1 = + boost::dynamic_pointer_cast( + this->world->GetEntity(_msg->contact(i).collision1())); + physics::CollisionPtr collision2 = + boost::dynamic_pointer_cast( + this->world->GetEntity(_msg->contact(i).collision2())); + + if ((collision1 && !collision1->IsStatic()) && (collision2 + && !collision2->IsStatic())) + { + std::string name1 = collision1->GetScopedName(); + std::string name2 = collision2->GetScopedName(); + + //gzmsg<<"OBJ CONTACT! "<contact(i).position_size(); + + // Check to see if the contact arrays all have the same size. + if ((count != _msg->contact(i).normal_size()) || + (count != _msg->contact(i).wrench_size()) || + (count != _msg->contact(i).depth_size())) + { + gzerr << "GazeboGraspFix: Contact message has invalid array sizes\n" << + std::endl; + continue; + } + + std::string collidingObjName, collidingLink, gripperOfCollidingLink; + physics::CollisionPtr linkCollision; + physics::CollisionPtr objCollision; + + physics::Contact contact; + contact = _msg->contact(i); + + if (contact.count < 1) + { + std::cerr << "ERROR: GazeboGraspFix: Not enough forces given for contact of ." + << name1 << " / " << name2 << std::endl; + continue; + } + + // all force vectors which are part of this contact + std::vector force; + + // find out which part of the colliding entities is the object, *not* the gripper, + // and copy all the forces applied to it into the vector 'force'. + std::map::const_iterator gripperCollIt = + this->collisions.find(name2); + if (gripperCollIt != this->collisions.end()) + { + // collision 1 is the object + collidingObjName = name1; + collidingLink = name2; + linkCollision = collision2; + objCollision = collision1; + gripperOfCollidingLink = gripperCollIt->second; + for (int k = 0; k < contact.count; ++k) + force.push_back(contact.wrench[k].body1Force); + } + else if ((gripperCollIt = this->collisions.find(name1)) != + this->collisions.end()) + { + // collision 2 is the object + collidingObjName = name2; + collidingLink = name1; + linkCollision = collision1; + objCollision = collision2; + gripperOfCollidingLink = gripperCollIt->second; + for (int k = 0; k < contact.count; ++k) + force.push_back(contact.wrench[k].body2Force); + } + + gazebo::math::Vector3 avgForce; + // compute average/sum of the forces applied on the object + for (int k = 0; k < force.size(); ++k) + { + avgForce += force[k]; + } + avgForce /= force.size(); + + gazebo::math::Vector3 avgPos; + // compute center point (average pose) of all the origin positions of the forces appied + for (int k = 0; k < contact.count; ++k) avgPos += contact.positions[k]; + avgPos /= contact.count; + + // now, get average pose relative to the colliding link + gazebo::math::Pose linkWorldPose = linkCollision->GetLink()->GetWorldPose(); + + // To find out the collision point relative to the Link's local coordinate system, first get the Poses as 4x4 matrices + gazebo::math::Matrix4 worldToLink = linkWorldPose.rot.GetAsMatrix4(); + worldToLink.SetTranslate(linkWorldPose.pos); + + gazebo::math::Matrix4 worldToContact = gazebo::math::Matrix4::IDENTITY; + //we can assume that the contact has identity rotation because we don't care about its orientation. + //We could always set another rotation here too. + worldToContact.SetTranslate(avgPos); + + // now, worldToLink * contactInLocal = worldToContact + // hence, contactInLocal = worldToLink.Inv * worldToContact + gazebo::math::Matrix4 worldToLinkInv = worldToLink.Inverse(); + gazebo::math::Matrix4 contactInLocal = worldToLinkInv * worldToContact; + gazebo::math::Vector3 contactPosInLocal = contactInLocal.GetTranslation(); + + //gzmsg<<"---------"<GetLink()->GetWorldPose(); + gazebo::math::Matrix4 worldToObj = objWorldPose.rot.GetAsMatrix4(); + worldToObj.SetTranslate(objWorldPose.pos); + + gazebo::math::Matrix4 objInLocal = worldToLinkInv * worldToObj; + gazebo::math::Vector3 objPosInLocal = objInLocal.GetTranslation(); + + { + boost::mutex::scoped_lock lock(this->mutexContacts); + CollidingPoint &p = + this->contacts[collidingObjName][collidingLink]; // inserts new entry if doesn't exist + p.gripperName = gripperOfCollidingLink; + p.collLink = linkCollision; + p.collObj = objCollision; + p.force += avgForce; + p.pos += contactPosInLocal; + p.objPos += objPosInLocal; + p.sum++; + } + //gzmsg<<"Average force of contact= "< #include #include - + #include using gazebo::GazeboGraspGripper; #define DEFAULT_FORCES_ANGLE_TOLERANCE 120 #define DEFAULT_UPDATE_RATE 5 -#define DEFAULT_MAX_GRIP_COUNT 10 +#define DEFAULT_MAX_GRIP_COUNT 10 #define DEFAULT_RELEASE_TOLERANCE 0.005 #define DEFAULT_DISABLE_COLLISIONS_ON_ATTACH false +/////////////////////////////////////////////////////////////////////////////// GazeboGraspGripper::GazeboGraspGripper(): - attached(false) + attached(false) { } -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) +/////////////////////////////////////////////////////////////////////////////// +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) {} -GazeboGraspGripper::~GazeboGraspGripper() { - this->model.reset(); +/////////////////////////////////////////////////////////////////////////////// +GazeboGraspGripper::~GazeboGraspGripper() +{ + this->model.reset(); } - - -bool GazeboGraspGripper::Init(physics::ModelPtr& _model, - const std::string& _gripperName, - const std::string& palmLinkName, - const std::vector& fingerLinkNames, - bool _disableCollisionsOnAttach, - std::map& _collisionElems) +/////////////////////////////////////////////////////////////////////////////// +bool GazeboGraspGripper::Init(physics::ModelPtr &_model, + const std::string &_gripperName, + const std::string &palmLinkName, + const std::vector &fingerLinkNames, + bool _disableCollisionsOnAttach, + std::map &_collisionElems) { - this->gripperName=_gripperName; - this->attached = false; - this->disableCollisionsOnAttach = _disableCollisionsOnAttach; - this->model = _model; - physics::PhysicsEnginePtr physics = this->model->GetWorld()->GetPhysicsEngine(); - this->fixedJoint = physics->CreateJoint("revolute"); - - this->palmLink = this->model->GetLink(palmLinkName); - if (!this->palmLink) + this->gripperName = _gripperName; + this->attached = false; + this->disableCollisionsOnAttach = _disableCollisionsOnAttach; + this->model = _model; + physics::PhysicsEnginePtr physics = this->model->GetWorld()->GetPhysicsEngine(); + 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::const_iterator fingerIt = + fingerLinkNames.begin(); + fingerIt != fingerLinkNames.end(); ++fingerIt) + { + physics::LinkPtr link = this->model->GetLink(*fingerIt); + //gzmsg<<"Got link "<Get()<::const_iterator fingerIt=fingerLinkNames.begin(); - fingerIt!=fingerLinkNames.end(); ++fingerIt) + for (unsigned int j = 0; j < link->GetChildCount(); ++j) { - physics::LinkPtr link = this->model->GetLink(*fingerIt); - //std::cout<<"Got link "<Get()<GetChildCount(); ++j) - { - physics::CollisionPtr collision = link->GetCollision(j); - std::string collName = collision->GetScopedName(); - //collision->SetContactsEnabled(true); - std::map::iterator collIter = collisionElems.find(collName); - if (collIter != this->collisionElems.end()) { //this collision was already added before - gzwarn <<"GazeboGraspGripper: Adding Gazebo collision link element "<collisionElems[collName] = collision; - _collisionElems[collName] = collision; - } + physics::CollisionPtr collision = link->GetCollision(j); + std::string collName = collision->GetScopedName(); + //collision->SetContactsEnabled(true); + std::map::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; } - return !this->collisionElems.empty(); + } + return !this->collisionElems.empty(); } - - -const std::string& GazeboGraspGripper::getGripperName() const +/////////////////////////////////////////////////////////////////////////////// +const std::string &GazeboGraspGripper::getGripperName() const { - return gripperName; + return gripperName; } -bool GazeboGraspGripper::hasLink(const std::string& linkName) const +/////////////////////////////////////////////////////////////////////////////// +bool GazeboGraspGripper::hasLink(const std::string &linkName) const { - for (std::vector::const_iterator it=linkNames.begin(); it!=linkNames.end(); ++it) - { - if (*it==linkName) return true; - } - return false; + for (std::vector::const_iterator it = linkNames.begin(); + it != linkNames.end(); ++it) + { + if (*it == linkName) return true; + } + return false; } -bool GazeboGraspGripper::hasCollisionLink(const std::string& linkName) const +/////////////////////////////////////////////////////////////////////////////// +bool GazeboGraspGripper::hasCollisionLink(const std::string &linkName) const { - return collisionElems.find(linkName) != collisionElems.end(); + return collisionElems.find(linkName) != collisionElems.end(); } +/////////////////////////////////////////////////////////////////////////////// bool GazeboGraspGripper::isObjectAttached() const { - return attached; + return attached; } -const std::string& GazeboGraspGripper::attachedObject() const +/////////////////////////////////////////////////////////////////////////////// +const std::string &GazeboGraspGripper::attachedObject() const { - return attachedObjName; + return attachedObjName; } +/////////////////////////////////////////////////////////////////////////////// // #define USE_MODEL_ATTACH // this only works if the object is a model in itself, which is usually not - // 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) +// 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) { - if (!this->palmLink) { - gzwarn << "GazeboGraspGripper: palm link not found, not enforcing grasp hack!\n"<model->GetWorld(); - if (!world.get()) - { - gzerr << "GazeboGraspGripper: world is NULL"<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; + } #ifdef USE_MODEL_ATTACH - physics::ModelPtr obj = world->GetModel(objName); - if (!obj.get()){ - std::cerr<<"ERROR: Object ModelPtr "<GetLink()->GetWorldPose() - this->palmLink->GetWorldPose(); - this->palmLink->AttachStaticModel(obj,diff); + 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); #else - physics::CollisionPtr obj = boost::dynamic_pointer_cast(world->GetEntity(objName)); - if (!obj.get()){ - std::cerr<<"ERROR: Object "<GetLink()->GetWorldPose() - this->palmLink->GetWorldPose(); - this->fixedJoint->Load(this->palmLink,obj->GetLink(), diff); - this->fixedJoint->Init(); - this->fixedJoint->SetHighStop(0, 0); - this->fixedJoint->SetLowStop(0, 0); - 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"); - } + physics::CollisionPtr obj = boost::dynamic_pointer_cast + (world->GetEntity(objName)); + if (!obj.get()) + { + std::cerr << "ERROR: Object " << objName << + " not found in world, can't attach it" << std::endl; + return false; + } + gazebo::math::Pose diff = obj->GetLink()->GetWorldPose() - + this->palmLink->GetWorldPose(); + this->fixedJoint->Load(this->palmLink, obj->GetLink(), diff); + this->fixedJoint->Init(); + this->fixedJoint->SetHighStop(0, 0); + this->fixedJoint->SetLowStop(0, 0); + 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"); + } #endif // USE_MODEL_ATTACH - this->attached=true; - this->attachedObjName=objName; - return true; + this->attached = true; + this->attachedObjName = objName; + return true; } -void GazeboGraspGripper::HandleDetach(const std::string& objName) +/////////////////////////////////////////////////////////////////////////////// +void GazeboGraspGripper::HandleDetach(const std::string &objName) { - physics::WorldPtr world = this->model->GetWorld(); - if (!world.get()) - { - gzerr << "GazeboGraspGripper: world is NULL"<model->GetWorld(); + if (!world.get()) + { + gzerr << "GazeboGraspGripper: world is NULL" << std::endl << std::endl; + return; + } #ifdef USE_MODEL_ATTACH - physics::ModelPtr obj = world->GetModel(objName); - if (!obj.get()){ - std::cerr<<"ERROR: Object ModelPtr "<palmLink->DetachStaticModel(objName); + 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); #else - physics::CollisionPtr obj = boost::dynamic_pointer_cast(world->GetEntity(objName)); - if (!obj.get()){ - std::cerr<<"ERROR: Object "<disableCollisionsOnAttach) - { - obj->GetLink()->SetCollideMode("all"); - } - this->fixedJoint->Detach(); + physics::CollisionPtr obj = boost::dynamic_pointer_cast + (world->GetEntity(objName)); + 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 + // ------------------- + #endif // USE_MODEL_ATTACH - this->attached=false; - this->attachedObjName=""; + this->attached = false; + this->attachedObjName = ""; }