humotion

#include "icub_data_receiver.h"

#include <humotion/server/joint_interface.h>

#include <yarp/os/Property.h>

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

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

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

    // store pointer to icub jointinterface

    icub_jointinterface_ = icub_jointinterface;

    //fetch yarp iencs view:

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

    bool success = poly_driver->view(yarp_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;

    yarp_iencs_->getAxes(&joints);

    yarp_positions_.resize(joints);

    yarp_timestamps_.resize(joints);

}

bool iCubDataReceiver::threadInit() {

    return true;

}

void iCubDataReceiver::threadRelease() {

}

void iCubDataReceiver::run() {

    //grab pos+vel data:

    yarp_iencs_->getEncodersTimed(yarp_positions_.data(), yarp_timestamps_.data());

    //iencs->getEncoderSpeeds(velocities.data());

    //publish data to humotion

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

        store_incoming_position(i, yarp_positions_[i], yarp_timestamps_[i]);

    }

    printf("\n");

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

    //fixme: use real id

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

}

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

    cout << "iCubDataReceiver::store_incoming_position(icubid=" << 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);

    }

}