humotion

PROJECT(meka_humotion_server)

SET(MAIN maka_humotion_server)

set(ENV{ROS_LANG_DISABLE} "genjava")

set(ROS_BUILD_TYPE Debug)

################################################################

# check for ROS support:

find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs message_generation genmsg)

IF (catkin_FOUND)

    set(ROS_FOUND 1)

    message(STATUS "ROS Support is ON")

    add_definitions(-DROS_SUPPORT=1)

ENDIF (catkin_FOUND)

IF (NOT catkin_FOUND)

  message(FATAL_ERROR "Error: could not find ROS!")

ENDIF ()

#add_message_files(

#    FILES

#)

#generate_messages(

#    DEPENDENCIES

#    std_msgs

#)

catkin_package(

    INCLUDE_DIRS include

    LIBRARIES humotion

    #CATKIN_DEPENDS message_runtime

    #DEPENDS system_lib

)

include_directories( ${catkin_INCLUDE_DIRS})

SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${ICUB_LINK_FLAGS}")

#add_dependencies(${MAIN} ${catkin_EXPORTED_TARGETS})

ADD_EXECUTABLE(${MAIN} src/main.cpp src/mekajointinterface.cpp ${DUMMY_HEADER_LIST})

TARGET_LINK_LIBRARIES(${MAIN} ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${humotion_LIBRARIES})

#INSTALL(TARGETS ${MAIN} DESTINATION bin)

install(TARGETS ${MAIN}

    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

)

#include "ros/ros.h"

#include "sensor_msgs/JointState.h"

    //void fetch_position(int id, double value, double timestamp);

    //void fetch_speed(int id, double value, double timestamp);

    void incoming_jointstates(const sensor_msgs::JointState & msg);

    ros::Subscriber joint_state_subscriber;

<?xml version="1.0"?>

<package>

  <name>meka_humotion_server</name>

  <version>0.0.1</version>

  <description>humotion server for the mekabot</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag --> 

  <maintainer email="sschulz@todo.todo">Simon Schulz</maintainer>

  <!-- One license tag required, multiple allowed, one license per tag -->

  <!-- Commonly used license strings: -->

  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->

  <license>TODO</license>

  <!-- Url tags are optional, but mutiple are allowed, one per tag -->

  <!-- Optional attribute type can be: website, bugtracker, or repository -->

  <!-- Example: -->

  <url type="website">https://projects.cit-ec.uni-bielefeld.de/git/flobidev.core.git</url> -->

  <!-- Author tags are optional, mutiple are allowed, one per tag -->

  <!-- Authors do not have to be maintianers, but could be -->

  <!-- Example: -->

  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->

  <!-- The *_depend tags are used to specify dependencies -->

  <!-- Dependencies can be catkin packages or system dependencies -->

  <!-- Examples: -->

  <!-- Use build_depend for packages you need at compile time: -->

  <!-- Use buildtool_depend for build tool packages: -->

  <!--   <buildtool_depend>catkin</buildtool_depend> -->

  <!-- Use run_depend for packages you need at runtime: -->

  <!-- <run_depend>message_runtime</run_depend> -->

  <!-- Use test_depend for packages you need only for testing: -->

  <!--   <test_depend>gtest</test_depend> -->

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>

  <build_depend>std_msgs</build_depend>

  <!-- <run_depend>roscpp</run_depend> -->

  <!-- <run_depend>std_msgs</run_depend> -->

  <!--<build_depend>actionlib</build_depend>

  <build_depend>actionlib_msgs</build_depend>

  <run_depend>actionlib</run_depend> -->

  <!-- <run_depend>actionlib_msgs</run_depend> -->

  <!-- The export tag contains other, unspecified, tags -->

  <export>

    <!-- You can specify that this package is a metapackage here: -->

    <!-- <metapackage/> -->

    <!-- Other tools can request additional information be placed here -->

  </export>

</package>

    string scope = "/";

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

    //smoother motion

    max_accel = max_accel * 0.7; //1.0; //0.7;