humotion

void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){

    //joint_pos_global = msg.position[3];

    cout << msg;

}

    //subscribe to meka joint states

    ros::NodeHandle n;

    joint_state_subscriber = n.subscribe(scope + "joint_states", 150, &MekaJointInterface::incoming_jointstates , this);

void MekaJointInterface::run(){

   //iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this);

   //data_receiver->start();

}

//! set the target position of a joint

//! \param enum id of joint

//! \param float value

void MekaJointInterface::publish_target_position(int e){

    #if 0

    //first: convert humotion enum to our enum:

    int id = convert_enum_to_motorid(e);

    if (id == -1){

        return; //we are not interested in that data, so we just return here

    }

    if (id == ICUB_ID_NECK_PAN){

        //PAN seems to be swapped

        store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);

    }else if ((id == ICUB_ID_EYES_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){

        //icub handles eyes differently, we have to set pan angle + vergence

        float pan      = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;

        float vergence = (joint_target[ID_EYES_LEFT_LR]  - joint_target[ID_EYES_RIGHT_LR]);

        //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);

        store_joint(ICUB_ID_EYES_PAN, pan);

        store_joint(ICUB_ID_EYES_VERGENCE, vergence);

    }else{

        store_joint(id, joint_target[e]);

    }

    #endif

}

//! actually execute the scheduled motion commands

void MekaJointInterface::execute_motion(){

    #if 0

    // set up neck and eye motion commands:

    if (POSITION_CONTROL){

        //position control

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

            set_target_in_positionmode(i, target_angle[i]);

        }

    }else{

        //velocity control

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

            set_target_in_velocitymode(i, target_angle[i]);

        }

    }

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

    //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:

    set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);

    //eyebrows are set using a special command as well:

    set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW);

    set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW);

    //mouth

    set_mouth();

    #endif

}

//! prepare and enable a joint

//! NOTE: this should also prefill the min/max positions for this joint

//! \param the enum id of a joint

void MekaJointInterface::enable_joint(int e){

#if 0

    //FIXME ADD THIS:

    // enable the amplifier and the pid controller on each joint

    /*for (i = 0; i < nj; i++) {

       amp->enableAmp(i);

       pid->enablePid(i);

    }*/

    //set up smooth motion controller

    //step1: set up framerate

    //dev->set_register_blocking(XSC3_REGISTER_PID_RAMP, framerate, true);

    //step2: set controllertype:

    //printf("> activating smooth motion control for joint id 0x%02X (%s)\n", motor_id, joint_name.c_str());

    //dev->set_register_blocking(XSC3_REGISTER_PID_CONTROLLER, XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK, true);

    //step3: set pid controller:

    /*if ((e == ID_LIP_LEFT_UPPER) || (e == ID_LIP_LEFT_LOWER) || (e == ID_LIP_CENTER_UPPER) || (e == ID_LIP_CENTER_LOWER) || (e == ID_LIP_RIGHT_UPPER) || (e == ID_LIP_RIGHT_LOWER)){

        printf("> fixing PID i controller value for smooth motion (FIXME: restore old value!!)\n");

        dev->set_register_blocking(XSC3_REGISTER_CONST_I, 10, true);

    }*/

    //uint16_t result = dev->get_register_blocking_raw(XSC3_REGISTER_PID_CONTROLLER);

    //check if setting pid controllertype was successfull:

    /*if (result != XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK){

        printf("> failed to set smooth motion controller for joint %s (res=0x%04X)\n",joint_name.c_str(),result);

        exit(1);

    }*/

    //fetch min/max:

   // init_joint(e);

    //ok fine, now enable motor:

    //printf("> enabling motor %s\n", joint_name.c_str());

    //dev->set_register_blocking(XSC3_REGISTER_STATUS, XSC3_PROTOCOL_STATUS_BITMASK_MOTOR_ENABLE, true);

#endif

}

//! shutdown and disable a joint

//! \param the enum id of a joint

void MekaJointInterface::disable_joint(int e){

    /*

        //first: convert humotion enum to our enum:

        int motor_id = convert_enum_to_motorid(e);

        if (motor_id == -1){

            return; //we are not interested in that data, so we just return here

        }

        //fetch device:

        Device *dev = get_device(motor_id);

        printf("> FIXME: ADD DISABLE CODE\n");

        printf("> FIXME: ADD DISABLE CODE\n");

        printf("> FIXME: ADD DISABLE CODE\n");

        printf("> FIXME: ADD DISABLE CODE\n");

        printf("> FIXME: ADD DISABLE CODE\n");

        printf("> FIXME: ADD DISABLE CODE\n");

        printf("> FIXME: ADD DISABLE CODE\n");

        printf("> FIXME: ADD DISABLE CODE\n");

        printf("> FIXME: ADD DISABLE CODE\n");

        */

}

#if 0

#endif