HandPlugin.hh

Go to the documentation of this file.

#ifndef _HAND_PLUGIN_HH_
#define _HAND_PLUGIN_HH_

// Gazebo
#include "gazebo/common/Plugin.hh"
#include <gazebo/msgs/msgs.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>

// Custom messages
#include "hand.pb.h"

// Boost - for convenient string split
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>

namespace HandPlugin {

    // Plugin parameters instanced in SDF

    #define PARAM_NAME              "name"
    #define PARAM_TARGET            "target"
    #define PARAM_MULTIPLIER        "multiplier"
    #define PARAM_JOINT_GROUP       "actuatedJoint"
    #define PARAM_MIMIC_JOINT       "mimicJoint"
    #define PARAM_VIRTUAL_JOINTS    "virtualJoints"
    #define PARAM_CONTROLLERS       "controllers"
    #define PARAM_CTRL_REAL         "real"
    #define PARAM_CTRL_VIRTUAL      "virtual"
    #define PARAM_CTRL_TYPE         "type"
    #define PARAM_GRAVITY           "gravity"

    #define FORCE grasp::msgs::Target::FORCE
    #define POSITION grasp::msgs::Target::POSITION
    #define VELOCITY grasp::msgs::Target::VELOCITY

    // Plugin messages

    #define REQUEST_TOPIC    "~/hand"
    #define RESPONSE_TOPIC    "~/hand/response"
}

namespace gazebo {

    typedef grasp::msgs::Hand HandMsg;
    typedef const boost::shared_ptr<const grasp::msgs::Hand>
        HandMsgPtr;

    // Forward declaration of private joint group class
    class JointGroup;

    // Forward declaration of private joint force pair class
    class JointForcePair;

    // Forward declaration of private data class
    class HandPluginPrivate;

    class HandPlugin : public ModelPlugin
    {
        // Private attributes

        private: std::unique_ptr<HandPluginPrivate> data_ptr;
        private: physics::ModelPtr model;
        private: physics::WorldPtr world;
        private: event::ConnectionPtr update_connection;
        private: event::ConnectionPtr reset_connection;

        private: std::vector<JointGroup> joint_groups;
        private: std::vector<physics::JointPtr> virtual_joints;

        private: bool timer_active {false};
        private: common::Time timeout;

        private: const common::Time routine_timestep {0.02};
        private: bool routine_active {false};
        private: common::Time routine_timeout;
        private: std::vector<JointForcePair> joint_force_pairs;

        private: ignition::math::Pose3d init_pose {0,0,1,0,0,0};
        private: bool recv_msg {false};
        private: boost::shared_ptr<HandMsg const> msg_req;

        // Protected attributes

        // Public methods

        public: HandPlugin();

        public: virtual ~HandPlugin();

        public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);

        public: void onUpdate();

        public: void onReset();

        public: void onRequest(HandMsgPtr & _msg);

        // Private methods

        private: bool loadMimicJoints(sdf::ElementPtr _sdf, JointGroup & joint);

        private: bool loadJointGroups(sdf::ElementPtr _sdf);

        private: bool loadVirtualJoints(sdf::ElementPtr _sdf);

        private: bool loadControllers(sdf::ElementPtr _sdf);

        private: void imobilise();

        private: void resetJoints();

        // Tools

        private: void setupTimer(
            common::Time & timeout,
            bool & trigger,
            const common::Time & duration);

        // Requests

        private: void setPose(const ignition::math::Pose3d & pose);

        private: void updatePose(HandMsgPtr & _msg);

        private: void updateTimer(HandMsgPtr & _msg);

        private: void updateForces();

        // TODO
        private: void setPIDController(
            physics::JointControllerPtr controller,
            int type,
            physics::JointPtr joint,
            double p, double i, double d,
            double initial_value);

        private: void setPIDTarget(
            int type,
            physics::JointPtr joint,
            double value,
            bool force=false);

        private: void updatePIDTargets(HandMsgPtr & _msg);

        private: void checkReset(HandMsgPtr & _msg);

        private: void sendTimeout();

        private: void resetWorld();
    };
}

#endif