Commit a5500d64 authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

Style formatting, and some test prints for issue #26

parent bc04d963
......@@ -10,7 +10,8 @@
#include <gazebo_grasp_plugin/GazeboGraspGripper.h>
// #include <gazebo_grasp_plugin/CollidingPoint.h>
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 {
* - ``<arm_name>`` is the name of this arm. Has to be unique.
* - ``<palm_link>`` has to be the link to which the finger joints are attached.
* - ``<gripper_link>`` 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).
* - ``<update_rate>`` 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 @@ public:
/**
* 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 @@ private:
* 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<math::Vector3>& forces, float minAngleDiff, float lengthRatio);
bool CheckGrip(const std::vector<math::Vector3> &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<std::string, std::string> getAttachedObjects() const;
std::map<std::string, std::string> 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<std::string, GazeboGraspGripper> 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 @@ private:
class CollidingPoint;
// Contact forces sorted by object name the gripper collides with (first key)
// and the link colliding (second key).
std::map<std::string, std::map<std::string, CollidingPoint> > contacts;
// and the link colliding (second key).
std::map<std::string, std::map<std::string, CollidingPoint> > contacts;
boost::mutex mutexContacts; //mutex protects contacts
// when an object was first attached, it had these colliding points.
......@@ -221,18 +228,19 @@ private:
// 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<std::string, std::map<std::string, CollidingPoint> > 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<std::string, std::map<std::string, CollidingPoint> >
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<std::string, int> gripCounts;
std::map<std::string, int> 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;
......
......@@ -8,61 +8,63 @@
#include <gazebo/transport/TransportTypes.hh>
#include <stdio.h>
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<std::string>& fingerLinkNames,
bool _disableCollisionsOnAttach,
std::map<std::string, physics::CollisionPtr>& _collisions);
*/
bool 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> &_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<std::string> linkNames;
// names and Collision objects of the collision links in Gazebo (scoped names)
......@@ -80,7 +82,7 @@ private:
// 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;
};
}
......
This diff is collapsed.
......@@ -5,192 +5,252 @@
#include <gazebo/physics/Contact.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>
#include <gazebo_grasp_plugin/GazeboGraspGripper.h>
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<std::string>& fingerLinkNames,
bool _disableCollisionsOnAttach,
std::map<std::string, physics::CollisionPtr>& _collisionElems)
///////////////////////////////////////////////////////////////////////////////
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)
{
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<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())
{
gzerr << "GazeboGraspGripper: Palm link "<<palmLinkName<<" not found. The gazebo grasp plugin will not work."<<std::endl;
return false;
gzerr << "GazeboGraspGripper ERROR: Link " << *fingerIt <<
" can't be found in gazebo for GazeboGraspGripper model plugin. Skipping." <<
std::endl;
continue;
}
for (std::vector<std::string>::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 "<<fingerLinkElem->Get<std::string>()<<std::endl;
if (!link.get()){
gzerr << "GazeboGraspGripper ERROR: Link "<<*fingerIt<<" can't be found in gazebo for GazeboGraspGripper model plugin. Skipping."<<std::endl;
continue;
}
for (unsigned int j = 0; j < link->GetChildCount(); ++j)
{
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;
}
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;
}
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<std::string>::const_iterator it=linkNames.begin(); it!=linkNames.end(); ++it)
{
if (*it==linkName) return true;
}
return false;
for (std::vector<std::string>::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"<<std::endl;
return false;
}
physics::WorldPtr world = this->model->GetWorld();
if (!world.get())
{
gzerr << "GazeboGraspGripper: world is NULL"<<std::endl;
return false;
}
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;
}
#ifdef USE_MODEL_ATTACH
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);
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<physics::Collision>(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");
}
physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>
(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"<<std::endl<<std::endl;
return;
}
physics::WorldPtr world