GazeboGraspFix.h 3.66 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
24
25
26
27
28
29
30
31
32
33
34
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
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
 *
 * ```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>
 *       </plugin>
 *   </gazebo>
 * ```
 *
 * \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);
    void OnUpdate();
private: 

    void InitValues();
    void OnContact(const ConstContactsPtr& ptr);

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

    /**
     * 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.
     */
    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
    
    std::map<std::string, physics::CollisionPtr> collisions;

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

    //Contact forces sorted by object name the gripper collides with, and the link colliding. 
    //This is a vector summed up over time, sum count is kept in contactSumCnt.
    std::map<std::string, std::map<std::string, CollidingPoint> > contacts; 
    boost::mutex mutexContacts; //mutex protects contacts

    //when an object was attached, it had these colliding points.
    std::map<std::string, std::map<std::string, CollidingPoint> > attachGripContacts; 
    
    //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 grip_count_threshold, it will be attached.
    //The number won't increase above max_grip_count once it has reached that number.
    std::map<std::string, int> gripCounts; 
    //maximum record in gripCounts
    int max_grip_count;    
    //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 grip_count_threshold;

    //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 release_tolerance;

    bool attached;
    std::string attachedObjName;    

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

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

}