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:
2 <
plugin name=
"gazebo_grasp_fix" filename=
"libgazebo_grasp_fix.so">
4 <
arm_name>
name-of-arm</
arm_name>
5 <
palm_link>
hand_link_name </
palm_link>
6 <
gripper_link>
finger_index_link_1 </
gripper_link>
7 <
gripper_link>
finger_index_link_2 </
gripper_link>
8 <
gripper_link> ... </
gripper_link>
10 <
forces_angle_tolerance>100</
forces_angle_tolerance>
11 <
update_rate>4</
update_rate>
12 <
grip_count_threshold>4</
grip_count_threshold>
13 <
max_grip_count>8</
max_grip_count>
14 <
release_tolerance>0.005</
release_tolerance>
15 <
disable_collisions_on_attach>
false</
disable_collisions_on_attach>
16 <
contact_topic>
__default_topic__</
contact_topic>
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 -__default_topic__-.
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 96 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.
