Commit 20f6a768 authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

tidy up

parent 7d2a285c
......@@ -155,37 +155,12 @@ private:
*/
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;
}
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> 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;
}
std::map<std::string, std::string> getAttachedObjects() const;
/**
* Helper class to collect contact info per object.
......
......@@ -11,7 +11,9 @@
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 {
......@@ -32,41 +34,21 @@ public:
bool _disableCollisionsOnAttach,
std::map<std::string, physics::CollisionPtr>& _collisions);
const std::string& getGripperName() const
{
return gripperName;
}
const std::string& getGripperName() const;
/**
* 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;
}
bool hasLink(const std::string& linkName) const;
/**
* Has the collision link name (Gazebo collision element name)
*/
bool hasCollisionLink(const std::string& linkName) const
{
return collisionElems.find(linkName) != collisionElems.end();
}
bool hasCollisionLink(const std::string& linkName) const;
bool isObjectAttached() const
{
return attached;
}
bool isObjectAttached() const;
const std::string& attachedObject() const
{
return attachedObjName;
}
const std::string& attachedObject() const;
/**
* \param gripContacts contact forces on the object sorted by the link name colliding.
......@@ -95,29 +77,6 @@ private:
// 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.
......@@ -125,5 +84,4 @@ private:
};
}
#endif // GAZEBO_GAZEBOGRASPGRIPPER_H
......@@ -231,6 +231,39 @@ class GazeboGraspFix::ObjectContactInfo
};
bool GazeboGraspFix::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;
}
std::map<std::string, std::string> GazeboGraspFix::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;
}
bool GazeboGraspFix::objectAttachedToGripper(const ObjectContactInfo& objContInfo, std::string& attachedToGripper) const
{
for (std::map<std::string, int>::const_iterator gripInvIt=objContInfo.grippersInvolved.begin();
......
......@@ -38,6 +38,7 @@ GazeboGraspGripper::~GazeboGraspGripper() {
}
bool GazeboGraspGripper::Init(physics::ModelPtr& _model,
const std::string& _gripperName,
const std::string& palmLinkName,
......@@ -84,6 +85,40 @@ bool GazeboGraspGripper::Init(physics::ModelPtr& _model,
return !this->collisionElems.empty();
}
const std::string& GazeboGraspGripper::getGripperName() const
{
return gripperName;
}
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;
}
bool GazeboGraspGripper::hasCollisionLink(const std::string& linkName) const
{
return collisionElems.find(linkName) != collisionElems.end();
}
bool GazeboGraspGripper::isObjectAttached() const
{
return attached;
}
const std::string& GazeboGraspGripper::attachedObject() const
{
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.
......
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