#include <GazeboGraspFix.h>
Classes | |
class | CollidingPoint |
class | ObjectContactInfo |
Public Member Functions | |
GazeboGraspFix () | |
GazeboGraspFix (physics::ModelPtr _model) | |
virtual void | OnAttach (const std::string &objectName, const std::string &armName) |
virtual void | OnDetach (const std::string &objectName, const std::string &armName) |
virtual | ~GazeboGraspFix () |
Private Member Functions | |
bool | checkGrip (const std::vector< math::Vector3 > &forces, float minAngleDiff, float lengthRatio) |
std::map< std::string, std::string > | getAttachedObjects () const |
virtual void | Init () |
void | InitValues () |
bool | isGripperLink (const std::string &linkName, std::string &gripperName) const |
virtual void | Load (physics::ModelPtr _parent, sdf::ElementPtr _sdf) |
bool | objectAttachedToGripper (const ObjectContactInfo &objContInfo, std::string &attachedToGripper) const |
bool | objectAttachedToGripper (const std::string &gripperName, std::string &attachedToGripper) const |
void | OnContact (const ConstContactsPtr &ptr) |
void | OnUpdate () |
Private Attributes | |
std::map< std::string, std::map< std::string, CollidingPoint > > | attachGripContacts |
std::map< std::string, std::string > | collisions |
std::map< std::string, std::map< std::string, CollidingPoint > > | contacts |
transport::SubscriberPtr | contactSub |
bool | disableCollisionsOnAttach |
float | forcesAngleTolerance |
std::map< std::string, int > | gripCounts |
int | gripCountThreshold |
std::map< std::string, GazeboGraspGripper > | grippers |
int | maxGripCount |
boost::mutex | mutexContacts |
transport::NodePtr | node |
common::Time | prevUpdateTime |
float | releaseTolerance |
event::ConnectionPtr | update_connection |
common::Time | updateRate |
physics::WorldPtr | world |
Inspired by gazebo::physics::Gripper, this plugin fixes an object which is grasped to the robot hand to avoid problems with physics engines and to help the object staying in the robot hand without slipping out.
This is a *model* plugin, so you have to load the model plugin from the robot URDF:
```xml <gazebo> <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so"> <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> <max_grip_count>8</max_grip_count> <release_tolerance>0.005</release_tolerance> <disable_collisions_on_attach>false</disable_collisions_on_attach> <contact_topic>__default_topic__</contact_topic> </plugin> </gazebo> ```
Description of the arguments:
Current limitations:
Definition at line 96 of file GazeboGraspFix.h.
Definition at line 28 of file GazeboGraspFix.cpp.
GazeboGraspFix::GazeboGraspFix | ( | physics::ModelPtr | _model | ) |
Definition at line 32 of file GazeboGraspFix.cpp.
GazeboGraspFix::~GazeboGraspFix | ( | ) | [virtual] |
Definition at line 37 of file GazeboGraspFix.cpp.
bool GazeboGraspFix::checkGrip | ( | const std::vector< math::Vector3 > & | forces, |
float | minAngleDiff, | ||
float | lengthRatio | ||
) | [private] |
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.
Definition at line 640 of file GazeboGraspFix.cpp.
std::map< std::string, std::string > GazeboGraspFix::getAttachedObjects | ( | ) | const [private] |
return objects (key) and the gripper (value) to which it is attached
Definition at line 258 of file GazeboGraspFix.cpp.
void GazeboGraspFix::Init | ( | ) | [private, virtual] |
Definition at line 44 of file GazeboGraspFix.cpp.
void GazeboGraspFix::InitValues | ( | ) | [private] |
Definition at line 49 of file GazeboGraspFix.cpp.
bool GazeboGraspFix::isGripperLink | ( | const std::string & | linkName, |
std::string & | gripperName | ||
) | const [private] |
Definition at line 245 of file GazeboGraspFix.cpp.
void GazeboGraspFix::Load | ( | physics::ModelPtr | _parent, |
sdf::ElementPtr | _sdf | ||
) | [private, virtual] |
Definition at line 66 of file GazeboGraspFix.cpp.
bool GazeboGraspFix::objectAttachedToGripper | ( | const ObjectContactInfo & | objContInfo, |
std::string & | attachedToGripper | ||
) | const [private] |
Helper function to determine if object attached to a gripper in ObjectContactInfo.
Definition at line 277 of file GazeboGraspFix.cpp.
bool GazeboGraspFix::objectAttachedToGripper | ( | const std::string & | gripperName, |
std::string & | attachedToGripper | ||
) | const [private] |
Helper function to determine if object attached to this gripper
Definition at line 291 of file GazeboGraspFix.cpp.
virtual void gazebo::GazeboGraspFix::OnAttach | ( | const std::string & | objectName, |
const std::string & | armName | ||
) | [inline, virtual] |
Gets called just after the object has been attached to the palm link on armName
Definition at line 105 of file GazeboGraspFix.h.
void GazeboGraspFix::OnContact | ( | const ConstContactsPtr & | ptr | ) | [private] |
Gets called upon detection of contacts. A list of contacts is passed in . One contact has two bodies, and only the ones where one of the bodies is a gripper link are considered. Each contact consists of a *list* of forces with their own origin/position each (e.g. when the object and gripper are colliding at several places). The averages of each contact's force vectors along with their origins is computed. This "average contact force/origin" for each contact is then added to the 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.
Definition at line 680 of file GazeboGraspFix.cpp.
virtual void gazebo::GazeboGraspFix::OnDetach | ( | const std::string & | objectName, |
const std::string & | armName | ||
) | [inline, virtual] |
Gets called just after the object has been detached to the palm link on armName
Definition at line 109 of file GazeboGraspFix.h.
void GazeboGraspFix::OnUpdate | ( | ) | [private] |
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 list of all forces applied. If checkGrip() returns true, the number of "grip counts" is increased for the holding arm (but grip counts will never exceed max_grip_count). If the number of grip counts for this object exceeds grip_count_threshold, the object is attached by calling GazeboGraspGripper::HandleAttach(object-name), setting attached and attachedObjName, and GazeboGraspGripper::attachGripContacts is updated with the contact points currently existing for this object (current entry in contacts).
Then, goes through all entries in gripCount, and unless it's an object we just detected as "gripped", the counter is decreased. If the counter is is smaller than grip_count_threshold, the object should potentially be released, but this criterion happens too easily (the fingers in gazebo may have started wobbling as the arm moves around, and although they are still close to the object, the grip is not detected any more). So to be sure, and additional criterion has to be satisfied before the object is released: check that the collision point (the place on the link where the contact originally was detected) has not moved too far from where it originally was, relative to the object.
Definition at line 362 of file GazeboGraspFix.cpp.
std::map<std::string, std::map<std::string, CollidingPoint> > gazebo::GazeboGraspFix::attachGripContacts [private] |
Definition at line 228 of file GazeboGraspFix.h.
std::map<std::string, std::string> gazebo::GazeboGraspFix::collisions [private] |
Definition at line 209 of file GazeboGraspFix.h.
std::map<std::string, std::map<std::string, CollidingPoint> > gazebo::GazeboGraspFix::contacts [private] |
Definition at line 215 of file GazeboGraspFix.h.
transport::SubscriberPtr gazebo::GazeboGraspFix::contactSub [private] |
Definition at line 195 of file GazeboGraspFix.h.
bool gazebo::GazeboGraspFix::disableCollisionsOnAttach [private] |
Definition at line 203 of file GazeboGraspFix.h.
float gazebo::GazeboGraspFix::forcesAngleTolerance [private] |
Definition at line 199 of file GazeboGraspFix.h.
std::map<std::string, int> gazebo::GazeboGraspFix::gripCounts [private] |
Definition at line 235 of file GazeboGraspFix.h.
int gazebo::GazeboGraspFix::gripCountThreshold [private] |
Definition at line 242 of file GazeboGraspFix.h.
std::map<std::string, GazeboGraspGripper> gazebo::GazeboGraspFix::grippers [private] |
Definition at line 191 of file GazeboGraspFix.h.
int gazebo::GazeboGraspFix::maxGripCount [private] |
Definition at line 238 of file GazeboGraspFix.h.
boost::mutex gazebo::GazeboGraspFix::mutexContacts [private] |
Definition at line 220 of file GazeboGraspFix.h.
transport::NodePtr gazebo::GazeboGraspFix::node [private] |
Definition at line 194 of file GazeboGraspFix.h.
common::Time gazebo::GazeboGraspFix::prevUpdateTime [private] |
Definition at line 253 of file GazeboGraspFix.h.
float gazebo::GazeboGraspFix::releaseTolerance [private] |
Definition at line 247 of file GazeboGraspFix.h.
Definition at line 193 of file GazeboGraspFix.h.
common::Time gazebo::GazeboGraspFix::updateRate [private] |
Definition at line 250 of file GazeboGraspFix.h.
physics::WorldPtr gazebo::GazeboGraspFix::world [private] |
Definition at line 188 of file GazeboGraspFix.h.