humotion

#include "icub_data_receiver.h"

#include <humotion/server/joint_interface.h>

#include <yarp/os/Property.h>

#include <boost/format.hpp>

//using namespace yarp::dev;

//using namespace yarp::sig;

//using namespace yarp::os;

using std::cout;

using std::string;

using humotion::server::JointInterface;

using yarp::dev::IEncodersTimed;

using yarp::sig::Vector;

#define ICUB_DATA_RECEIVER_USE_ENCODERSPEED 0

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

    : yarp::os::RateThread(period) {

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

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

}

bool iCubDataReceiver::threadInit() {

    return true;

}

void iCubDataReceiver::threadRelease() {

}

yarp::sig::Vector iCubDataReceiver::calculate_velocities(yarp::sig::Vector positions,

                                                         yarp::sig::Vector timestamps) {

    yarp::sig::Vector velocities;

    velocities.resize(positions.size());

    cout << "\n";

    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;

            cout << " [" << i << "]=" <<  boost::format("%6.3f") % velocities[i];

        }

    }

    cout << " VEL\n";

    previous_positions_ = positions;

    previous_timestamps_ = timestamps;

    return velocities;

}

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!

    iencs_->getEncoderSpeeds(velocities_.data());

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

}

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

    cout << "iCubDataReceiver::store_incoming_position(icub=" << icub_id << ", " << value << ")\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_ = value;

        } else {

            target_eye_vergence_ = -value;

        }

        float left  = target_eye_pan_ + target_eye_vergence_/2.0;

        float right = 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

            value = -value;

        }

        // 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, value, 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 left  = target_eye_pan_velocity_ + target_eye_vergence_velocity_/2.0;

        float right = 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);

    }

}