humotion

PROJECT(meka_humotion_server)

cmake_minimum_required(VERSION 2.8)

SET(MAIN ${PROJECT_NAME})

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 ()

FIND_PACKAGE(Boost REQUIRED COMPONENTS system thread)

FIND_PACKAGE(humotion)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")

#add_message_files(

#    FILES

#)

#generate_messages(

#    DEPENDENCIES

#    std_msgs

#)

#hack to allow sub dir calls work

catkin_package(

    INCLUDE_DIRS include

    LIBRARIES humotion

    #CATKIN_DEPENDS message_runtime

    #DEPENDS system_lib

)

include_directories( ${catkin_INCLUDE_DIRS})

# add include directories

INCLUDE_DIRECTORIES(${YARP_INCLUDE_DIRS} ${ICUB_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} include/)

link_directories(${Boost_LIBRARY_DIRS} ${humotion_LIBRARY_DIRS})

# add required linker flags

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

file(GLOB DUMMY_HEADER_LIST RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} include/*.h)

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

set_property(TARGET ${MAIN} PROPERTY INSTALL_RPATH_USE_LINK_PATH TRUE)

#INSTALL(TARGETS ${MAIN} DESTINATION bin)

install(TARGETS ${MAIN}

    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

    RUNTIME DESTINATION bin 

)

#pragma once

#include <humotion/server/joint_interface.h>

#include <humotion/server/server.h>

#include <boost/bimap.hpp>

//#include "meka_data_receiver.h"

//class MekaDataReceiver;

#include "ros/ros.h"

#include "sensor_msgs/JointState.h"

#include "trajectory_msgs/JointTrajectory.h"

#include <m3meka_msgs/M3ControlStates.h>

class MekaJointInterface : public humotion::server::JointInterface{

public:

    MekaJointInterface(std::string _input_scope, std::string control_scope, std::string _output_scope);

    ~MekaJointInterface();

    //void fetch_position(Device *dev, double timestamp);

    //void fetch_speed(Device *dev, double timestamp);

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

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

    void run();

    static const int MAIN_LOOP_FREQUENCY = 50;

protected:

    void disable_joint(int e);

    void publish_target(int e, float position, float velocity);

    void enable_joint(int e);

    void execute_motion();

private:

    void incoming_controlstate(const m3meka_msgs::M3ControlStates &control_state);

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

    void store_dummy_data(int id, double timestamp);

    void store_min_max(int id, float min, float max);

    ros::Subscriber joint_state_subscriber;

    ros::Subscriber control_state_subscriber;

    ros::Publisher target_publisher;

    double get_timestamp_s();

    void set_eyelid_angle(double angle);

    void set_eyebrow_angle(int id);

    void set_mouth();

    //iCubDataReceiver *icub_data_receiver;

    void init_joints();

    double lid_angle;

    int lid_opening_previous;

    int previous_mouth_state;

    std::string input_scope;

    std::string control_scope;

    std::string output_scope;

    float last_pos_eye_vergence;

    float last_pos_eye_pan;

    float last_vel_eye_vergence;

    float last_vel_eye_pan;

    bool controller_enabled;

    void store_joint(int id, float value);

    void set_target_in_positionmode(int id, double value);

    void set_target_in_velocitymode(int id, double value);

    int convert_enum_to_motorid(int e);

    int convert_motorid_to_enum(int id);

    typedef boost::bimap<int, int > enum_id_bimap_t;

    typedef enum_id_bimap_t::value_type enum_id_bimap_entry_t;

    enum_id_bimap_t enum_id_bimap;

};

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

#include <stdio.h>

#include <humotion/server/server.h>

#include <string>

#include <iostream>

//#include "meka_data_receiver.h"

#include "mekajointinterface.h"

using namespace std;

int main(int argc, char *argv[]){

    /*Property params;

    params.fromCommand(argc, argv);

    if (!params.check("robot")){

        fprintf(stderr, "Please specify the name of the robot\n");

        fprintf(stderr, "--robot name (e.g. icub or icubSim)\n");

        return -1;

    }

    string robotName=params.find("robot").asString().c_str();

    string scope="/"+robotName;

*/

    if (argc != 5){

        printf("> ERROR: invalid number of parameters passed to server!\n\n");

        printf("usage   : %s <humotion base topic> <jointstates topic> <controlstates topic> <control output topic>\n\n",argv[0]);

        printf("example : %s /meka /joint_states  /meka_roscontrol_state_manager/state /meka_roscontrol/head_position_trajectory_controller/command)\n\n",argv[0]);

        exit(EXIT_FAILURE);

    }

    //create humotion interface

    string humotion_scope      = argv[1];

    string jointstates_scope   = argv[2];

    string controlstates_scope = argv[3];

    string control_scope       = argv[4];

    MekaJointInterface *jointinterface = new MekaJointInterface(jointstates_scope, controlstates_scope, control_scope);

    humotion::server::Server *humotion_server = new humotion::server::Server(humotion_scope, "ROS", jointinterface);

    //finally run it

    jointinterface->run();

    while(humotion_server->ok()){

        usleep(100000);

    }

    printf("> ros connection died, will exit now\n");

    return 0;

}

#include "mekajointinterface.h"

using namespace std;

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

#define POSITION_CONTROL 1

void MekaJointInterface::incoming_controlstate(const m3meka_msgs::M3ControlStates &control_state){

    //incoming controller status

    for (unsigned int i = 0; i<control_state.group_name.size(); i++){

        if (control_state.group_name[i] == "head"){

            //enable/disable based on controller status

            if (control_state.state[i] == m3meka_msgs::M3ControlStates::START){

                //controller up and running

                if (!controller_enabled){

                    printf("> incoming control state (%d), enabling jointstate output\n",control_state.state[i]);

                    controller_enabled = true;

                }

            }else{

                //controller in estop/stopped etc

                if (controller_enabled){

                    printf("> incoming control state (%d), DISABLING jointstate output\n",control_state.state[i]);

                    controller_enabled = false;

                }

            }

        }

    }

}

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

    //fetch current timestamp

    double timestamp = msg.header.stamp.toSec();

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

            if (id == ID_NECK_PAN){

                //joint is inverted

                JointInterface::store_incoming_position(id, -180.0 / M_PI * msg.position[i], timestamp);

                JointInterface::store_incoming_speed(   id, -180.0 / M_PI * msg.velocity[i], timestamp);

            }else if (id == ID_NECK_TILT){

                JointInterface::store_incoming_position(id, 180.0 / M_PI * msg.position[i], timestamp);

                JointInterface::store_incoming_speed(   id, 180.0 / M_PI * msg.velocity[i], timestamp);

            }

        }

    }

    //dummy data uses current time

    timestamp = get_timestamp_s();

    //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 control_scope, string _output_scope) : humotion::server::JointInterface(){

    input_scope = _input_scope;

    output_scope = _output_scope;

    controller_enabled = false;

    //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("> listening on controlstates on '%s'\n",control_scope.c_str());

    control_state_subscriber = n.subscribe(control_scope, 150, &MekaJointInterface::incoming_controlstate, 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(){

   ros::spin();

}

double MekaJointInterface::get_timestamp_s(){

    struct timespec spec;

    clock_gettime(CLOCK_REALTIME, &spec);

    return spec.tv_sec + spec.tv_nsec / 1.0e9;

}

//! set the target position of a joint

//! \param enum id of joint

//! \param float value

void MekaJointInterface::publish_target(int e, float position, float velocity){

   //we do this in execute motion for all joints at once...

}

//! actually execute the scheduled motion commands

void MekaJointInterface::execute_motion(){

    if (controller_enabled){

        //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(get_target_position(ID_NECK_TILT) * M_PI / 180.0);

        //pan joint is inverted!

        p.positions.push_back(-get_target_position(ID_NECK_PAN) * M_PI / 180.0);

        //printf("targets pan=%4.1f tilt=%4.1f (eye p %4.1f t %4.2f)\n",joint_target[ID_NECK_TILT],joint_target[ID_NECK_PAN],joint_target[ID_EYES_LEFT_LR],joint_target[ID_EYES_BOTH_UD]);

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

        msg.points.push_back(p);

        target_publisher.publish(msg);

    }

}

//! 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, -37, 1);

    store_min_max(ID_NECK_PAN, -70, 70);

    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);

}

PROJECT(humotion_yarp_icub)

cmake_minimum_required(VERSION 2.8)

FIND_PACKAGE(YARP)

FIND_PACKAGE(ICUB)

FIND_PACKAGE(Boost REQUIRED COMPONENTS system thread)

FIND_PACKAGE(humotion)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")

# add include directories

INCLUDE_DIRECTORIES(${YARP_INCLUDE_DIRS} ${ICUB_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} include/)

link_directories(${Boost_LIBRARY_DIRS} ${humotion_LIBRARY_DIRS})

# add required linker flags

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

SET(MAIN icub_humotion_server)

file(GLOB DUMMY_HEADER_LIST RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} include/*.h)

ADD_EXECUTABLE(${MAIN} src/main.cpp src/icub_jointinterface.cpp src/icub_faceinterface.cpp src/icub_data_receiver.cpp ${DUMMY_HEADER_LIST})

# we now add the YARP and iCub libraries to our project.

TARGET_LINK_LIBRARIES(${MAIN} ${Boost_LIBRARIES} ${YARP_LIBRARIES} ${ICUB_LIBRARIES} ${humotion_LIBRARIES})

set_property(TARGET ${MAIN} PROPERTY INSTALL_RPATH_USE_LINK_PATH TRUE)

INSTALL(TARGETS ${MAIN} DESTINATION bin)

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

## style guide ##

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

set(ENABLE_CPPLINT 1)

include(${CMAKE_CURRENT_SOURCE_DIR}/../../stylecheck/CpplintWrapper.cmake)

CPPLINT_RECURSIVE(cpplint_include

  ${CMAKE_CURRENT_SOURCE_DIR}/include

  ${CMAKE_CURRENT_SOURCE_DIR}/include

  ${CMAKE_CURRENT_BINARY_DIR}/include)

CPPLINT_RECURSIVE(cpplint_src

  ${CMAKE_CURRENT_SOURCE_DIR}/src

  ${CMAKE_CURRENT_SOURCE_DIR}/src

  ${CMAKE_CURRENT_BINARY_DIR}/src)

add_dependencies(${MAIN} cpplint_src cpplint_include)

#workaround for qtcreator ide integration. do not remove!

file(GLOB_RECURSE NODE_DUMMY_TARGETS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.h *.cfg *.yaml *.xml *.launch)

add_custom_target(_dummy_target SOURCES ${NODE_DUMMY_TARGETS})

<?xml version="1.0" encoding="UTF-8"?>

<!DOCTYPE QtCreatorProject>

<!-- Written by QtCreator 3.0.1, 2016-02-15T14:37:35. -->

<qtcreator>

 <data>

  <variable>ProjectExplorer.Project.ActiveTarget</variable>

  <value type="int">0</value>

 </data>

 <data>

  <variable>ProjectExplorer.Project.EditorSettings</variable>

  <valuemap type="QVariantMap">

   <value type="bool" key="EditorConfiguration.AutoIndent">true</value>

   <value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>

   <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>

   <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">

    <value type="QString" key="language">Cpp</value>

    <valuemap type="QVariantMap" key="value">

     <value type="QByteArray" key="CurrentPreferences">CppGlobal</value>

    </valuemap>

   </valuemap>

   <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">

    <value type="QString" key="language">QmlJS</value>

    <valuemap type="QVariantMap" key="value">

     <value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>

    </valuemap>

   </valuemap>

   <value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>

   <value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>

   <value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>

   <value type="int" key="EditorConfiguration.IndentSize">4</value>

   <value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>

   <value type="bool" key="EditorConfiguration.MouseNavigation">true</value>

   <value type="int" key="EditorConfiguration.PaddingMode">1</value>

   <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>

   <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>

   <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>

   <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>

   <value type="int" key="EditorConfiguration.TabSize">8</value>

   <value type="bool" key="EditorConfiguration.UseGlobal">true</value>

   <value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>

   <value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>

   <value type="bool" key="EditorConfiguration.cleanIndentation">true</value>

   <value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>

   <value type="bool" key="EditorConfiguration.inEntireDocument">false</value>

  </valuemap>

 </data>

 <data>

  <variable>ProjectExplorer.Project.PluginSettings</variable>

  <valuemap type="QVariantMap"/>

 </data>

 <data>

  <variable>ProjectExplorer.Project.Target.0</variable>

  <valuemap type="QVariantMap">

   <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>

   <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>

   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{0ff36a7d-682d-4601-a9c1-275f77eefc70}</value>

   <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>

   <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>

   <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>

   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">

    <value type="bool" key="CMakeProjectManager.CMakeBuildConfiguration.UseNinja">false</value>

    <value type="QString" key="CMakeProjectManager.CMakeBuildConfiguration.UserArguments">-DCMAKE_INSTALL_PREFIX=/vol/icub/sschulz/releases/roman16-humotion-nightly/</value>

    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/homes/sschulz/src/flobi/humotion/examples/hum </value>

    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">

     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">

      <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>

      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets"/>

      <value type="bool" key="CMakeProjectManager.MakeStep.Clean">false</value>

      <value type="bool" key="CMakeProjectManager.MakeStep.UseNinja">false</value>

      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>

      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>

      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>

     </valuemap>

     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>

    </valuemap>

    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">

     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">

      <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments">clean</value>

      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets"/>

      <value type="bool" key="CMakeProjectManager.MakeStep.Clean">true</value>

      <value type="bool" key="CMakeProjectManager.MakeStep.UseNinja">false</value>

      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>

      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>

      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>

     </valuemap>

     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>

    </valuemap>

    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>

    <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>

    <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Default</value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Default</value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeBuildConfiguration</value>

   </valuemap>

   <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>

   <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">

    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">

     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>

    </valuemap>

    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy locally</value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>

   </valuemap>

   <value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>

   <valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>

   <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">

    <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>

    <value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>

    <value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>

    <value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>

    <value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>

    <value type="int" key="Analyzer.Valgrind.NumCallers">25</value>

    <valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>

    <value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>

    <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>

    <value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>

    <value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>

    <value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>

    <valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">

     <value type="int">0</value>

     <value type="int">1</value>

     <value type="int">2</value>

     <value type="int">3</value>

     <value type="int">4</value>

     <value type="int">5</value>

     <value type="int">6</value>

     <value type="int">7</value>

     <value type="int">8</value>

     <value type="int">9</value>

     <value type="int">10</value>

     <value type="int">11</value>

     <value type="int">12</value>

     <value type="int">13</value>

     <value type="int">14</value>

    </valuelist>

    <value type="QString" key="CMakeProjectManager.CMakeRunConfiguation.Title">icub_humotion_server</value>

    <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.Arguments"></value>

    <value type="bool" key="CMakeProjectManager.CMakeRunConfiguration.UseTerminal">false</value>

    <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.UserWorkingDirectory"></value>

    <value type="int" key="PE.EnvironmentAspect.Base">2</value>

    <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">icub_humotion_server</value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeRunConfiguration.icub_humotion_server</value>

    <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>

    <value type="bool" key="RunConfiguration.UseCppDebugger">true</value>

    <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">false</value>

    <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>

    <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>

    <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>

   </valuemap>

   <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>

  </valuemap>

 </data>

 <data>

  <variable>ProjectExplorer.Project.TargetCount</variable>

  <value type="int">1</value>

 </data>

 <data>

  <variable>ProjectExplorer.Project.Updater.EnvironmentId</variable>

  <value type="QByteArray">{44110186-3313-405d-bf42-c546b33c1fcb}</value>

 </data>

 <data>

  <variable>ProjectExplorer.Project.Updater.FileVersion</variable>

  <value type="int">15</value>

 </data>

</qtcreator>

/*

* This file is part of humotion

*

* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de

* http://opensource.cit-ec.de/projects/humotion

*

* This file may be licensed under the terms of the

* GNU Lesser General Public License Version 3 (the ``LGPL''),

* or (at your option) any later version.

*

* Software distributed under the License is distributed

* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either

* express or implied. See the LGPL for the specific language

* governing rights and limitations.

*

* You should have received a copy of the LGPL along with this

* program. If not, go to http://www.gnu.org/licenses/lgpl.html

* or write to the Free Software Foundation, Inc.,

* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

*

* The development of this software was supported by the

* Excellence Cluster EXC 277 Cognitive Interaction Technology.

* The Excellence Cluster EXC 277 is a grant of the Deutsche

* Forschungsgemeinschaft (DFG) in the context of the German

* Excellence Initiative.

*/

#ifndef EXAMPLES_YARP_ICUB_INCLUDE_ICUB_DATA_RECEIVER_H_

#define EXAMPLES_YARP_ICUB_INCLUDE_ICUB_DATA_RECEIVER_H_

#include <humotion/server/joint_interface.h>

#include <humotion/server/server.h>

#include <boost/bimap.hpp>

#include <yarp/dev/ControlBoardInterfaces.h>

#include <yarp/dev/IControlLimits2.h>

#include <yarp/dev/PolyDriver.h>

#include <yarp/os/Network.h>

#include <yarp/os/Property.h>

#include <yarp/os/RateThread.h>

#include <yarp/os/Time.h>

#include <yarp/sig/Vector.h>

#include "icub_humotion/icub_jointinterface.h"

// forward declaration to solve loop

class iCubJointInterface;

class iCubDataReceiver : public yarp::os::RateThread{

public:

    iCubDataReceiver(int period, iCubJointInterface *icub_jointinterface);

    bool threadInit();

    void threadRelease();

    void run();

private:

    void store_incoming_position(int icub_id, double value, double timestamp);

    void store_incoming_velocity(int icub_id, double velocity, double timestamp);

    yarp::sig::Vector calculate_velocities(yarp::sig::Vector positions,

                              yarp::sig::Vector timestamps);

    void dump_incoming_data();

    float target_eye_pan_;

    float target_eye_vergence_;

    float target_eye_pan_velocity_;

    float target_eye_vergence_velocity_;

    yarp::sig::Vector positions_;

    yarp::sig::Vector timestamps_;

    yarp::sig::Vector velocities_;

    yarp::sig::Vector commands_;

    yarp::sig::Vector previous_positions_;

    yarp::sig::Vector previous_timestamps_;

    iCubJointInterface *icub_jointinterface_;

    yarp::dev::IEncodersTimed *iencs_;

};

#endif  // EXAMPLES_YARP_ICUB_INCLUDE_ICUB_DATA_RECEIVER_H_

/*

* This file is part of humotion

*

* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de

* http://opensource.cit-ec.de/projects/humotion

*

* This file may be licensed under the terms of the

* GNU Lesser General Public License Version 3 (the ``LGPL''),

* or (at your option) any later version.

*

* Software distributed under the License is distributed

* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either

* express or implied. See the LGPL for the specific language

* governing rights and limitations.

*

* You should have received a copy of the LGPL along with this

* program. If not, go to http://www.gnu.org/licenses/lgpl.html

* or write to the Free Software Foundation, Inc.,

* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

*

* The development of this software was supported by the

* Excellence Cluster EXC 277 Cognitive Interaction Technology.

* The Excellence Cluster EXC 277 is a grant of the Deutsche

* Forschungsgemeinschaft (DFG) in the context of the German

* Excellence Initiative.

*/

#pragma once

#include <yarp/dev/ControlBoardInterfaces.h>

#include <yarp/os/Network.h>

#include "icub_jointinterface.h"

/*

 * #include <humotion/server/server.h>

#include <yarp/os/RateThread.h>

#include <yarp/os/Time.h>

#include <yarp/os/Property.h>

#include <yarp/os/Port.h>

#include <yarp/dev/ControlBoardInterfaces.h>

#include "icub_data_receiver.h"

*/

class iCubFaceInterface {

public:

    iCubFaceInterface(std::string scope);

    ~iCubFaceInterface();

    void set_eyelid_angle(float angle);

    void set_eyebrow_angle(int id, float *target_angle);

    void set_mouth(float *target_angle);

private:

    double lid_angle;

    int lid_opening_previous;

    int previous_mouth_state;

    double target_angle_previous[iCubJointInterface::ICUB_JOINT_ID_ENUM_SIZE];

    std::string scope;

