humotion

#include "icub_data_receiver.h"

#include <humotion/server/joint_interface.h>

#include <yarp/os/Property.h>

#include <boost/format.hpp>

using std::cout;

using std::cerr;

using std::string;

using humotion::server::JointInterface;

using yarp::dev::IEncodersTimed;

using yarp::sig::Vector;

#define ICUB_DATA_RECEIVER_USE_ENCODERSPEED 0

#define ICUB_DATA_RECEIVER_DUMP_DATA 0

//! constructor

//! \param period_ms for the yarp rate thread

//! \param icub_jointinterface

iCubDataReceiver::iCubDataReceiver(int period_ms, iCubJointInterface *icub_jointinterface)

    : yarp::os::RateThread(period_ms) {

    // store pointer to icub jointinterface

    icub_jointinterface_ = icub_jointinterface;

    //fetch iencs view from yarp

    yarp::dev::PolyDriver *poly_driver = icub_jointinterface->get_yarp_polydriver();

    bool success = poly_driver->view(iencs_);

    if (!success) {

        cerr << "ERROR: polydriver failed to init iencs view\n";

        exit(EXIT_FAILURE);

    }

    // resize data storage vectors to match the number of axes

    int joints;

    iencs_->getAxes(&joints);

    positions_.resize(joints);

    velocities_.resize(joints);

    timestamps_.resize(joints);

}

//! yarp rate thread initializer

bool iCubDataReceiver::threadInit() {

    return true;

}

//! yarp thread release function

void iCubDataReceiver::threadRelease() {

}

//! manully calculate joint velocities (instead of using joint encoder speeds)

//! \param positions vector with current encoder position

//! \param timestamps vector with the associated timestamps

//! \return velocities as vector

Vector iCubDataReceiver::calculate_velocities(Vector positions, Vector timestamps) {

    Vector velocities;

    velocities.resize(positions.size());

    if (previous_positions_.size() == 0){

        // first run, no valid old position available, return zero velocities

        // by setting all all elements to zero

        velocities = 0.0;

    }else{

        // calculate speed based on positions:

        for(int i=0; i<positions.size(); i++){

            float diff = positions[i] - previous_positions_[i];

            float timediff = timestamps[i] - previous_timestamps_[i];

            // calc speed:

            velocities[i] = diff / timediff;

        }

    }

    previous_positions_ = positions;

    previous_timestamps_ = timestamps;

    return velocities;

}

//! main loop routine, called by yarp rate thread

void iCubDataReceiver::run() {

    float velocity;

    //grab pos+vel data:

    iencs_->getEncodersTimed(positions_.data(), timestamps_.data());

    #if ICUB_DATA_RECEIVER_USE_ENCODERSPEED

    // fetch data from icub. NOTE: make sure to enable the vel broadcast in the ini file!

    if (! iencs_->getEncoderSpeeds(velocities_.data())){

        cout << "Failed to fetch encoder speeds...\n";

        return;

    }

    #else

    // manually calculate the speed based on old position:

    velocities_ = calculate_velocities(positions_, timestamps_);

    #endif

    // publish data to humotion

    for(int i=0; i<positions_.size(); i++){

        // store position values

        store_incoming_position(i, positions_[i], timestamps_[i]);

        // store velocity

        store_incoming_velocity(i, velocities_[i], timestamps_[i]);

    }

    //small hack to tell humotion to update the lid angle

    //fixme: use real id

    store_incoming_position(100, 0.0, timestamps_[0]);

    #if ICUB_DATA_RECEIVER_DUMP_DATA

    dump_incoming_data();

    #endif

}

//! store incoming position for a given icub joint

//! \param icub _id icub joint id

//! \param position

//! \param timestamp

