GazeboGraspFix.h 12 KB
Newer Older
1
2
3
#ifndef GAZEBO_GAZEBOGRASPFIX_H
#define GAZEBO_GAZEBOGRASPFIX_H

Jennifer Buehler's avatar
Jennifer Buehler committed
4
5
6
7
8
9
#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>
10
#include <gazebo_grasp_plugin/GazeboGraspGripper.h>
Jennifer Buehler's avatar
Jennifer Buehler committed
11

12
13
namespace gazebo
{
Jennifer Buehler's avatar
Jennifer Buehler committed
14
15

/**
16
17
18
 * 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
19
 *
20
 * This is a *model* plugin, so you have to load the model plugin from the robot URDF:
Jennifer Buehler's avatar
Jennifer Buehler committed
21
22
23
24
 *
 * ```xml
 *   <gazebo>
 *       <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
25
26
27
28
29
30
31
 *          <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>
Jennifer Buehler's avatar
Jennifer Buehler committed
32
33
34
35
 *           <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>
36
37
38
 *           <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
39
40
41
42
 *       </plugin>
 *   </gazebo>
 * ```
 *
Jennifer Buehler's avatar
Jennifer Buehler committed
43
44
 *    Description of the arguments:
 *
45
46
47
48
 *    - ``<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
49
 *              actively grasp objects (these are the links which determine whether a "grasp" exists according to
50
 *              above described criterion).
Jennifer Buehler's avatar
Jennifer Buehler committed
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
 *    - ``<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\_\_-.
83
 *
84
 * Current limitations:
Jennifer Buehler's avatar
Jennifer Buehler committed
85
 *
86
87
88
 *  - 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)
89
 *
Jennifer Buehler's avatar
Jennifer Buehler committed
90
 * \author Jennifer Buehler
91
92
93
94
 */
class GazeboGraspFix : public ModelPlugin
{
  public:
Jennifer Buehler's avatar
Jennifer Buehler committed
95
96
97
    GazeboGraspFix();
    GazeboGraspFix(physics::ModelPtr _model);
    virtual ~GazeboGraspFix();
98
99

    /**
100
     * Gets called just after the object has been attached to the palm link on \e armName
101
     */
102
103
    virtual void OnAttach(const std::string &objectName,
                          const std::string &armName) {}
104
    /**
105
     * Gets called just after the object has been detached to the palm link on \e armName
106
     */
107
108
    virtual void OnDetach(const std::string &objectName,
                          const std::string &armName) {}
109

110
111
  private:
    virtual void Init();
Jennifer Buehler's avatar
Jennifer Buehler committed
112
    virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
113
114
115
    /**
     * Collects for each object all forces which are currently applied on it.
     * Then, for each object, checks whether of all the forces applied,
Jennifer Buehler's avatar
Jennifer Buehler committed
116
117
     * there are opposing forces.
     * This is done by calling CheckGrip() with the list of all forces applied.
118
     * If CheckGrip() returns true, the number of "grip counts"
119
     * is increased for the holding arm (but grip counts will never exceed \e max_grip_count).
120
     * If the number of grip counts for this object exceeds \e grip_count_threshold,
121
122
123
     * the object is attached by calling GazeboGraspGripper::HandleAttach(object-name),
     * setting \e attached and \e attachedObjName, and \e GazeboGraspGripper::attachGripContacts
     * is updated with the contact points currently existing for this object (current entry in \e contacts).
124
125
126
127
128
129
130
131
132
133
134
135
     *
     * 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
136
137

    void InitValues();
138
139
140
141
142
143
144
145
146
147
148
149


    /**
     * 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
150
     * the average force application over time between link and object.
151
     */
152
    void OnContact(const ConstContactsPtr &ptr);
Jennifer Buehler's avatar
Jennifer Buehler committed
153

Jennifer Buehler's avatar
Jennifer Buehler committed
154
155
//    bool CheckGrip(const std::vector<GzVector3> &forces, float minAngleDiff,
//                   float lengthRatio);
156

157
    bool IsGripperLink(const std::string &linkName, std::string &gripperName) const;
158
159
160
161

    /**
     * return objects (key) and the gripper (value) to which it is attached
     */
162
    std::map<std::string, std::string> GetAttachedObjects() const;
163
164
165
166
167
168

    /**
     * Helper class to collect contact info per object.
     * Forward declaration here.
     */
    class ObjectContactInfo;
169

170
171
172
    /**
     * Helper function to determine if object attached to a gripper in ObjectContactInfo.
     */
173
174
    bool ObjectAttachedToGripper(const ObjectContactInfo &objContInfo,
                                 std::string &attachedToGripper) const;
175
176
177
178

    /**
     * Helper function to determine if object attached to this gripper
     */
179
180
181
    bool ObjectAttachedToGripper(const std::string &gripperName,
                                 std::string &attachedToGripper) const;

Jennifer Buehler's avatar
Jennifer Buehler committed
182

183
184
    // physics::ModelPtr model;
    // physics::PhysicsEnginePtr physics;
Jennifer Buehler's avatar
Jennifer Buehler committed
185
    physics::WorldPtr world;
186
187
188

    // sorted by their name, all grippers of the robot
    std::map<std::string, GazeboGraspGripper> grippers;
189

Jennifer Buehler's avatar
Jennifer Buehler committed
190
191
192
    event::ConnectionPtr update_connection;
    transport::NodePtr node;
    transport::SubscriberPtr contactSub; //subscriber to contact updates
193

194
195
196
    // tolerance (in degrees) between force vectors to
    // beconsidered "opposing"
    float forcesAngleTolerance;
197

198
199
200
    // when an object is attached, collisions with it may be disabled, in case the
    // robot still keeps wobbling.
    bool disableCollisionsOnAttach;
201

202
203
204
205
206
207
    // all collisions per gazebo collision link (each entry
    // belongs to a physics::CollisionPtr element). The key
    // is the collision link name, the value is the gripper name
    // this collision link belongs to.
    std::map<std::string, std::string> collisions;

Jennifer Buehler's avatar
Jennifer Buehler committed
208
209

    /**
210
     * Helper class to encapsulate a collision information. Forward declaration here.
Jennifer Buehler's avatar
Jennifer Buehler committed
211
212
213
     */
    class CollidingPoint;

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

219
    // when an object was first attached, it had these colliding points.
220
221
222
223
224
    // First key is object name, second is the link colliding, as in \e contacts.
    // Only the links of *one* gripper are stored here. This indirectly imposes the
    // limitation that no two grippers can grasp the object (while it would be
    // possible, the release condition is tied to only one link, so the object may
    // not be released properly).
225
226
227
228
229
230
    std::map<std::string, std::map<std::string, CollidingPoint> >
    attachGripContacts;


    // Records how many subsequent update calls the grip on that object has been recorded
    // as "holding". Every loop, if a grip is not recorded, this number decreases.
231
232
    // 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.
233
    std::map<std::string, int> gripCounts;
Jennifer Buehler's avatar
Jennifer Buehler committed
234

235
    // *maximum* number in \e gripCounts to be recorded.
236
237
    int maxGripCount;

238
239
240
241
242
243
244
245
    // 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
246
247
248
249
250
251
252
253
254

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

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

}
255
256

#endif  // GAZEBO_GAZEBOGRASPFIX_H