    yarp::os::BufferedPort<yarp::os::Bottle> emotion_port[4];

};

/*

* This file is part of humotion

*

* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de

* http://opensource.cit-ec.de/projects/humotion

*

* This file may be licensed under the terms of the

* GNU Lesser General Public License Version 3 (the ``LGPL''),

* or (at your option) any later version.

*

* Software distributed under the License is distributed

* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either

* express or implied. See the LGPL for the specific language

* governing rights and limitations.

*

* You should have received a copy of the LGPL along with this

* program. If not, go to http://www.gnu.org/licenses/lgpl.html

* or write to the Free Software Foundation, Inc.,

* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

*

* The development of this software was supported by the

* Excellence Cluster EXC 277 Cognitive Interaction Technology.

* The Excellence Cluster EXC 277 is a grant of the Deutsche

* Forschungsgemeinschaft (DFG) in the context of the German

* Excellence Initiative.

*/

#pragma once

#include <humotion/server/joint_interface.h>

#include <boost/bimap.hpp>

#include <yarp/dev/PolyDriver.h>

#include <yarp/dev/IControlMode.h>

#include <yarp/dev/IControlLimits2.h>

#include <yarp/dev/ControlBoardInterfaces.h>

#include <yarp/os/Time.h>

#include <yarp/sig/Vector.h>

#include "icub_jointinterface.h"

#include <humotion/server/server.h>

#include <yarp/os/Network.h>

#include <yarp/os/RateThread.h>

#include <yarp/os/Time.h>

#include <yarp/os/Property.h>

#include <yarp/os/Port.h>

#include <yarp/dev/ControlBoardInterfaces.h>

#include "icub_data_receiver.h"

class iCubDataReceiver;

class iCubFaceInterface;

class iCubJointInterface : public humotion::server::JointInterface{

public:

    iCubJointInterface(std::string scope);

    ~iCubJointInterface();

    //void store_incoming_position(int humotion_id, double position, double timestamp);

    //void store_incoming_velocity(int humotion_id, double position, double timestamp);

    void run();

    int convert_icub_jointid_to_humotion(int id);

    int convert_humotion_jointid_to_icub(int id);

    enum JOINT_ID_ENUM{

        ICUB_ID_NECK_TILT = 0,

        ICUB_ID_NECK_ROLL = 1,

        ICUB_ID_NECK_PAN  = 2,

        ICUB_ID_EYES_BOTH_UD = 3,

        ICUB_ID_EYES_PAN = 4,

        ICUB_ID_EYES_VERGENCE = 5,

        ICUB_ID_EYES_LEFT_LID_LOWER,

        ICUB_ID_EYES_LEFT_LID_UPPER,

        ICUB_ID_EYES_RIGHT_LID_LOWER,

        ICUB_ID_EYES_RIGHT_LID_UPPER,

        ICUB_ID_EYES_LEFT_BROW,

        ICUB_ID_EYES_RIGHT_BROW,

        ICUB_ID_LIP_LEFT_UPPER,

        ICUB_ID_LIP_LEFT_LOWER,

        ICUB_ID_LIP_CENTER_UPPER,

        ICUB_ID_LIP_CENTER_LOWER,

        ICUB_ID_LIP_RIGHT_UPPER,

        ICUB_ID_LIP_RIGHT_LOWER,

        ICUB_JOINT_ID_ENUM_SIZE

    };

    yarp::dev::PolyDriver *get_yarp_polydriver() { return &yarp_polydriver_; }

    static const int MAIN_LOOP_FREQUENCY = 50;

protected:

    void disable_joint(int e);

    void publish_target(int e, float position, float velocity);

    void enable_joint(int e);

    void execute_motion();

private:

    yarp::dev::PolyDriver yarp_polydriver_;

    std::vector<double> pv_mix_last_error_;

    std::vector<double> pv_mix_pid_p_;

    std::vector<double> pv_mix_pid_d_;

    // yarp views

    yarp::dev::IVelocityControl *yarp_ivel_;

    //yarp::dev::IPositionControl *yarp_ipos_;

    //yarp::dev::IEncodersTimed *yarp_iencs_;

    yarp::dev::IControlLimits *yarp_ilimits_;

    yarp::dev::IAmplifierControl *yarp_amp_;

    yarp::dev::IPidControl *yarp_pid_;

    yarp::dev::IControlMode *yarp_icontrol_;

    yarp::sig::Vector yarp_commands_;

    float target_angle_[ICUB_JOINT_ID_ENUM_SIZE];

    float target_velocity_[ICUB_JOINT_ID_ENUM_SIZE];

    void init_controller();

    void init_id_map();

    void init_pv_mix_pid();

    void insert_icupid_to_humotionid_mapping(int icubid, int humotionid);

    void store_icub_joint_target(int icub_id, float position, float velocity);

    //*******************************************

    void set_joint_enable_state(int e, bool enabled);

    //double target_angle[ICUB_JOINT_ID_ENUM_SIZE];

    //double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];

    iCubDataReceiver *icub_data_receiver;

    void init_joints();

    std::string scope;

    //yarp::sig::Vector positions;

    //yarp::sig::Vector velocities;

