humotion

    iCubJointInterface(std::string scope);

    ~iCubJointInterface();

    void run();

    int convert_icub_jointid_to_humotion(int id);

}

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

    // store joint position in humotion backend

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

            (icub_id == iCubJointInterface::ICUB_ID_EYES_VERGENCE)) {

        if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {

            target_eye_pan_ = value;

        } else {

        }

        float left  = target_eye_pan_ + target_eye_vergence_/2.0;

        //icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,

        //                                             lid_angle, timestamp);

    } else {

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

        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

        // calculate target velocities

        // for now just use the same velocity for pan and vergence

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

}

    assert(!POSITION_CONTROL);

    // fetch humotion id from icub joint id

    // fetch the target velocity

    float target_velocity = target_velocity_[icub_id];

    //execute:

    //ivel->velocityMove(id, speed);

        (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

//! \param id of joint

//! \param float value of position

//! \param double timestamp

}

/*

    //store joint based on id: