humotion

#include "icub_jointinterface.h"

#include "icub_faceinterface.h"

#include <yarp/os/Property.h>

using namespace yarp::dev;

using namespace yarp::sig;

using namespace yarp::os;

using namespace std;

/*running:

/media/local_data/sschulz/iros15/icub-nightly/share/iCub/contexts/simConfig:> iCub_SIM

/media/local_data/sschulz/iros15/icub-nightly/share/iCub/contexts/simFaceExpressions:> ../../../../bin/simFaceExpressions

yarp connect /face/eyelids /icubSim/face/eyelids

yarp connect /face/image/out /icubSim/texture/face

TEST: yarp write /writer /icubSim/face/raw/in

http://wiki.icub.org/wiki/Motor_control

*/

//WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED

#define POSITION_CONTROL 0

//! constructor

iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointInterface(){

    scope = _scope;

    face_interface = new iCubFaceInterface(scope);

    //add mapping from ids to enums:

    //this might look strange at the first sight but we need to have a generic

    //way to acces joints from libhumotion. therefore the lib uses its enum with ID_* enum ids

    //to access the joints. now we need to define a mapping to map those to our motor ids.

    //this is what we use the enum bimap for (convertion fro/to motorid is handled

    //by \sa convert_enum_to_motorid() and \sa convert_motorid_to_enum() lateron

    //MOUTH

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_UPPER,   ID_LIP_LEFT_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_LOWER,   ID_LIP_LEFT_LOWER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_UPPER, ID_LIP_CENTER_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_LOWER, ID_LIP_CENTER_LOWER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_UPPER,  ID_LIP_RIGHT_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_LOWER,  ID_LIP_RIGHT_LOWER));

    //NECK

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_PAN,    ID_NECK_PAN));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_TILT,   ID_NECK_TILT));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_ROLL,   ID_NECK_ROLL));

    //EYES

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_PAN,   ID_EYES_LEFT_LR));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_VERGENCE,   ID_EYES_RIGHT_LR));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_BOTH_UD,   ID_EYES_BOTH_UD));

    //EYELIDS

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_LOWER, ID_EYES_LEFT_LID_LOWER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_UPPER, ID_EYES_LEFT_LID_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_BROW, ID_EYES_LEFT_BROW));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_LOWER, ID_EYES_RIGHT_LID_LOWER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_UPPER,ID_EYES_RIGHT_LID_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW));

    //init pd control variables

    enum_id_bimap_t::const_iterator it;

    last_position_error.resize(ICUB_JOINT_ID_ENUM_SIZE);

    PID_P.resize(ICUB_JOINT_ID_ENUM_SIZE);

    PID_D.resize(ICUB_JOINT_ID_ENUM_SIZE);

    for(it = enum_id_bimap.begin(); it != enum_id_bimap.end(); ++it) {

        int id = it->left;

        last_position_error[id] = 0.0;

        PID_P[id] = 0.5;

        PID_D[id] = 0.1;

    }

    Property options;

    options.put("device", "remote_controlboard");

    options.put("local", "/local/head");

    options.put("remote", scope+"/head");

    dd.open(options);

    //fetch views:

    dd.view(iencs);

    dd.view(ipos);

    dd.view(ivel);

    dd.view(ilimits);

    dd.view(pid);

    dd.view(amp);

    if ( (!iencs) || (!ipos) || (!ilimits) || (!ivel) || (!amp) || (!pid)){

        printf("> ERROR: failed to open icub views\n");

        exit(EXIT_FAILURE);

    }

    int joints;

    //tell humotion about min/max joint values:

    init_joints();

    iencs->getAxes(&joints);

    positions.resize(joints);

    velocities.resize(joints);

    commands.resize(joints);

    //set position mode:

    if (POSITION_CONTROL){

        commands=200000.0;

        ipos->setRefAccelerations(commands.data());

        ipos->setPositionMode();

    }else{

        ivel->setVelocityMode();

        commands=100.0;

        ivel->setRefAccelerations(commands.data());

    }

}

//! destructor

iCubJointInterface::~iCubJointInterface(){

}

//! conversion table for humotion motor ids to our ids:

//! \param enum from JointInterface::JOINT_ID_ENUM

//! \return int value of motor id

int iCubJointInterface::convert_enum_to_motorid(int e){

    enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(e);

    if(it == enum_id_bimap.right.end()) {

        //key does not exists, we are not interested in that dataset, return -1

        return -1;

    }

    return it->second;

}

//! conversion table for our ids to humotion motor ids:

//! \param  int value of motor id

//! \return enum from JointInterface::JOINT_ID_ENUM

int iCubJointInterface::convert_motorid_to_enum(int id){

    enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(id);

    if(it == enum_id_bimap.left.end()) {

        //key does not exists, we are not interested in that dataset, return -1

        return -1;

    }

    return it->second;

}

void iCubJointInterface::run(){

    iCubDataReceiver *data_receiver = new iCubDataReceiver(0.5 * 1000.0 / MAIN_LOOP_FREQUENCY, iencs, this);

    data_receiver->start();

}

//! set the target position of a joint

//! \param enum id of joint

//! \param float value

void iCubJointInterface::publish_target_position(int e){

    //first: convert humotion enum to our enum:

    int id = convert_enum_to_motorid(e);

    if (id == -1){

        return; //we are not interested in that data, so we just return here

    }

    if (id == ICUB_ID_NECK_PAN){

        //PAN seems to be swapped

        store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);

    }else if ((id == ICUB_ID_EYES_PAN) || ( id == ICUB_ID_EYES_VERGENCE)){

        //icub handles eyes differently, we have to set pan angle + vergence

        float pan      = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;

        float vergence = (joint_target[ID_EYES_LEFT_LR]  - joint_target[ID_EYES_RIGHT_LR]);

        //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);

        store_joint(ICUB_ID_EYES_PAN, pan);

        store_joint(ICUB_ID_EYES_VERGENCE, vergence);

    }else{

        store_joint(id, joint_target[e]);

    }

}

//! set the target position of a joint

//! \param id of joint

//! \param float value of position

void iCubJointInterface::store_joint(int id, float value){

    printf("> set joint %d = %f\n",id,value);

    target_angle[id] = value;

}

//! execute a move in position mode

//! \param id of joint

//! \param angle

void iCubJointInterface::set_target_in_positionmode(int id){

    double target = target_angle[id];

    if (id>ICUB_ID_EYES_VERGENCE){

        printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,target);

        return;

    }

    // execute motion as position control cmd

    ipos->positionMove(id, target);

}

//! execute a move in velocity mode

//! \param id of joint

//! \param angle

void iCubJointInterface::set_target_in_velocitymode(int id){

    // set speed cacluated as in velocity + set position -> replicates smoothmotion from flobi?!

    //first: calculate necessary speed to reach the given target within the next clock tick:

    double distance = target_angle[id] - target_angle_previous[id];

    //make the motion smooth: we want to reach 85% of the target in the next iteration:

    distance = 0.85 * distance;

    //distance = -5.0 / 50.0;

    //calculate speed

    //double speed = distance * ((double)MAIN_LOOP_FREQUENCY);

    int e = convert_motorid_to_enum(id);

    double speed = joint_target_speed[e];

    double max = 150.0;

    if (speed > max) speed = max;

    if (speed < -max) speed = -max;

    //speed = -speed;

    // find out the latency between incoming data and now:

    float latency = get_ts_speed(e).get_last_timestamp().to_seconds() - humotion::Timestamp::now().to_seconds();

    printf("TS DIFF %fms\n",latency*1000.0);

    //execute:

    //ivel->velocityMove(id, speed);

    if ((id == ICUB_ID_NECK_PAN)  || (id == ICUB_ID_EYES_BOTH_UD) || (id == ICUB_ID_NECK_TILT) || (id == ICUB_ID_EYES_BOTH_UD) ||  (id == ICUB_ID_NECK_TILT) ){

        //do a pd control for velocity moves that incorporates position errors:

        humotion::Timestamp data_ts = get_ts_position(e).get_last_timestamp();

        //TODO: add interpolation into future!

        //humotion::Timestamp data_ts = humotion::Timestamp::now(); and extend get_interpol value with get_future_value

        double position_error = target_angle[id] - get_ts_position(e).get_interpolated_value(data_ts);

        double error_d = (position_error - last_position_error[id]) / (framerate*1000.0);

        last_position_error[id] = position_error;

        //finally do a PD loop to get the target velocity

        double target_velocity = PID_P[id] * position_error + PID_D[id]*error_d + speed;

        printf("%f %f %f %f %f %f PID%d\n",

               get_ts_position(e).get_interpolated_value(data_ts),

               target_angle[id],

               get_ts_speed(e).get_interpolated_value(data_ts),

               target_velocity,

               speed,

               position_error,

               id

               );

        //if (id == ICUB_ID_NECK_PAN) speed = -speed;

        ivel->velocityMove(id, target_velocity);

        printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],target_angle[id],distance,speed);

    }

    target_angle_previous[id] = get_ts_position(convert_motorid_to_enum(id)).get_newest_value();

}

//! actually execute the scheduled motion commands

void iCubJointInterface::execute_motion(){

    // set up neck and eye motion commands:

    if (POSITION_CONTROL){

        //position control

        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){

            set_target_in_positionmode(i);

        }

    }else{

        //velocity control

        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){

            set_target_in_velocitymode(i);

        }

    }

    //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);

    //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:

    face_interface->set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);

    //eyebrows are set using a special command as well:

    face_interface->set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW, target_angle);

    face_interface->set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW, target_angle);

    //mouth

    face_interface->set_mouth(target_angle);

    //store joint values which we do not handle on icub here:

    double timestamp = get_timestamp_ms();

    JointInterface::store_incoming_position(ID_LIP_LEFT_UPPER,   target_angle[ICUB_ID_LIP_LEFT_UPPER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_LEFT_LOWER,   target_angle[ICUB_ID_LIP_LEFT_LOWER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_CENTER_UPPER, target_angle[ICUB_ID_LIP_CENTER_UPPER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_CENTER_LOWER, target_angle[ICUB_ID_LIP_CENTER_LOWER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_RIGHT_UPPER,  target_angle[ICUB_ID_LIP_RIGHT_UPPER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_RIGHT_LOWER,  target_angle[ICUB_ID_LIP_RIGHT_LOWER], timestamp);

}

double iCubJointInterface::get_timestamp_ms(){

    struct timespec spec;

    clock_gettime(CLOCK_REALTIME, &spec);

    return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6;

}

//! set the current position of a joint

//! \param id of joint

//! \param float value of position

//! \param double timestamp

void iCubJointInterface::fetch_position(int id, double value, double timestamp){

    //store joint based on id:

    switch(id){

        default:

            printf("> ERROR: unhandled joint id %d\n",id);

            return;

        case(100):

            //JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp);

            break;

        case(ICUB_ID_NECK_PAN):

            //PAN is inverted!

            JointInterface::store_incoming_position(ID_NECK_PAN, value, timestamp);

            break;

        case(ICUB_ID_NECK_TILT):

            JointInterface::store_incoming_position(ID_NECK_TILT, value, timestamp);

            break;

        case(ICUB_ID_NECK_ROLL):

            JointInterface::store_incoming_position(ID_NECK_ROLL, value, timestamp);

            break;

        case(ICUB_ID_EYES_BOTH_UD):

            JointInterface::store_incoming_position(ID_EYES_BOTH_UD, value, timestamp);

            break;

        //icub handles eyes differently, we have to set pan angle + vergence

        case(ICUB_ID_EYES_PAN): {//pan

            last_pos_eye_pan = value;

            float left  = last_pos_eye_pan + last_pos_eye_vergence/2.0;

            float right = last_pos_eye_pan - last_pos_eye_vergence/2.0;

            //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);

            JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp);

            JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp);

            break;

        }

        case(ICUB_ID_EYES_VERGENCE): { //vergence

            last_pos_eye_vergence = value;

            float left  = last_pos_eye_pan + last_pos_eye_vergence/2.0;

            float right = last_pos_eye_pan - last_pos_eye_vergence/2.0;

            //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);

            JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp);

            JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp);

            break;

        }

    }

}

//! set the current speed of a joint

//! \param enum id of joint

//! \param float value of speed

//! \param double timestamp

void iCubJointInterface::fetch_speed(int id, double value, double timestamp){

    switch(id){

        default:

            printf("> ERROR: unhandled joint id %d\n",id);

            return;

        case(ICUB_ID_NECK_PAN):

            //PAN IS INVERTED

            JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp);

            break;

        case(ICUB_ID_NECK_TILT):

            JointInterface::store_incoming_speed(ID_NECK_TILT, value, timestamp);

            break;

        case(ICUB_ID_NECK_ROLL):

            JointInterface::store_incoming_speed(ID_NECK_ROLL, value, timestamp);

            break;

        case(ICUB_ID_EYES_BOTH_UD):

            JointInterface::store_incoming_speed(ID_EYES_BOTH_UD, value, timestamp);

            break;

        //icub handles eyes differently, we have to set pan angle + vergence

        case(ICUB_ID_EYES_PAN): {//pan

            last_vel_eye_pan = value;

            float left  = last_vel_eye_pan + last_vel_eye_vergence/2.0;

            float right = last_vel_eye_pan - last_vel_eye_vergence/2.0;

            //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);

            JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp);

            JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp);

            break;

        }

        case(ICUB_ID_EYES_VERGENCE): { //vergence

            last_vel_eye_pan = value;

            float left  = last_vel_eye_pan + last_vel_eye_vergence/2.0;

            float right = last_vel_eye_pan - last_vel_eye_vergence/2.0;

            //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);

            JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp);

            JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp);

            break;

        }

    }

}

void iCubJointInterface::set_joint_enable_state(int e, bool enable) {

    int icub_jointid = -1;

    switch(e){

        default:

            break;

    case(ID_NECK_PAN):

        icub_jointid = ICUB_ID_NECK_PAN;

        break;

    case(ID_NECK_TILT):

        icub_jointid = ICUB_ID_NECK_TILT;

        break;

    case(ID_NECK_ROLL):

        icub_jointid = ICUB_ID_NECK_ROLL;

        break;

    case(ID_EYES_BOTH_UD):

        icub_jointid = ICUB_ID_EYES_BOTH_UD;

        break;

    // icub handles eyes as pan angle + vergence...

    // -> hack: left eye enables pan and right eye enables vergence

    case(ID_EYES_LEFT_LR):

        icub_jointid = ICUB_ID_EYES_PAN;

        break;

    case(ID_EYES_RIGHT_LR):

        icub_jointid = ICUB_ID_EYES_VERGENCE;

        break;

    }

    if (icub_jointid != -1) {

        if (enable) {

            amp->enableAmp(icub_jointid);

            pid->enablePid(icub_jointid);

        } else {

            pid->disablePid(icub_jointid);

            amp->disableAmp(icub_jointid);

        }

    }

}

//! prepare and enable a joint

//! NOTE: this should also prefill the min/max positions for this joint

//! \param the enum id of a joint

void iCubJointInterface::enable_joint(int e){

    set_joint_enable_state(e, true);

}

//! shutdown and disable a joint

//! \param the enum id of a joint

void iCubJointInterface::disable_joint(int e){

    set_joint_enable_state(e, false);

}

void iCubJointInterface::store_min_max(IControlLimits *ilimits, int id, int e){

    double min, max;

    ilimits->getLimits(id, &min, &max);

    joint_min[e] = min;

    joint_max[e] = max;

}

//! initialise a joint (set up controller mode etc)

//! \param joint enum

void iCubJointInterface::init_joints(){

    store_min_max(ilimits, ICUB_ID_NECK_TILT, ID_NECK_TILT);

    store_min_max(ilimits, ICUB_ID_NECK_ROLL, ID_NECK_ROLL);

    store_min_max(ilimits, ICUB_ID_NECK_PAN, ID_NECK_PAN);

    store_min_max(ilimits, ICUB_ID_EYES_BOTH_UD, ID_EYES_BOTH_UD);

    //icub handles eyes differently, we have to set pan angle + vergence

    double pan_min, pan_max, vergence_min, vergence_max;

    ilimits->getLimits(ICUB_ID_EYES_PAN, &pan_min, &pan_max);

    ilimits->getLimits(ICUB_ID_EYES_VERGENCE, &vergence_min, &vergence_max);

    //this is not 100% correct, should be fixed:

    joint_min[ID_EYES_LEFT_LR] = pan_min; // - vergence_max/2;

    joint_max[ID_EYES_LEFT_LR] = pan_max; // - vergence_max/2;

    joint_min[ID_EYES_RIGHT_LR] = joint_min[ID_EYES_LEFT_LR];

    joint_max[ID_EYES_RIGHT_LR] = joint_max[ID_EYES_LEFT_LR];

    //eyelids:

    joint_min[ID_EYES_RIGHT_LID_UPPER] = -50; //24-30;

    joint_max[ID_EYES_RIGHT_LID_UPPER] = 50; //48-30;

    //lid_angle = joint_max[ID_EYES_RIGHT_LID_UPPER];

    //eyebrows:

    joint_min[ID_EYES_LEFT_BROW] = -50;

    joint_max[ID_EYES_LEFT_BROW] = 50;

    joint_min[ID_EYES_RIGHT_BROW] = joint_min[ID_EYES_LEFT_BROW];

    joint_max[ID_EYES_RIGHT_BROW] = joint_max[ID_EYES_LEFT_BROW];

    //mouth:

    joint_min[ID_LIP_CENTER_UPPER] = 5;

    joint_max[ID_LIP_CENTER_UPPER] = 50;

    joint_min[ID_LIP_CENTER_LOWER] = 5;

    joint_max[ID_LIP_CENTER_LOWER] = 50;

    joint_min[ID_LIP_LEFT_UPPER] = 5;

    joint_max[ID_LIP_LEFT_UPPER] = 50;

    joint_min[ID_LIP_LEFT_LOWER] = 5;

    joint_max[ID_LIP_LEFT_LOWER] = 50;

    joint_min[ID_LIP_RIGHT_UPPER] = 5;

    joint_max[ID_LIP_RIGHT_UPPER] = 50;

    joint_min[ID_LIP_RIGHT_LOWER] = 5;

    joint_max[ID_LIP_RIGHT_LOWER] = 50;

}