humotion

#pragma once

#include <humotion/server/joint_interface.h>

#include <boost/bimap.hpp>

#include <yarp/dev/PolyDriver.h>

#include <yarp/dev/IControlMode.h>

#include <yarp/dev/IControlLimits2.h>

#include <yarp/dev/ControlBoardInterfaces.h>

#include <yarp/os/Time.h>

#include <yarp/sig/Vector.h>

#include "icub_jointinterface.h"

#include <humotion/server/server.h>

#include <yarp/os/Network.h>

#include <yarp/os/RateThread.h>

#include <yarp/os/Time.h>

#include <yarp/os/Property.h>

#include <yarp/os/Port.h>

#include <yarp/dev/ControlBoardInterfaces.h>

#include "icub_data_receiver.h"

class iCubDataReceiver;

class iCubFaceInterface;

class iCubJointInterface : public humotion::server::JointInterface{

public:

    iCubJointInterface(std::string scope);

    ~iCubJointInterface();

    //void fetch_position(Device *dev, double timestamp);

    //void fetch_speed(Device *dev, double timestamp);

    void fetch_position(int id, double value, double timestamp);

    void fetch_speed(int id, double value, double timestamp);

    void run();

    enum JOINT_ID_ENUM{

        ICUB_ID_NECK_TILT = 0,

        ICUB_ID_NECK_ROLL = 1,

        ICUB_ID_NECK_PAN  = 2,

        ICUB_ID_EYES_BOTH_UD = 3,

        ICUB_ID_EYES_PAN = 4,

        ICUB_ID_EYES_VERGENCE = 5,

        //the following ids are used for remapping:

        //ICUB_ID_EYES_LEFT_LR,

        //ICUB_ID_EYES_RIGHT_LR,

        ICUB_ID_EYES_LEFT_LID_LOWER,

        ICUB_ID_EYES_LEFT_LID_UPPER,

        ICUB_ID_EYES_RIGHT_LID_LOWER,

        ICUB_ID_EYES_RIGHT_LID_UPPER,

        ICUB_ID_EYES_LEFT_BROW,

        ICUB_ID_EYES_RIGHT_BROW,

        ICUB_ID_LIP_LEFT_UPPER,

        ICUB_ID_LIP_LEFT_LOWER,

        ICUB_ID_LIP_CENTER_UPPER,

        ICUB_ID_LIP_CENTER_LOWER,

        ICUB_ID_LIP_RIGHT_UPPER,

        ICUB_ID_LIP_RIGHT_LOWER,

        ICUB_JOINT_ID_ENUM_SIZE

    };

    static const int MAIN_LOOP_FREQUENCY = 50;

protected:

    void disable_joint(int e);

    void publish_target_position(int e);

    void enable_joint(int e);

    void execute_motion();

private:

    void set_joint_enable_state(int e, bool enabled);

    double get_timestamp_ms();

    double target_angle[ICUB_JOINT_ID_ENUM_SIZE];

    double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];

    iCubDataReceiver *icub_data_receiver;

    void init_joints();

    std::string scope;

    yarp::dev::PolyDriver dd;

    yarp::sig::Vector positions;

    yarp::sig::Vector velocities;

    yarp::sig::Vector commands;

    yarp::dev::IVelocityControl *ivel;

    yarp::dev::IPositionControl *ipos;

    yarp::dev::IEncodersTimed *iencs;

    yarp::dev::IControlLimits *ilimits;

    yarp::dev::IAmplifierControl *amp;

    yarp::dev::IPidControl *pid;

    yarp::dev::IControlMode *icontrol;

    std::vector<double> last_position_error;

    std::vector<double> PID_P;

    std::vector<double> PID_D;

    void store_min_max(yarp::dev::IControlLimits *ilimits, int id, int e);

    float last_pos_eye_vergence;

    float last_pos_eye_pan;

    float last_vel_eye_vergence;

    float last_vel_eye_pan;

    void store_joint(int id, float value);

    void set_target_in_positionmode(int id);

    void set_target_in_velocitymode(int id);

    int convert_enum_to_motorid(int e);

    int convert_motorid_to_enum(int id);

    iCubFaceInterface *face_interface;

    typedef boost::bimap<int, int > enum_id_bimap_t;

    typedef enum_id_bimap_t::value_type enum_id_bimap_entry_t;

    enum_id_bimap_t enum_id_bimap;

};