Commit 7d2a285c authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

Now Gazebo grasp fix should work for several arms, but not directly tested on current test bed

parent e3ba65a4
......@@ -53,7 +53,7 @@ link_directories(
)
## Declare a cpp library
add_library(gazebo_grasp_fix SHARED src/GazeboGraspFix.cpp)
add_library(gazebo_grasp_fix SHARED src/GazeboGraspFix.cpp src/GazeboGraspGripper.cpp)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
......
#ifndef GAZEBO_GAZEBOGRASPFIX_H
#define GAZEBO_GAZEBOGRASPFIX_H
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <stdio.h>
#include <gazebo_grasp_plugin/GazeboGraspGripper.h>
// #include <gazebo_grasp_plugin/CollidingPoint.h>
namespace gazebo {
......@@ -17,10 +22,13 @@ namespace gazebo {
* ```xml
* <gazebo>
* <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
* <palm_link> hand_link_name </palm_link>
* <gripper_link> finger_index_link_1 </gripper_link>
* <gripper_link> finger_index_link_2 </gripper_link>
* <gripper_link> ... </gripper_link>
* <arm>
* <arm_name>name-of-arm</arm_name>
* <palm_link> hand_link_name </palm_link>
* <gripper_link> finger_index_link_1 </gripper_link>
* <gripper_link> finger_index_link_2 </gripper_link>
* <gripper_link> ... </gripper_link>
* </arm>
* <forces_angle_tolerance>100</forces_angle_tolerance>
* <update_rate>4</update_rate>
* <grip_count_threshold>4</grip_count_threshold>
......@@ -34,10 +42,12 @@ namespace gazebo {
*
* Description of the arguments:
*
* - ``<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
* above described criterion).
* - ``<arm>`` contains a collections of specification for each arm which can grasp one object (there may be several ``<arm>`` tags):
* - ``<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
* 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
* a higher rate (the Gazebo world update rate). The ``update_rate`` is only the rate at
......@@ -71,9 +81,12 @@ namespace gazebo {
* robot still keeps wobbling.
* - ``<contact_topic>`` is the gazebo topic of contacts. Should normally be left at -\_\_default_topic\_\_-.
*
* Current limitation:
* - Only one object can be attached per gripper.
* Current limitations:
*
* - 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 {
......@@ -83,13 +96,13 @@ public:
virtual ~GazeboGraspFix();
/**
* Gets called just after the object has been attached to the link
* Gets called just after the object has been attached to the palm link on \e armName
*/
virtual void OnAttach(const std::string& objectName){}
virtual void OnAttach(const std::string& objectName, const std::string& armName){}
/**
* Gets called just after the object has been detached to the link
* Gets called just after the object has been detached to the palm link on \e armName
*/
virtual void OnDetach(const std::string& objectName){}
virtual void OnDetach(const std::string& objectName, const std::string& armName){}
private:
virtual void Init();
......@@ -99,12 +112,12 @@ private:
* Then, for each object, checks whether of all the forces applied,
* 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" (field \e gripCounts)
* is increased (but grip counts will never exceed \e max_grip_count).
* 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 HandleAttach(object-name),
* setting \e attached and \e attachedObjName, and \e attachGripContacts is updated with the
* contact points currently existing for this object (current entry in \e contacts).
* the object is attached by calling GazeboGraspGripper::HandleAttach(object-name),
* setting \e attached and \e attachedObjName, and \e GazeboGraspGripper::attachGripContacts
* is updated with the contact points currently existing for this object (current entry in \e contacts).
*
* Then, goes through all entries in \e gripCount, and unless it's an object
* we just detected as "gripped", the counter is decreased.
......@@ -135,23 +148,69 @@ private:
*/
void OnContact(const ConstContactsPtr& ptr);
bool HandleAttach(const std::string& objName);
void HandleDetach(const std::string& objName);
/**
* 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 isGripperLink(const std::string& linkName, std::string& gripperName) const
{
for (std::map<std::string, GazeboGraspGripper>::const_iterator it=grippers.begin(); it!=grippers.end(); ++it)
{
if (it->second.hasLink(linkName))
{
gripperName=it->first;
return true;
}
}
return false;
}
/**
* 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> ret;
for (std::map<std::string, GazeboGraspGripper>::const_iterator it=grippers.begin(); it!=grippers.end(); ++it)
{
const std::string& gripperName = it->first;
const GazeboGraspGripper& gripper = it->second;
if (gripper.isObjectAttached())
{
ret[gripper.attachedObject()]=gripperName;
}
}
return ret;
}
/**
* Helper class to collect contact info per object.
* Forward declaration here.
*/
class ObjectContactInfo;
physics::ModelPtr model;
/**
* Helper function to determine if object attached to a gripper in ObjectContactInfo.
*/
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;
physics::PhysicsEnginePtr physics;
// physics::ModelPtr model;
// physics::PhysicsEnginePtr physics;
physics::WorldPtr world;
physics::JointPtr fixedJoint;
physics::LinkPtr palmLink;
// 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
......@@ -164,12 +223,15 @@ private:
// robot still keeps wobbling.
bool disableCollisionsOnAttach;
// list of current collisions per gripper link
std::map<std::string, physics::CollisionPtr> collisions;
// 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
// this collision link belongs to.
std::map<std::string, std::string> collisions;
/**
* Helper class to encapsulate a collision information.
* Forward declaration here.
* Helper class to encapsulate a collision information. Forward declaration here.
*/
class CollidingPoint;
......@@ -179,10 +241,15 @@ private:
boost::mutex mutexContacts; //mutex protects contacts
// when an object was first attached, it had these colliding points.
// Key is object name.
// First key is object name, second is the link colliding, as in \e contacts.
// Only the links of *one* gripper are stored here. This indirectly imposes the
// 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 update loops (at updateRate) the grip on that object has been recorded
// 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.
......@@ -190,7 +257,7 @@ private:
// *maximum* number in \e gripCounts to be recorded.
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;
......@@ -200,11 +267,6 @@ private:
// the object is released.
float releaseTolerance;
// flag holding whether an object is attached. Object name in \e attachedObjName
bool attached;
// name of the object currently attached.
std::string attachedObjName;
//nano seconds between two updates
common::Time updateRate;
......@@ -213,3 +275,5 @@ private:
};
}
#endif // GAZEBO_GAZEBOGRASPFIX_H
#ifndef GAZEBO_GAZEBOGRASPGRIPPER_H
#define GAZEBO_GAZEBOGRASPGRIPPER_H
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <stdio.h>
namespace gazebo {
/**
* \author Jennifer Buehler
*/
class GazeboGraspGripper {
public:
GazeboGraspGripper();
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);
const std::string& getGripperName() const
{
return gripperName;
}
/**
* Has the link name (URDF)
*/
bool 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;
}
/**
* Has the collision link name (Gazebo collision element name)
*/
bool hasCollisionLink(const std::string& linkName) const
{
return collisionElems.find(linkName) != collisionElems.end();
}
bool isObjectAttached() const
{
return attached;
}
const std::string& attachedObject() const
{
return attachedObjName;
}
/**
* \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);
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)
// Not necessarily equal names and size to linkNames.
std::map<std::string, physics::CollisionPtr> collisionElems;
physics::JointPtr fixedJoint;
physics::LinkPtr palmLink;
// when an object is attached, collisions with it may be disabled, in case the
// robot still keeps wobbling.
bool disableCollisionsOnAttach;
#if 0
// tolerance (in degrees) between force vectors to
// beconsidered "opposing"
float forcesAngleTolerance;
// list of current collisions per gripper link
std::map<std::string, physics::CollisionPtr> collisions;
// when an object was first attached, it had these colliding points.
// Key is object name.
std::map<std::string, std::map<std::string, CollidingPoint> > attachGripContacts;
// 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;
// once an object is gripped, the relative position of the collision link surface to the
// object is remembered. As soon as this distance changes more than release_tolerance,
// the object is released.
float releaseTolerance;
#endif
// flag holding whether an object is attached. Object name in \e attachedObjName
bool attached;
// name of the object currently attached.
std::string attachedObjName;
};
}
#endif // GAZEBO_GAZEBOGRASPGRIPPER_H
This diff is collapsed.
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/physics/ContactManager.hh>
#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_RELEASE_TOLERANCE 0.005
#define DEFAULT_DISABLE_COLLISIONS_ON_ATTACH false
GazeboGraspGripper::GazeboGraspGripper():
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() {
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)
{
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);
//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;
}
}
return !this->collisionElems.empty();
}
// #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)
{
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);
#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");
}
#endif // USE_MODEL_ATTACH
this->attached=true;
this->attachedObjName=objName;
return true;
}
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;
}
#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 detach it"<<std::endl;
return;
}
this->palmLink->DetachStaticModel(objName);
#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;
}
else if (this->disableCollisionsOnAttach)
{
obj->GetLink()->SetCollideMode("all");
}
this->fixedJoint->Detach();
#endif // USE_MODEL_ATTACH
this->attached=false;
this->attachedObjName="";
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment