humotion

#include "icub_jointinterface.h"

#include "icub_faceinterface.h"

#include <yarp/os/Property.h>

using std::cout;

using std::cerr;

using std::string;

//! constructor

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

    scope = _scope;

    // add mappings from icub ids to humotion ids

    init_id_map();

    // initialise the pd controller for the velocity and position mixer

    init_pv_mix_pid();

    // intantiate the face interface

    face_interface_ = new iCubFaceInterface(scope);

    // intantiate the polydriver

    yarp::os::Property options;

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

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

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

    yarp_polydriver_.open(options);

    // fetch yarp views:

    bool success = true;

    //success &= yarp_polydriver_.view(yarp_iencs_);

    //success &= yarp_polydriver_.view(yarp_ipos_);

    success &= yarp_polydriver_.view(yarp_ivel_);

    success &= yarp_polydriver_.view(yarp_ilimits_);

    success &= yarp_polydriver_.view(yarp_pid_);

    success &= yarp_polydriver_.view(yarp_amp_);

    if (!success) {

        cout << "ERROR: failed to fetch one or more yarp views... exiting\n";

        exit(EXIT_FAILURE);

    }

    //tell humotion about min/max joint values:

    init_joints();

    //initialise joint controller

    init_controller();

}

//! destructor

iCubJointInterface::~iCubJointInterface(){

    //stop velocity controller on exit

    yarp_ivel_->stop();

}

//! init the controller that allows to write target angles or velocities

void iCubJointInterface::init_controller() {

    int number_of_joints;

    // use position controller, first fetch no of axes:

    yarp_ivel_->getAxes(&number_of_joints);

    // set ref acceleration to a value for all axes:

    yarp_commands_.resize(number_of_joints);

    yarp_commands_ = 1e6;

    yarp_ivel_->setRefAccelerations(yarp_commands_.data());

    yarp_ivel_->setVelocityMode();

}

//! initialise icub joint id to humotion joint id mappings

void iCubJointInterface::init_id_map() {

        insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_LEFT_UPPER,   ID_LIP_LEFT_UPPER);

        insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_LEFT_LOWER,   ID_LIP_LEFT_LOWER);

        insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_CENTER_UPPER, ID_LIP_CENTER_UPPER);

        insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_CENTER_LOWER, ID_LIP_CENTER_LOWER);

        insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_RIGHT_UPPER,  ID_LIP_RIGHT_UPPER);

        insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_RIGHT_LOWER,  ID_LIP_RIGHT_LOWER);

        insert_icupid_to_humotionid_mapping(ICUB_ID_NECK_PAN,    ID_NECK_PAN);

        insert_icupid_to_humotionid_mapping(ICUB_ID_NECK_TILT,   ID_NECK_TILT);

        insert_icupid_to_humotionid_mapping(ICUB_ID_NECK_ROLL,   ID_NECK_ROLL);

        // FIXME: remove this hack tha repurposes LEFT/RIGHT eye pan from humotion as vergence/pan

        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_PAN,   ID_EYES_LEFT_LR);

        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_VERGENCE,   ID_EYES_RIGHT_LR);

        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_BOTH_UD,   ID_EYES_BOTH_UD);

        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_LEFT_LID_LOWER, ID_EYES_LEFT_LID_LOWER);

        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_LEFT_LID_UPPER, ID_EYES_LEFT_LID_UPPER);

        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_LEFT_BROW, ID_EYES_LEFT_BROW);

        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_RIGHT_LID_LOWER, ID_EYES_RIGHT_LID_LOWER);

        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_RIGHT_LID_UPPER,ID_EYES_RIGHT_LID_UPPER);

        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW);

}

//! initialize the position and velocity mixer PD controller

void iCubJointInterface::init_pv_mix_pid() {

    // init control variables and last error variable for the internal

    // position and velocity mixer PD controller:

    pv_mix_pid_p_.resize(ICUB_JOINT_ID_ENUM_SIZE);

    pv_mix_pid_d_.resize(ICUB_JOINT_ID_ENUM_SIZE);

    pv_mix_last_error_.resize(ICUB_JOINT_ID_ENUM_SIZE);

    enum_id_bimap_t::const_iterator it;

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

        int id = it->left;

        pv_mix_pid_p_[id] = 15.5;

        pv_mix_pid_d_[id] = 0.3;

        pv_mix_last_error_[id] = 0.0;

    }

}

//! add mapping from icub joint ids to humotion ids

//! 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 the icub motor ids.

void iCubJointInterface::insert_icupid_to_humotionid_mapping(int icubid, int humotionid) {

    enum_id_bimap.insert(enum_id_bimap_entry_t(icubid, humotionid));

}

void iCubJointInterface::run(){

    float loop_duration_ms = 1000.0 / MAIN_LOOP_FREQUENCY;

    iCubDataReceiver *data_receiver = new iCubDataReceiver(loop_duration_ms, this);

    data_receiver->start();

}