    void store_min_max(yarp::dev::IControlLimits *ilimits, int icub_id);

   //float last_pos_eye_vergence;

    //float last_pos_eye_pan;

   // float last_vel_eye_vergence;

    //float last_vel_eye_pan;

    //void store_joint(int id, float value);

    void set_target_in_velocitymode(int id);

    //int convert_enum_to_motorid(int e);

    //int convert_motorid_to_enum(int id);

    iCubFaceInterface *face_interface_;

    typedef boost::bimap<int, int > enum_id_bimap_t;

    typedef enum_id_bimap_t::value_type enum_id_bimap_entry_t;

    enum_id_bimap_t enum_id_bimap;

};

/*

* This file is part of humotion

*

* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de

* http://opensource.cit-ec.de/projects/humotion

*

* This file may be licensed under the terms of the

* GNU Lesser General Public License Version 3 (the ``LGPL''),

* or (at your option) any later version.

*

* Software distributed under the License is distributed

* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either

* express or implied. See the LGPL for the specific language

* governing rights and limitations.

*

* You should have received a copy of the LGPL along with this

* program. If not, go to http://www.gnu.org/licenses/lgpl.html

* or write to the Free Software Foundation, Inc.,

* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

*

* The development of this software was supported by the

* Excellence Cluster EXC 277 Cognitive Interaction Technology.

* The Excellence Cluster EXC 277 is a grant of the Deutsche

* Forschungsgemeinschaft (DFG) in the context of the German

* Excellence Initiative.

*/

#include <boost/format.hpp>

#include <humotion/server/joint_interface.h>

#include <yarp/os/Property.h>

#include "humotion_yarp_icub/icub_data_receiver.h"

using std::cout;

using std::cerr;

using std::string;

using humotion::server::JointInterface;

using yarp::dev::IEncodersTimed;

using yarp::sig::Vector;

#define ICUB_DATA_RECEIVER_USE_ENCODERSPEED 0

#define ICUB_DATA_RECEIVER_DUMP_DATA 0

//! constructor

//! \param period_ms for the yarp rate thread

//! \param icub_jointinterface

iCubDataReceiver::iCubDataReceiver(int period_ms, iCubJointInterface *icub_jointinterface)

    : yarp::os::RateThread(period_ms) {

    // store pointer to icub jointinterface

    icub_jointinterface_ = icub_jointinterface;

    // fetch iencs view from yarp

    yarp::dev::PolyDriver *poly_driver = icub_jointinterface->get_yarp_polydriver();

    bool success = poly_driver->view(iencs_);

    if (!success) {

        cerr << "ERROR: polydriver failed to init iencs view\n";

        exit(EXIT_FAILURE);

    }

    // resize data storage vectors to match the number of axes

    int joints;

    iencs_->getAxes(&joints);

    positions_.resize(joints);

    velocities_.resize(joints);

    timestamps_.resize(joints);

}

//! yarp rate thread initializer

bool iCubDataReceiver::threadInit() {

    return true;

}

//! yarp thread release function

void iCubDataReceiver::threadRelease() {

}

//! manully calculate joint velocities (instead of using joint encoder speeds)

//! \param positions vector with current encoder position

//! \param timestamps vector with the associated timestamps

//! \return velocities as vector

Vector iCubDataReceiver::calculate_velocities(Vector positions, Vector timestamps) {

    Vector velocities;

    velocities.resize(positions.size());

    if (previous_positions_.size() == 0) {

        // first run, no valid old position available, return zero velocities

        // by setting all all elements to zero

        velocities = 0.0;

    } else {

        // calculate speed based on positions:

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

            float diff = positions[i] - previous_positions_[i];

            float timediff = timestamps[i] - previous_timestamps_[i];

            // calc speed:

            velocities[i] = diff / timediff;

        }

    }

    previous_positions_ = positions;

    previous_timestamps_ = timestamps;

    return velocities;

}

//! main loop routine, called by yarp rate thread

void iCubDataReceiver::run() {

    float velocity;

    // grab pos+vel data:

    iencs_->getEncodersTimed(positions_.data(), timestamps_.data());

    #if ICUB_DATA_RECEIVER_USE_ENCODERSPEED

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

    if (!iencs_->getEncoderSpeeds(velocities_.data())) {

        cout << "Failed to fetch encoder speeds...\n";

        return;

    }

    #else

    // manually calculate the speed based on old position:

    velocities_ = calculate_velocities(positions_, timestamps_);

    #endif

    // publish data to humotion

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

        // store position values

        store_incoming_position(i, positions_[i], timestamps_[i]);

        // store velocity

        store_incoming_velocity(i, velocities_[i], timestamps_[i]);

    }

    // small hack to tell humotion to update the lid angle

    // fixme: use real id

    store_incoming_position(100, 0.0, timestamps_[0]);

    #if ICUB_DATA_RECEIVER_DUMP_DATA

    dump_incoming_data();

    #endif

}

//! store incoming position for a given icub joint

//! \param icub _id icub joint id

//! \param position

//! \param timestamp

