GazeboGraspFix.h 10 KB
Newer Older
Jennifer Buehler's avatar
Jennifer Buehler committed
1
2
3
4
5
6
7
8
9
10
#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 {

/**
11
12
13
 * 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.
Jennifer Buehler's avatar
Jennifer Buehler committed
14
 *
15
 * This is a *model* plugin, so you have to load the model plugin from the robot URDF:
Jennifer Buehler's avatar
Jennifer Buehler committed
16
17
18
19
20
21
22
23
 *
 * ```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>
Jennifer Buehler's avatar
Jennifer Buehler committed
24
25
26
27
 *           <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>
28
29
30
 *           <release_tolerance>0.005</release_tolerance>
 *           <disable_collisions_on_attach>false</disable_collisions_on_attach>
 *           <contact_topic>__default_topic__</contact_topic>
Jennifer Buehler's avatar
Jennifer Buehler committed
31
32
33
34
 *       </plugin>
 *   </gazebo>
 * ```
 *
Jennifer Buehler's avatar
Jennifer Buehler committed
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
 *    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).
 *    - ``<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\_\_-.
73
74
75
 *
 * Current limitation:
 *  - Only one object can be attached per gripper.
Jennifer Buehler's avatar
Jennifer Buehler committed
76
 *
Jennifer Buehler's avatar
Jennifer Buehler committed
77
78
79
80
81
82
83
84
85
86
 * \author Jennifer Buehler
 */ 
class GazeboGraspFix : public ModelPlugin {
public:
    GazeboGraspFix();
    GazeboGraspFix(physics::ModelPtr _model);
    virtual ~GazeboGraspFix();
    virtual void Init(); 
    virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
private: 
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
    /**
     * 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" (field \e gripCounts)
     * is increased (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).
     *
     * Then, goes through all entries in \e gripCount, and unless it's an object
     * we just detected as "gripped", the counter is decreased.
     * If the counter is is smaller than \e 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.
     */
    void OnUpdate();
Jennifer Buehler's avatar
Jennifer Buehler committed
110
111

    void InitValues();
112
113
114
115
116
117
118
119
120
121
122
123
124
125


    /**
     * Gets called upon detection of contacts.
     * A list of contacts is passed in \_msg. 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 \e 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.  
     */
Jennifer Buehler's avatar
Jennifer Buehler committed
126
127
128
129
130
131
    void OnContact(const ConstContactsPtr& ptr);

    bool HandleAttach(const std::string& objName);
    void HandleDetach(const std::string& objName);

    /**
132
133
134
     * 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.
Jennifer Buehler's avatar
Jennifer Buehler committed
135
136
137
138
139
140
141
142
143
144
145
146
147
     */
    bool checkGrip(const std::vector<math::Vector3>& forces, float minAngleDiff, float lengthRatio);
    
    physics::ModelPtr model;

    physics::PhysicsEnginePtr physics;
    physics::WorldPtr world;
    physics::JointPtr fixedJoint;
    
    physics::LinkPtr palmLink;
    event::ConnectionPtr update_connection;
    transport::NodePtr node;
    transport::SubscriberPtr contactSub; //subscriber to contact updates
148
149
150
151
  
    // tolerance (in degrees) between force vectors to
    // beconsidered "opposing"
    float forcesAngleTolerance;
Jennifer Buehler's avatar
Jennifer Buehler committed
152
    
153
154
155
156
157
    // when an object is attached, collisions with it may be disabled, in case the
    // robot still keeps wobbling.
    bool disableCollisionsOnAttach;
 
    // list of current collisions per gripper link 
Jennifer Buehler's avatar
Jennifer Buehler committed
158
159
160
161
162
163
164
165
    std::map<std::string, physics::CollisionPtr> collisions;

    /**
     * Helper class to encapsulate a collision information.
     * Forward declaration here.
     */
    class CollidingPoint;

166
167
    // Contact forces sorted by object name the gripper collides with (first key)
    // and the link colliding (second key). 
Jennifer Buehler's avatar
Jennifer Buehler committed
168
169
170
    std::map<std::string, std::map<std::string, CollidingPoint> > contacts; 
    boost::mutex mutexContacts; //mutex protects contacts

171
172
    // when an object was first attached, it had these colliding points.
    // Key is object name.
Jennifer Buehler's avatar
Jennifer Buehler committed
173
174
    std::map<std::string, std::map<std::string, CollidingPoint> > attachGripContacts; 
    
175
176
177
178
    // Records how many update loops (at updateRate) 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.
Jennifer Buehler's avatar
Jennifer Buehler committed
179
180
    std::map<std::string, int> gripCounts; 

181
182
183
184
185
186
187
188
189
190
191
    // *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;

    // 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;
Jennifer Buehler's avatar
Jennifer Buehler committed
192

193
    // flag holding whether an object is attached. Object name in \e attachedObjName
Jennifer Buehler's avatar
Jennifer Buehler committed
194
    bool attached;
195
    // name of the object currently attached.
Jennifer Buehler's avatar
Jennifer Buehler committed
196
197
198
199
200
201
202
203
204
205
    std::string attachedObjName;    

    //nano seconds between two updates
    common::Time updateRate;

    //last time OnUpdate() was called
    common::Time prevUpdateTime;
};

}