//! stores the target position & velocity of a given joint

//! \param enum id of joint

//! \param float value

void iCubJointInterface::publish_target(int humotion_id, float position, float velocity){

    // special handler for eye joints

    if ((humotion_id == JointInterface::ID_EYES_LEFT_LR) ||

        (humotion_id == JointInterface::ID_EYES_RIGHT_LR)){

        // the icub has a combined pan angle for both eyes, so seperate this:

        float target_position_left  = get_target_position(JointInterface::ID_EYES_LEFT_LR);

        float target_position_right = get_target_position(JointInterface::ID_EYES_RIGHT_LR);

        float target_velocity_left  = get_target_velocity(JointInterface::ID_EYES_LEFT_LR);

        float target_velocity_right = get_target_velocity(JointInterface::ID_EYES_RIGHT_LR);

        // calculate target angles

        float target_position_pan      = (target_position_right + target_position_left) / 2;

        float target_position_vergence = (target_position_right - target_position_left);

        cout << "LR " << target_position_left << " " << target_position_right <<

                " PAN " << target_position_pan << " VERGENCE " << target_position_vergence << "\n";

        // calculate target velocities

        // for now just use the same velocity for pan and vergence

        float target_velocity_pan = (target_velocity_left + target_velocity_right) / 2.0;

        float target_velocity_vergence = target_velocity_left - target_velocity_right;

        store_icub_joint_target(ICUB_ID_EYES_PAN,

                                target_position_pan, target_velocity_pan);

        store_icub_joint_target(ICUB_ID_EYES_VERGENCE,

                                target_position_vergence, target_velocity_vergence);

    }else{

        // convert to icub joint id

        int icub_id = convert_humotion_jointid_to_icub(humotion_id);

        // store target data

        store_icub_joint_target(icub_id, position, velocity);

    }

}

//! set the target data for a given icub joint

//! \param id of joint

//! \param float value of position

void iCubJointInterface::store_icub_joint_target(int icub_id, float position, float velocity) {

    cout << "store_icub_joint_target(" << icub_id << ", " << position << ", ..)\n";

    if ((icub_id == ICUB_ID_NECK_PAN) || (icub_id == ICUB_ID_EYES_VERGENCE)){

        // icub uses an inverted neck pan specification

        position = -position;

        velocity = -velocity;

    }

    // store values

    target_angle_[icub_id] = position;

    target_velocity_[icub_id] = velocity;

}

//! execute a move in velocity mode

//! \param id of joint

//! \param angle

void iCubJointInterface::set_target_in_velocitymode(int icub_id) {

    // fetch humotion id from icub joint id

    int humotion_id = convert_icub_jointid_to_humotion(icub_id);

    // fetch the target velocity

    float target_velocity = target_velocity_[icub_id];

    float vmax = 350.0;

    if (target_velocity > vmax)  target_velocity = vmax;

    if (target_velocity < -vmax) target_velocity = -vmax;

    //execute:

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

    if ((icub_id != ICUB_ID_NECK_PAN)

        //&& (icub_id != ICUB_ID_EYES_BOTH_UD)

        //&& (icub_id != ICUB_ID_NECK_TILT)

        && (icub_id != ICUB_ID_EYES_PAN)

        && (icub_id != ICUB_ID_EYES_VERGENCE)

        //&& (icub_id != ICUB_ID_NECK_TILT)

            ){

        // limit to some joints for debugging...

        return;

    }

    // we now add a pd control loop for velocity moves in order to handle position errors

    // TODO: add position interpolation into future. this requires to enable the velocity

    //       broadcast in the torso and head ini file and fetch that velocity in the

    //       icub_data_receiver as well. TODO: check if the can bus has enough bandwidth for that...

    // first: fetch the timstamp of the last known position

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

    // fetch current position & velocity

    float current_position = get_ts_position(humotion_id).get_interpolated_value(data_ts);

    float current_velocity = get_ts_speed(humotion_id).get_interpolated_value(data_ts);

    // the icub has a combined eye pan actuator, therefore

    // we have to calculate pan angle and vergence based on the left and right target angles:

    if (icub_id == ICUB_ID_EYES_PAN) {

        // fetch timestamp

        data_ts = get_ts_position(ID_EYES_LEFT_LR).get_last_timestamp();

        // get the left and right positions

        float pos_left  = get_ts_position(ID_EYES_LEFT_LR).get_interpolated_value(data_ts);

        float pos_right = get_ts_position(ID_EYES_RIGHT_LR).get_interpolated_value(data_ts);

        current_position = (pos_left + pos_right) / 2.0;

        // same for velocities:

        float vel_left  = get_ts_speed(ID_EYES_LEFT_LR).get_interpolated_value(data_ts);

        float vel_right = get_ts_speed(ID_EYES_RIGHT_LR).get_interpolated_value(data_ts);

        current_velocity = (vel_left + vel_right) / 2.0;

    }else if (icub_id == ICUB_ID_EYES_VERGENCE) {

        // fetch timestamp

        data_ts = get_ts_position(ID_EYES_LEFT_LR).get_last_timestamp();

        // get the left and right positions

        float pos_left  = get_ts_position(ID_EYES_LEFT_LR).get_interpolated_value(data_ts);

        float pos_right = get_ts_position(ID_EYES_RIGHT_LR).get_interpolated_value(data_ts);

        current_position = pos_left - pos_right;

        // same for velocities:

        float vel_left  = get_ts_speed(ID_EYES_LEFT_LR).get_interpolated_value(data_ts);

        float vel_right = get_ts_speed(ID_EYES_RIGHT_LR).get_interpolated_value(data_ts);

        current_velocity = (vel_left - vel_right);

    }

    // calculate position error:

    float position_error = target_angle_[icub_id] - current_position;

    // calculate d term

    float error_d = (position_error - pv_mix_last_error_[icub_id]) / (framerate*1000.0);

    pv_mix_last_error_[icub_id] = position_error;

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

    float pv_mix_velocity = pv_mix_pid_p_[icub_id] * position_error

                            + pv_mix_pid_p_[icub_id]*error_d

                            + target_velocity;

    //if (icub_id == ICUB_ID_EYES_VERGENCE) { target_velocity = 0.0; pv_mix_velocity = 0.0;}

    cout << "\n"

         << current_position << " "

         << target_angle_[icub_id] << " "

         << current_velocity << " "

         << pv_mix_velocity  << " "

         << target_velocity  << " "

         << position_error << " "

         << " PID" << icub_id << "\n";

    // execute velocity move

    bool success;

    int count = 0;

    do{

        success = yarp_ivel_->velocityMove(icub_id, pv_mix_velocity);

        if (count++ >= 10 ) {

            cerr << "ERROR: failed to send velocity move command! gave up after 10 trials\n";

            yarp_ivel_->stop();

            exit(EXIT_FAILURE);

        }

    } while(!success);

}

//! actually execute the scheduled motion commands

void iCubJointInterface::execute_motion(){

    // set up neck and eye motion commands in velocitymode

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

        set_target_in_velocitymode(i);

    }

/*

    // eyelids: unfortuantely the icub has only 1dof for eyelids

    // therefore we can only use an opening value

    float opening_left  = target_angle_[ICUB_ID_EYES_LEFT_LID_UPPER]

                          - target_angle_[ICUB_ID_EYES_LEFT_LID_LOWER];

    float opening_right = target_angle_[ICUB_ID_EYES_RIGHT_LID_UPPER]

                          - target_angle_[ICUB_ID_EYES_RIGHT_LID_LOWER];

    float opening = (opening_left + opening_right) / 2.0;

    // send it to icub face if

    face_interface_->set_eyelid_angle(opening);

    //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 = humotion::Timestamp::now().to_seconds();

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

}

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

    int icub_jointid = convert_humotion_jointid_to_icub(humotion_id);

/*

    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) && (icub_jointid <= ICUB_ID_EYES_VERGENCE)) {

        if (enable) {

            yarp_amp_->enableAmp(icub_jointid);

            yarp_pid_->enablePid(icub_jointid);

        } else {

            yarp_pid_->disablePid(icub_jointid);

            yarp_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(yarp::dev::IControlLimits *ilimits, int icub_id){

    double min, max;

    int humotion_id = convert_icub_jointid_to_humotion(icub_id);

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

    joint_min[humotion_id] = min;

    joint_max[humotion_id] = max;

}

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

void iCubJointInterface::init_joints(){

    store_min_max(yarp_ilimits_, ICUB_ID_NECK_TILT);

    store_min_max(yarp_ilimits_, ICUB_ID_NECK_ROLL);

    store_min_max(yarp_ilimits_, ICUB_ID_NECK_PAN);

    store_min_max(yarp_ilimits_, ICUB_ID_EYES_BOTH_UD);

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

    double pan_min, pan_max, vergence_min, vergence_max;

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

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

}

//! conversion table for humotion joint id to icub joint id

//! \param int value for humotion joint id from JointInterface::JOINT_ID_ENUM

//! \return int value of icub joint id

int iCubJointInterface::convert_humotion_jointid_to_icub(int humotion_id){

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

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

        //key does not exist

        cerr << "ERROR: invalid humotion joint id (" << humotion_id << ") given. can not convert this. exiting\n";

        exit(EXIT_FAILURE);

    }

    return it->second;

}

//! conversion table for icub joint id to humotion joint id

//! \param  int value of icub joint id

//! \return int value of humotion joint id from JointInterface::JOINT_ID_ENUM

int iCubJointInterface::convert_icub_jointid_to_humotion(int icub_id){

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

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

        //key does not exist

        cout << "ERROR: invalid icub joint id given. can not convert this. exiting\n";

        exit(EXIT_FAILURE);

    }

    return it->second;

}