#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
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

Detailed Description

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:

``<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 which they are processed, which takes a bit of computation time, and therefore should be lower than the gazebo world update rate.
``<forces_angle_tolerance>`` is the tolerance angle (in degrees) between two force vectors to be considered "opposing forces". If the angle is smaller than this, they are not opposing.
``<grip_count_threshold>`` is number of times in the update loop (running at update_rate) that an object has to be detected as "gripped" in order to attach the object. Adjust this with the update rate.
``<max_grip_count>`` is the maximum number of a counter: At each update iteration (running at update_rate), if the "gripping criterion" is met for an object, a counter for this object is increased. ``max_grip_count`` is the maximum number recorded for an object. As soon as the counter goes beyond this number, the counter is stopped. As soon as the "gripping criterion" does not hold any more, the number will start to decrease again, (by 1 each time the object is detected as "not grasped" in an update iteration). So this counter is like a "buffer" which, when it is full, maintains the state, and when it is empty, again, the object is released. This should be at least double of ``grip_count_threshold``.
``<release_tolerance>`` is the distance which the gripper links are allowed to move away from the object during- a grasp without the object being detached, even if there are currently no actual contacts on the object. This condition can happen if the fingers "wobble" or move ever so slightly away from the object, and therefore the "gripping criterion" fails in a few subsequent update iterations. This setting is to make the behaviour more stable. Setting this number too high will also lead to the object not being detached even if the grippers have opened up to release it, so use this with care.
``<disable_collisions_on_attach>`` can be used for the following: When an object is attached, collisions with it may be disabled, in case the robot still keeps wobbling.
``<contact_topic>`` is the gazebo topic of contacts. Should normally be left at --.

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

Definition at line 92 of file GazeboGraspFix.h.

Constructor & Destructor Documentation

GazeboGraspFix::GazeboGraspFix ( )

Definition at line 25 of file GazeboGraspFix.cpp.

GazeboGraspFix::GazeboGraspFix ( physics::ModelPtr _model )

Definition at line 31 of file GazeboGraspFix.cpp.

GazeboGraspFix::~GazeboGraspFix ( ) [virtual]

Definition at line 37 of file GazeboGraspFix.cpp.

Member Function Documentation

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 316 of file GazeboGraspFix.cpp.

void GazeboGraspFix::Init ( ) [private, virtual]

Definition at line 45 of file GazeboGraspFix.cpp.

void GazeboGraspFix::InitValues ( ) [private]

Definition at line 51 of file GazeboGraspFix.cpp.

bool GazeboGraspFix::IsGripperLink	(	const std::string &	linkName,
		std::string &	gripperName
	)		const `[private]`

Definition at line 300 of file GazeboGraspFix.cpp.

void GazeboGraspFix::Load	(	physics::ModelPtr	_parent,
		sdf::ElementPtr	_sdf
	)		`[private, virtual]`

Definition at line 68 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 333 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 350 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 102 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 802 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 107 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 490 of file GazeboGraspFix.cpp.