void iCubDataReceiver::store_incoming_position(int icub_id, double position, double timestamp) {

    //cout << "store_incoming_position(icub=" << icub_id << ", " << position << ")\n";

    // store joint position in humotion backend

    if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||

            (icub_id == iCubJointInterface::ICUB_ID_EYES_VERGENCE)) {

        // the icub handles eyes differently

        // instead of using seperate left/right pan the icub uses

        // a combined pan angle and vergence. therfore we have to convert this here:

        if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {

            target_eye_pan_ = position;

        } else {

            target_eye_vergence_ = -position;

        }

        float right = target_eye_pan_ + target_eye_vergence_/2.0;

        float left  = target_eye_pan_ - target_eye_vergence_/2.0;

        icub_jointinterface_->store_incoming_position(JointInterface::ID_EYES_LEFT_LR,

                                                     left, timestamp);

        icub_jointinterface_->store_incoming_position(JointInterface::ID_EYES_RIGHT_LR,

                                                     right, timestamp);

    } else if (icub_id == 100) {

        //HACK

        //icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,

        //                                             lid_angle, timestamp);

    } else {

        if (icub_id == iCubJointInterface::ID_NECK_PAN) {

            // icub uses an inverted neck pan specification

            position = -position;

        }

        // known configured mapping between joint ids

        int humotion_id = icub_jointinterface_->convert_icub_jointid_to_humotion(icub_id);

        icub_jointinterface_->store_incoming_position(humotion_id, position, timestamp);

    }

}

//! store incoming velocity for a given icub joint

//! \param icub_id icub joint id

//! \param velocity

//! \param timestamp

void iCubDataReceiver::store_incoming_velocity(int icub_id, double velocity, double timestamp) {

    //cout << "store_incoming_velocity(icub=" << icub_id << ", " << velocity << ")\n";

    // store joint position in humotion backend

    if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||

            (icub_id == iCubJointInterface::ICUB_ID_EYES_VERGENCE)) {

        // the icub handles eyes differently

        // instead of using seperate left/right pan the icub uses

        // a combined pan angle and vergence. therfore we have to convert this here:

        if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {

            target_eye_pan_velocity_ = velocity;

        } else {

            target_eye_vergence_velocity_ = -velocity;

        }

        float right = target_eye_pan_velocity_ + target_eye_vergence_velocity_/2.0;

        float left  = target_eye_pan_velocity_ - target_eye_vergence_velocity_/2.0;

        icub_jointinterface_->store_incoming_velocity(JointInterface::ID_EYES_LEFT_LR,

                                                     left, timestamp);

        icub_jointinterface_->store_incoming_velocity(JointInterface::ID_EYES_RIGHT_LR,

                                                     right, timestamp);

    } else if (icub_id == 100) {

        //HACK

        //icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,

        //                                             lid_angle, timestamp);

    } else {

        if (icub_id == iCubJointInterface::ID_NECK_PAN) {

            // icub uses an inverted neck pan specification

            velocity = -velocity;

        }

        // known configured mapping between joint ids

        int humotion_id = icub_jointinterface_->convert_icub_jointid_to_humotion(icub_id);

        icub_jointinterface_->store_incoming_velocity(humotion_id, velocity, timestamp);

    }

}

//! helper for debugging purposes, feed this data into gnuplot for visual inspection

void iCubDataReceiver::dump_incoming_data() {

    // use gnuplot for viz:

    // ./icub_humotion_server --robot icub | grep "INCOMING" |tee  log

    // @gnuplot: plot "log" using 0:1 w l t "p neck tilt", "log" using 0:2 w l t "v neck tilt", \

    //                "log" using 0:5 w l t "p neck pan", "log" using 0:6 w l t "v neck pan", \

    //                "log" using 0:7 w l t "p eyes ud", "log" using 0:8 w l t "v eyes ud", \

    //                "log" using 0:9 w l t "p eyes vergence", "log" using 0:10 w l t "v eyes vergence"

    cout << "\n";

            // publish data to humotion

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

        cout << positions_[i] << " ";

        cout << velocities_[i] << " ";

    }

    cout << " #INCOMING_DATA_DUMP\n";

}

/*

* This file is part of humotion

*

* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de

* http://opensource.cit-ec.de/projects/humotion

*

* This file may be licensed under the terms of the

* GNU Lesser General Public License Version 3 (the ``LGPL''),

* or (at your option) any later version.

*

* Software distributed under the License is distributed

* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either

* express or implied. See the LGPL for the specific language

* governing rights and limitations.

*

* You should have received a copy of the LGPL along with this

* program. If not, go to http://www.gnu.org/licenses/lgpl.html

* or write to the Free Software Foundation, Inc.,

* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

*

* The development of this software was supported by the

* Excellence Cluster EXC 277 Cognitive Interaction Technology.

* The Excellence Cluster EXC 277 is a grant of the Deutsche

* Forschungsgemeinschaft (DFG) in the context of the German

* Excellence Initiative.

*/

#include "humotion_yarp_icub/icub_faceinterface.h"

#include <algorithm>

#include <string>

#include <boost/format.hpp>

using yarp::os::Network;

using yarp::os::Bottle;

using std::cout;

using std::cerr;

using std::string;

//! constructor

iCubFaceInterface::iCubFaceInterface(std::string _scope) {

    scope = _scope;

    // attach to facial expressions:

    std::string emotion_scope = scope + "/face/raw/in";

    cout << "opening connection to '"<< emotion_scope << "'\n";

    for (int i = 0; i < 4; i++) {

        // strange, if we use one output port only the first command is executed?! flushing issues?

        std::string emotion_port_out = "/emotionwriter" + std::to_string((unsigned long long)i);

        if (!emotion_port[i].open(emotion_port_out.c_str())) {

            cerr << "ERROR: failed to open emotion port '" << emotion_port_out << "'\n";

            exit(EXIT_FAILURE);

        }

        if (!Network::connect(emotion_port_out.c_str(), emotion_scope.c_str())) {

            cerr << "ERROR: failed to connect to emotion port '" << emotion_port_out << "'\n";

            exit(EXIT_FAILURE);

        }

    }

}

//! destructor

iCubFaceInterface::~iCubFaceInterface() {

}

//! special command to set eyelid angle

//! \param angle in degrees

void iCubFaceInterface::set_eyelid_angle(float angle) {

    if (emotion_port[0].getOutputCount() > 0) {

        // try to set the value based on the upper one

        // some guesses from the sim: S30 = 0° / S40 = 10°

        int opening = (25.0 + 0.8*angle);

        opening = std::min(48, std::max(24, opening));

        if (opening == lid_opening_previous) {

            // no update necessary

            return;

        }

        lid_angle = angle;

        lid_opening_previous = opening;

        char buf[20];

        snprintf(buf, sizeof(buf), "S%2d", opening);

        // cout << "SETTING EYELID '" << buf << "' (" << angle << " -> " << opening << "\n";

        Bottle &cmd = emotion_port[0].prepare();

        cmd.clear();

        cmd.addString(buf);

        emotion_port[0].writeStrict();

    } else {

        cerr << "ERROR: no icub emotion output\n";

        exit(EXIT_FAILURE);

    }

}

//! special command to set the eyebrow angle

//! \param id {0=left, 1=right)

//! \param angle in degrees

void iCubFaceInterface::set_eyebrow_angle(int id, float *target_angle) {

    int port_id;

    if (id == iCubJointInterface::ICUB_ID_EYES_LEFT_BROW) {

        port_id = 1;

    } else {

        port_id = 2;

    }

    if (emotion_port[port_id].getOutputCount() > 0) {

        double angle = target_angle[id];

        int icub_val = 0;

        // swap rotation direction for eyebrow

        if (id == iCubJointInterface::ICUB_ID_EYES_LEFT_BROW) {

            angle = -angle;

        }

        // convert to icub representation

        if (angle < -20) {

            icub_val = 1;

        } else if (angle < 10) {

            icub_val = 2;

        } else if (angle < 20) {

            icub_val = 4;

        } else {

            icub_val = 8;

        }

        // make sure to update only on new values

        if (icub_val == target_angle_previous[id]) {

                // no updata necessary

                return;

        }

        // store actual value

        target_angle_previous[id] = icub_val;

        std::string cmd_s;

        if (id == iCubJointInterface::ICUB_ID_EYES_LEFT_BROW) {

            cmd_s = "L0" + std::to_string((unsigned long long)icub_val);

        } else {

            cmd_s = "R0" + std::to_string((unsigned long long)icub_val);

        }

        cout << "SETTING EYEBROW " << id << " (" << angle << " -> " << cmd_s << ")\n";

        Bottle &cmd = emotion_port[port_id].prepare();

        cmd.clear();

        cmd.addString(cmd_s);

        emotion_port[port_id].writeStrict();

    } else {

        cerr << "ERROR: no icub emotion output\n";

        exit(EXIT_FAILURE);

    }

}

void iCubFaceInterface::set_mouth(float *target_angle) {

    // convert from 6DOF mouth displacement to icub leds:

    int led_value = 0;

    // fetch center opening

    double center_opening = target_angle[iCubJointInterface::ICUB_ID_LIP_CENTER_LOWER] -

            target_angle[iCubJointInterface::ICUB_ID_LIP_CENTER_UPPER];

    bool mouth_open = (center_opening > 15.0) ? true : false;

    // side of mouth high or low?

    double center_avg = (target_angle[iCubJointInterface::ICUB_ID_LIP_CENTER_LOWER] +

            target_angle[iCubJointInterface::ICUB_ID_LIP_CENTER_UPPER])/2.0;

    double left_avg   = (target_angle[iCubJointInterface::ICUB_ID_LIP_LEFT_LOWER] +

            target_angle[iCubJointInterface::ICUB_ID_LIP_LEFT_UPPER])/2.0;

    double right_avg  = (target_angle[iCubJointInterface::ICUB_ID_LIP_RIGHT_LOWER] +

            target_angle[iCubJointInterface::ICUB_ID_LIP_RIGHT_UPPER])/2.0;

    // happy, neutral or sad?

    double diff_l = center_avg - left_avg;

    double diff_r = center_avg - right_avg;

    double diff   = (diff_l+diff_r)/2.0;

    if (diff > 2.0) {

        if (mouth_open) {

            led_value = 0x14;

        } else {

            if (diff > 2.6) {

                led_value = 0x0A;

            } else {

                led_value = 0x0B;