humotion

    #if ICUB_DATA_RECEIVER_USE_ENCODERSPEED

    // fetch data from icub. NOTE: make sure to enable the vel broadcast in the ini file!

    #else

    // manually calculate the speed based on old position:

    velocities_ = calculate_velocities(positions_, timestamps_);

            target_eye_vergence_ = -position;

        }

        icub_jointinterface_->store_incoming_position(JointInterface::ID_EYES_LEFT_LR,

                                                     left, timestamp);

            target_eye_vergence_velocity_ = -velocity;

        }

        icub_jointinterface_->store_incoming_velocity(JointInterface::ID_EYES_LEFT_LR,

                                                     left, timestamp);

//! destructor

iCubJointInterface::~iCubJointInterface(){

}

//! init the controller that allows to write target angles or velocities

    // set ref acceleration to a value for all axes:

    yarp_commands_.resize(number_of_joints);

    yarp_ivel_->setRefAccelerations(yarp_commands_.data());

    yarp_ivel_->setVelocityMode();

}

    enum_id_bimap_t::const_iterator it;

    for(it = enum_id_bimap.begin(); it != enum_id_bimap.end(); ++it) {

        int id = it->left;

        pv_mix_pid_d_[id] = 0.3;

        pv_mix_last_error_[id] = 0.0;

    }

        // calculate target velocities

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

        float target_velocity_pan = (target_velocity_left + target_velocity_right) / 2.0;

        store_icub_joint_target(ICUB_ID_EYES_PAN,

                                target_position_pan, target_velocity_pan);

        store_icub_joint_target(ICUB_ID_EYES_VERGENCE,

    }else{

        // convert to icub joint id

        int icub_id = convert_humotion_jointid_to_icub(humotion_id);

void iCubJointInterface::store_icub_joint_target(int icub_id, float position, float velocity) {

    cout << "store_icub_joint_target(" << icub_id << ", " << position << ", ..)\n";

        // icub uses an inverted neck pan specification

        position = -position;

        velocity = -velocity;

    // fetch the target velocity

    float target_velocity = target_velocity_[icub_id];

    if (target_velocity > vmax)  target_velocity = vmax;

    if (target_velocity < -vmax) target_velocity = -vmax;

    //execute:

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

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

    // first: fetch the timstamp of the last known position

    humotion::Timestamp data_ts = get_ts_position(humotion_id).get_last_timestamp();

    // calculate position error:

    // calculate d term

    float error_d = (position_error - pv_mix_last_error_[icub_id]) / (framerate*1000.0);

                            + pv_mix_pid_p_[icub_id]*error_d

                            + target_velocity;

    cout << "\n"

         << target_angle_[icub_id] << " "

         << pv_mix_velocity  << " "

         << target_velocity  << " "

         << position_error << " "

    // execute velocity move

}

//! actually execute the scheduled motion commands

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

        set_target_in_velocitymode(i);

    }

    // eyelids: unfortuantely the icub has only 1dof for eyelids

    // therefore we can only use an opening value

    float opening_left  = target_angle_[ICUB_ID_EYES_LEFT_LID_UPPER]

    //mouth

    face_interface_->set_mouth(target_angle_);

    //store joint values which we do not handle on icub here:

    double timestamp = humotion::Timestamp::now().to_seconds();