void iCubDataReceiver::store_incoming_position(int icub_id, double position, double timestamp) {

    //cout << "iCubDataReceiver::store_incoming_position(icub=" << icub_id << ", " << position << ")\n";

    // store joint position in humotion backend

    if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||

            (icub_id == iCubJointInterface::ICUB_ID_EYES_VERGENCE)) {

        // the icub handles eyes differently

        // instead of using seperate left/right pan the icub uses

        // a combined pan angle and vergence. therfore we have to convert this here:

        if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {

            target_eye_pan_ = position;

        } else {

            target_eye_vergence_ = -position;

        }

        float right = target_eye_pan_ + target_eye_vergence_/2.0;

        float left  = target_eye_pan_ - target_eye_vergence_/2.0;

        icub_jointinterface_->store_incoming_position(JointInterface::ID_EYES_LEFT_LR,

                                                     left, timestamp);

        icub_jointinterface_->store_incoming_position(JointInterface::ID_EYES_RIGHT_LR,

                                                     right, timestamp);

    } else if (icub_id == 100) {

        //HACK

        //icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,

        //                                             lid_angle, timestamp);

    } else {

        if (icub_id == iCubJointInterface::ID_NECK_PAN) {

            // icub uses an inverted neck pan specification

            position = -position;

        }

        // known configured mapping between joint ids

        int humotion_id = icub_jointinterface_->convert_icub_jointid_to_humotion(icub_id);

        icub_jointinterface_->store_incoming_position(humotion_id, position, timestamp);

    }

}

//! store incoming velocity for a given icub joint

//! \param icub_id icub joint id

//! \param velocity

//! \param timestamp

void iCubDataReceiver::store_incoming_velocity(int icub_id, double velocity, double timestamp) {

    //cout << "iCubDataReceiver::store_incoming_velocity(icub=" << icub_id << ", " << velocity << ")\n";

    // store joint position in humotion backend

    if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||

            (icub_id == iCubJointInterface::ICUB_ID_EYES_VERGENCE)) {

        // the icub handles eyes differently

        // instead of using seperate left/right pan the icub uses

        // a combined pan angle and vergence. therfore we have to convert this here:

        if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {

            target_eye_pan_velocity_ = velocity;

        } else {

            target_eye_vergence_velocity_ = -velocity;

        }

        float right = target_eye_pan_velocity_ + target_eye_vergence_velocity_/2.0;

        float left  = target_eye_pan_velocity_ - target_eye_vergence_velocity_/2.0;

        icub_jointinterface_->store_incoming_velocity(JointInterface::ID_EYES_LEFT_LR,

                                                     left, timestamp);

        icub_jointinterface_->store_incoming_velocity(JointInterface::ID_EYES_RIGHT_LR,

                                                     right, timestamp);

    } else if (icub_id == 100) {

        //HACK

        //icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,

        //                                             lid_angle, timestamp);

    } else {

        if (icub_id == iCubJointInterface::ID_NECK_PAN) {

            // icub uses an inverted neck pan specification

            velocity = -velocity;

        }

        // known configured mapping between joint ids

        int humotion_id = icub_jointinterface_->convert_icub_jointid_to_humotion(icub_id);

        icub_jointinterface_->store_incoming_velocity(humotion_id, velocity, timestamp);

    }

}

//! helper for debugging purposes, feed this data into gnuplot for visual inspection

void iCubDataReceiver::dump_incoming_data() {

    // use gnuplot for viz:

    // ./icub_humotion_server --robot icub | grep "INCOMING" |tee  log

    // @gnuplot: plot "log" using 0:1 w l t "p neck tilt", "log" using 0:2 w l t "v neck tilt", \

    //                "log" using 0:5 w l t "p neck pan", "log" using 0:6 w l t "v neck pan", \

    //                "log" using 0:7 w l t "p eyes ud", "log" using 0:8 w l t "v eyes ud", \

    //                "log" using 0:9 w l t "p eyes vergence", "log" using 0:10 w l t "v eyes vergence"

    cout << "\n";

            // publish data to humotion

    for(int i=0; i<positions_.size(); i++){

        cout << positions_[i] << " ";

        cout << velocities_[i] << " ";

    }

    cout << " #INCOMING_DATA_DUMP\n";

}