humotion

#include "mekajointinterface.h"

using namespace std;

//WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED

#define POSITION_CONTROL 1

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

    //fetch current timestamp

    double timestamp = get_timestamp_ms();

    //iterate through incoming joints and filter out joints we need:

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

        string name = msg.name[i];

        //printf("incoming data for joint '%s'\n", name.c_str());

        int id = -1;

        if (name == "head_j1"){

            id = ID_NECK_PAN;

        }else if(name == "head_j0"){

            id = ID_NECK_TILT;

        }

        //store data:

        if (id != -1){

            //printf("> storing joint data for joint id %d\n", id);

            if (i >= msg.position.size()){

                printf("> joint state msg is missing position data for joint '%s'...\n", name.c_str());

                return;

            }

            if (i >= msg.velocity.size()){

                //printf("> joint state msg is missing velocity data for joint '%s'...\n", name.c_str());

                //exit(EXIT_FAILURE);

                return;

            }

            //ok, safe to access data

            JointInterface::store_incoming_position(id, msg.position[i], timestamp);

            JointInterface::store_incoming_speed(   id, msg.velocity[i], timestamp);

        }

    }

    //store dummy positions for joints we do not know about:

    store_dummy_data(ID_LIP_LEFT_UPPER, timestamp);

    store_dummy_data(ID_LIP_LEFT_LOWER, timestamp);

    store_dummy_data(ID_LIP_CENTER_UPPER, timestamp);

    store_dummy_data(ID_LIP_CENTER_LOWER, timestamp);

    store_dummy_data(ID_LIP_RIGHT_UPPER, timestamp);

    store_dummy_data(ID_LIP_RIGHT_LOWER, timestamp);

    store_dummy_data(ID_NECK_ROLL, timestamp);

    store_dummy_data(ID_EYES_BOTH_UD, timestamp);

    store_dummy_data(ID_EYES_LEFT_LR, timestamp);

    store_dummy_data(ID_EYES_RIGHT_LR, timestamp);

    store_dummy_data(ID_EYES_LEFT_LID_LOWER, timestamp);

    store_dummy_data(ID_EYES_LEFT_LID_UPPER, timestamp);

    store_dummy_data(ID_EYES_LEFT_BROW, timestamp);

    store_dummy_data(ID_EYES_RIGHT_LID_LOWER, timestamp);

    store_dummy_data(ID_EYES_RIGHT_LID_UPPER, timestamp);

    store_dummy_data(ID_EYES_RIGHT_BROW, timestamp);

}

void MekaJointInterface::store_dummy_data(int id, double timestamp){

    JointInterface::store_incoming_position(id, 0.0, timestamp);

    JointInterface::store_incoming_speed(id, 0.0, timestamp);

}

//! constructor

MekaJointInterface::MekaJointInterface(string _input_scope, string _output_scope) : humotion::server::JointInterface(){

    input_scope = _input_scope;

    output_scope = _output_scope;

    //subscribe to meka joint states

    int argc = 0;

    ros::init(argc, (char**)NULL, "meka_humotion");

    ros::NodeHandle n;

    printf("> listening on jointstates on '%s'\n",input_scope.c_str());

    joint_state_subscriber = n.subscribe(input_scope, 150, &MekaJointInterface::incoming_jointstates , this);

    printf("> sending targets on '%s'\n", output_scope.c_str());

    target_publisher = n.advertise<trajectory_msgs::JointTrajectory>(output_scope, 100);

    //tell humotion about min/max joint values:

    init_joints();

}

//! destructor

MekaJointInterface::~MekaJointInterface(){

}

void MekaJointInterface::run(){

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

   //data_receiver->start();

   ros::spin();

}

double MekaJointInterface::get_timestamp_ms(){

    struct timespec spec;

    clock_gettime(CLOCK_REALTIME, &spec);

    return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6;

}

//! 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(){

    //build msg

    trajectory_msgs::JointTrajectory msg;

    msg.joint_names.push_back("head_j0");

    msg.joint_names.push_back("head_j1");

    trajectory_msgs::JointTrajectoryPoint p;

    p.positions.push_back(joint_target[ID_NECK_TILT] * M_PI / 180.0);

    p.positions.push_back(joint_target[ID_NECK_PAN] * M_PI / 180.0);

    p.time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);

    msg.points.push_back(p);

    target_publisher.publish(msg);

    /*

     void MekaJointInterface::store_min_max(int id, float min, float max){

header:

  seq: 636

  stamp:

    secs: 0

    nsecs: 0

  frame_id: ''

joint_names: ['head_j0', 'head_j1']

points:

  -

    positions: [-0.31, 0.01954768762234005]

    velocities: []

    accelerations: []

    effort: []

    time_from_start:

      secs: 1

      nsecs: 0

     */

    #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){

    //meka does not support this, joints are always enabled

}

//! shutdown and disable a joint

//! \param the enum id of a joint

void MekaJointInterface::disable_joint(int e){

    //meka does not support this, joints are always enabled

}

void MekaJointInterface::store_min_max(int id, float min, float max){

    joint_min[id] = min;

    joint_max[id] = max;

}

void MekaJointInterface::init_joints(){

    store_min_max(ID_NECK_TILT, -10, 10);

    store_min_max(ID_NECK_PAN, -10, 10);

    store_min_max(ID_NECK_ROLL, -1, 1);

    store_min_max(ID_EYES_BOTH_UD, -1, 1);

    store_min_max(ID_EYES_LEFT_LR, -1, 1);

    store_min_max(ID_EYES_RIGHT_LR, -1, 1);

    store_min_max(ID_EYES_LEFT_LID_UPPER, -1, 1);

    store_min_max(ID_EYES_LEFT_LID_LOWER, -1, 1);

    store_min_max(ID_EYES_RIGHT_LID_UPPER, -1, 1);

    store_min_max(ID_EYES_RIGHT_LID_LOWER, -1, 1);

    store_min_max(ID_EYES_LEFT_BROW, -1, 1);

    store_min_max(ID_EYES_RIGHT_BROW, -1, 1);

    store_min_max(ID_LIP_CENTER_UPPER, -1, 1);

    store_min_max(ID_LIP_CENTER_LOWER, -1, 1);

    store_min_max(ID_LIP_LEFT_UPPER, -1, 1);

    store_min_max(ID_LIP_LEFT_LOWER, -1, 1);

    store_min_max(ID_LIP_RIGHT_UPPER, -1, 1);

    store_min_max(ID_LIP_RIGHT_LOWER, -1, 1);

}