humotion

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW));

    //init pd control variables

        PID_P[id] = 0.5;

        PID_D[id] = 0.1;

    }

void iCubJointInterface::set_target_in_velocitymode(int id){

    // set speed cacluated as in velocity + set position -> replicates smoothmotion from flobi?!

    //first: calculate necessary speed to reach the given target within the next clock tick:

    //make the motion smooth: we want to reach 85% of the target in the next iteration:

    distance = 0.85 * distance;

        //TODO: add interpolation into future!

        //humotion::Timestamp data_ts = humotion::Timestamp::now(); and extend get_interpol value with get_future_value

        double position_error = target_angle[id] - get_ts_position(id).get_interpolated_value(data_ts);

        last_position_error[id] = position_error;

        //finally do a PD loop to get the target velocity

        double target_velocity = PID_P[id] * position_error + PID_D[id]*error_d + speed;

        //if (id == ICUB_ID_NECK_PAN) speed = -speed;

        ivel->velocityMove(id, target_velocity);

    }

    target_angle_previous[id] = get_ts_position(convert_motorid_to_enum(id)).get_newest_value();

    if (POSITION_CONTROL){

        //position control

        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){

        }

    }else{

        //velocity control

        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){

        }

    }

    //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);