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

*/

#define POSITION_CONTROL 0

using std::cout;

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

    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(){

}

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

void iCubJointInterface::init_controller() {

    int number_of_joints;

    //set position mode:

    if (POSITION_CONTROL){

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

        yarp_ipos_->getAxes(&number_of_joints);

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

        yarp_commands_.resize(number_of_joints);

        yarp_commands_ = 200000.0;

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

        yarp_ipos_->setPositionMode();

    }else{

        // 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_=300.0;

        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] = 4.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_left + target_position_right) / 2;

        float target_position_vergence = (target_position_left - target_position_right);

        // 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_tilt = target_velocity_pan;

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

    }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) {

    printf("> set icub joint %d -> p = %f, v = %f\n",icub_id,position,velocity);

    target_angle_[icub_id] = position;

    target_velocity_[icub_id] = velocity;

}

//! execute a move in position mode

//! \param id of joint

//! \param angle

void iCubJointInterface::set_target_in_positionmode(int id) {

    assert(POSITION_CONTROL);

    assert(id<=ICUB_ID_EYES_VERGENCE);

    // fetch the current target position

    float target = target_angle_[id];

    // execute motion as position control cmd

    yarp_ipos_->positionMove(id, target);

}

//! execute a move in velocity mode

//! \param id of joint

//! \param angle

void iCubJointInterface::set_target_in_velocitymode(int icub_id) {

    assert(!POSITION_CONTROL);

    // fetch humotion id from icub joint id

    int humotion_id = convert_humotion_jointid_to_icub(icub_id);

    // fetch the target velocity

    float target_velocity = target_velocity_[icub_id];

    float vmax = 150.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_BOTH_UD) &&

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

    // calculate position error:

    float position_error = target_angle_[icub_id] - get_ts_position(humotion_id).get_interpolated_value(data_ts);

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

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

           get_ts_position(humotion_id).get_interpolated_value(data_ts),

           target_angle_[icub_id],

           123.4, //NOT USED ANYMORE//get_ts_speed(humotion_id).get_interpolated_value(data_ts),

           pv_mix_velocity,

           target_velocity,

           position_error,

           icub_id

           );

    // execute velocity move

    yarp_ivel_->velocityMove(icub_id, pv_mix_velocity);

}

//! 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 = 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);

}

//! set the current position of a joint

//! \param id of joint

//! \param float value of position

//! \param double timestamp

void iCubJointInterface::store_incoming_position(int humotion_id, double value, double timestamp){

    JointInterface::store_incoming_position(humotion_id, value, 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;

        }

    }

}*/

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

            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(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(yarp_ilimits_, ICUB_ID_NECK_TILT, ID_NECK_TILT);

    store_min_max(yarp_ilimits_, ICUB_ID_NECK_ROLL, ID_NECK_ROLL);

    store_min_max(yarp_ilimits_, ICUB_ID_NECK_PAN, ID_NECK_PAN);

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

    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

        cout << "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;

}