Commit 0b900eee authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

added parameters and more control, tested the issue #1 reported on github

parent e4b18b27
...@@ -17,14 +17,62 @@ namespace gazebo { ...@@ -17,14 +17,62 @@ namespace gazebo {
* ```xml * ```xml
* <gazebo> * <gazebo>
* <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so"> * <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
* <!-- Palm link: The link to which the object is attached -->
* <palm_link> hand_link_name </palm_link> * <palm_link> hand_link_name </palm_link>
*
* <!-- All gripper links which collide with the object to *actively* grasp it -->
* <gripper_link> finger_index_link_1 </gripper_link> * <gripper_link> finger_index_link_1 </gripper_link>
* <gripper_link> finger_index_link_2 </gripper_link> * <gripper_link> finger_index_link_2 </gripper_link>
* <gripper_link> ... </gripper_link> * <gripper_link> ... </gripper_link>
* <!-- tolerance angle (in degrees) between two force vectors to be considered
* "opposing forces". If the angle is smaller than this, they are not opposing.
* -->
* <forces_angle_tolerance>120</forces_angle_tolerance>
*
* <!-- the rate at which all collision points are checked against the "gripping criterion".
* Note that in-between such updates, existing collisions may be collected at
* a higher rate (the Gazebo world update rate). This 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. -->
* <update_rate>5</update_rate>
*
* <!-- Number of times an object has to be detected as "gripped" in order to
* attach the object. Adjust this with the update rate. -->
* <grip_count_threshold>5</grip_count_threshold>
*
* <!-- Maximum number of times that a "grasped" condition counted for an
* object. Should be at least double of grip_count_threshold. If the object
* has been counted this amount of times in subsequent update iterations as
* "grasped", the number will not increase any further. As soon as the grasp
* criterion does not hold any more, the number will start to decrease by one time
* each time the object is detected as "not grasped" in an update iteration. This is
* like a "buffer" which, when it is full, maintains the state, and when it is empty again,
* the object is released -->
* <max_grip_count>10</max_grip_count>
*
* <!-- 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
* contacts. This can happen if the fingers "wobble" or move ever so slightly, and therefore
* the grasp is not detected as active any more. Setting this number too high will also lead
* to the object not being detached even if the grippers have opened up to release it. -->
* <release_tolerance>0.005</release_tolerance>
*
* <!-- when an object is attached, collisions with it may be disabled, in case the
* robot still keeps wobbling. -->
* <disable_collisions_on_attach>false</disable_collisions_on_attach>
*
* <!-- The gazebo topic of contacts. Should normally be left at __default_topic__ -->
* <contact_topic>__default_topic__</contact_topic>
* </plugin> * </plugin>
* </gazebo> * </gazebo>
* ``` * ```
* *
* Methods OnUpdate() and OnContact() contain detailed documentation on how the plugin works.
* Summarizing it:
* XXX TODO
*
* Current limitation:
* - Only one object can be attached per gripper.
* \author Jennifer Buehler * \author Jennifer Buehler
*/ */
class GazeboGraspFix : public ModelPlugin { class GazeboGraspFix : public ModelPlugin {
...@@ -34,18 +82,55 @@ public: ...@@ -34,18 +82,55 @@ public:
virtual ~GazeboGraspFix(); virtual ~GazeboGraspFix();
virtual void Init(); virtual void Init();
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
void OnUpdate();
private: 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" (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();
void InitValues(); void InitValues();
/**
* 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.
*/
void OnContact(const ConstContactsPtr& ptr); void OnContact(const ConstContactsPtr& ptr);
bool HandleAttach(const std::string& objName); bool HandleAttach(const std::string& objName);
void HandleDetach(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 * Checks whether any two vectors in the set have an angle greater
* lengthRatio (0..1) of the other in it's length. * 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); bool checkGrip(const std::vector<math::Vector3>& forces, float minAngleDiff, float lengthRatio);
...@@ -59,7 +144,16 @@ private: ...@@ -59,7 +144,16 @@ private:
event::ConnectionPtr update_connection; event::ConnectionPtr update_connection;
transport::NodePtr node; transport::NodePtr node;
transport::SubscriberPtr contactSub; //subscriber to contact updates transport::SubscriberPtr contactSub; //subscriber to contact updates
// tolerance (in degrees) between force vectors to
// beconsidered "opposing"
float forcesAngleTolerance;
// 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
std::map<std::string, physics::CollisionPtr> collisions; std::map<std::string, physics::CollisionPtr> collisions;
/** /**
...@@ -68,31 +162,36 @@ private: ...@@ -68,31 +162,36 @@ private:
*/ */
class CollidingPoint; class CollidingPoint;
//Contact forces sorted by object name the gripper collides with, and the link colliding. // Contact forces sorted by object name the gripper collides with (first key)
//This is a vector summed up over time, sum count is kept in contactSumCnt. // and the link colliding (second key).
std::map<std::string, std::map<std::string, CollidingPoint> > contacts; std::map<std::string, std::map<std::string, CollidingPoint> > contacts;
boost::mutex mutexContacts; //mutex protects contacts boost::mutex mutexContacts; //mutex protects contacts
//when an object was attached, it had these colliding points. // when an object was first attached, it had these colliding points.
// Key is object name.
std::map<std::string, std::map<std::string, CollidingPoint> > attachGripContacts; 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 // 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. // as "holding". Every loop, if a grip is not recorded, this number decreases.
//When it reaches grip_count_threshold, it will be attached. // 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. // The number won't increase above max_grip_count once it has reached that number.
std::map<std::string, int> gripCounts; 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 // *maximum* number in \e gripCounts to be recorded.
//object is remembered. As soon as this distance changes more than release_tolerance, int maxGripCount;
//the object is released.
float release_tolerance; // 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;
// flag holding whether an object is attached. Object name in \e attachedObjName
bool attached; bool attached;
// name of the object currently attached.
std::string attachedObjName; std::string attachedObjName;
//nano seconds between two updates //nano seconds between two updates
......
This diff is collapsed.
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment