GazeboGraspFix.h

Go to the documentation of this file.

//
// Created by tom on 19/09/16.
//

#ifndef ROBOTICAN_COMMON_GAZEBOGRASPFIX_H
#define ROBOTICAN_COMMON_GAZEBOGRASPFIX_H

#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>
#include <robotican_common/GazeboGraspGripper.h>
// #include <gazebo_grasp_plugin/CollidingPoint.h>

namespace gazebo {

    class GazeboGraspFix : public ModelPlugin {
    public:
        GazeboGraspFix();
        GazeboGraspFix(physics::ModelPtr _model);
        virtual ~GazeboGraspFix();

        virtual void OnAttach(const std::string& objectName, const std::string& armName){}
        virtual void OnDetach(const std::string& objectName, const std::string& armName){}

    private:
        virtual void Init();
        virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
        void OnUpdate();

        void InitValues();


        void OnContact(const ConstContactsPtr& ptr);

        bool checkGrip(const std::vector<math::Vector3>& forces, float minAngleDiff, float lengthRatio);

        bool isGripperLink(const std::string& linkName, std::string& gripperName) const;

        std::map<std::string, std::string> getAttachedObjects() const;

        class ObjectContactInfo;

        bool objectAttachedToGripper(const ObjectContactInfo& objContInfo, std::string& attachedToGripper) const;

        bool objectAttachedToGripper(const std::string& gripperName, std::string& attachedToGripper) const;


        // physics::ModelPtr model;
        // physics::PhysicsEnginePtr physics;
        physics::WorldPtr world;

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

        event::ConnectionPtr update_connection;
        transport::NodePtr node;
        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;

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


        class CollidingPoint;

        // Contact forces sorted by object name the gripper collides with (first key)
        // and the link colliding (second key).
        std::map<std::string, std::map<std::string, CollidingPoint> > contacts;
        boost::mutex mutexContacts; //mutex protects contacts

        // when an object was first attached, it had these colliding points.
        // 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).
        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.
        // 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.
        std::map<std::string, int> gripCounts;

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

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

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

}


#endif //ROBOTICAN_COMMON_GAZEBOGRASPFIX_H