humotion

using std::cout;

using std::cerr;

using std::string;

using humotion::server::JointInterface;

using yarp::dev::IEncodersTimed;

using yarp::sig::Vector;

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

void iCubDataReceiver::run() {

    float velocity;

    // grab pos+vel data:

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

    // publish data to humotion

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

        // store position values

        // store velocity

    }

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

    // fixme: use real id

    #if ICUB_DATA_RECEIVER_DUMP_DATA

    dump_incoming_data();

//! \param icub _id icub joint id

//! \param position

//! \param timestamp

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

    // store joint position in humotion backend

        icub_jointinterface_->store_incoming_position(JointInterface::ID_EYES_RIGHT_LR,

                                                     right, timestamp);

    } else if (icub_id == 100) {

        // icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,

        //                                             lid_angle, timestamp);

    } else {

//! \param icub_id icub joint id

//! \param velocity

//! \param timestamp

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

    // store joint position in humotion backend

        icub_jointinterface_->store_incoming_velocity(JointInterface::ID_EYES_RIGHT_LR,

                                                     right, timestamp);

    } else if (icub_id == 100) {

        // icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,

        //                                             lid_angle, timestamp);

    } else {