humotion

cmake_minimum_required(VERSION 2.8.3)

project(humotion)

set(ENV{ROS_LANG_DISABLE} "genjava")

set(ROS_BUILD_TYPE Debug)

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

# check if we have RSB support:

FIND_PACKAGE(RSB 0.11)

IF (RSB_FOUND)

    #RSB

    SET(CMAKE_INSTALL_RPATH "\$ORIGIN/../lib:\$ORIGIN/")

    FIND_PACKAGE(RSC 0.11 REQUIRED)

    FIND_PACKAGE(RSB 0.11 REQUIRED)

    #RST

    FIND_PACKAGE(RST REQUIRED COMPONENTS sandbox)

    INCLUDE_DIRECTORIES(BEFORE SYSTEM ${RST_INCLUDE_DIRS})

    ADD_DEFINITIONS(${RST_CFLAGS} ${RSTSANDBOX_CFLAGS})

    INCLUDE_DIRECTORIES(BEFORE SYSTEM ${RSB_INCLUDE_DIRS})

    LIST(INSERT CMAKE_MODULE_PATH 0 ${RSC_CMAKE_MODULE_PATH})

    #RSB

    SET(CMAKE_INSTALL_RPATH "\$ORIGIN/../lib:\$ORIGIN/")

    FIND_PACKAGE(RSC 0.11 REQUIRED)

    FIND_PACKAGE(RSB 0.11 REQUIRED)

    #RST

    FIND_PACKAGE(RST REQUIRED COMPONENTS sandbox)

    INCLUDE_DIRECTORIES(BEFORE SYSTEM ${RST_INCLUDE_DIRS})

    ADD_DEFINITIONS(${RST_CFLAGS} ${RSTSANDBOX_CFLAGS})

    INCLUDE_DIRECTORIES(BEFORE SYSTEM ${RSB_INCLUDE_DIRS})

    LIST(INSERT CMAKE_MODULE_PATH 0 ${RSC_CMAKE_MODULE_PATH})

    message(STATUS "RSB Support is ON")

    add_definitions(-DRSB_SUPPORT=1)

ENDIF (RSB_FOUND)

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

# check for ROS support:

find_package(catkin)

IF (catkin_FOUND)

    SET(ROS_FOUND 1)

    find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs message_generation genmsg)

    message(STATUS "ROS Support is ON")

    add_definitions(-DROS_SUPPORT=1)

ENDIF (catkin_FOUND)

INCLUDE(FindPkgConfig)

##libreflexxes

IF (libreflexxes_DIR)

  MESSAGE("using libreflexxes_DIR as override ('${libreflexxes_DIR}')")

  SET(REFLEXXES_PREFIX ${libreflexxes_DIR})

ELSE ()

  PKG_CHECK_MODULES(REFLEXXES REQUIRED libReflexxesTypeII>=1.2.3)

  IF (NOT REFLEXXES_FOUND)

    message(FATAL_ERROR "Error: could not find lib libReflexxesTypeII")

  ENDIF ()

ENDIF()

IF (NOT catkin_FOUND)

    IF (NOT RSB_FOUND)

        message(FATAL_ERROR "Error: could neither find RSB or ROS middleware!")

    ENDIF (NOT RSB_FOUND)

ENDIF (NOT catkin_FOUND)

SET(REFLEXXES_LIBRARY_DIRS "${REFLEXXES_PREFIX}/lib")

SET(REFLEXXES_INCLUDE_DIRS "${REFLEXXES_PREFIX}/include")

MESSAGE("using libReflexxesTypeII version ${REFLEXXES_VERSION} from ${REFLEXXES_INCLUDE_DIRS} and ${REFLEXXES_LIBRARY_DIRS}")

set(CMAKE_CXX_FLAGS "-g -Wall")

add_definitions ("-Wall")

IF (catkin_FOUND)

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

    ## Declare ROS messages and services ##

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

    add_message_files(

            FILES

            gaze.msg

            position_lcr.msg

            mouth.msg

    )

    ## Generate added messages and services with any dependencies listed here

    generate_messages(

            DEPENDENCIES

            std_msgs

            humotion

    )

    #

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

    ## catkin specific configuration ##

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

    ## The catkin_package macro generates cmake config files for your package

    ## Declare things to be passed to dependent projects

    ## INCLUDE_DIRS: uncomment this if you package contains header files

    ## LIBRARIES: libraries you create in this project that dependent projects also need

    ## CATKIN_DEPENDS: catkin_packages dependent projects also need

    ## DEPENDS: system dependencies of this project that dependent projects also need

    catkin_package(

        INCLUDE_DIRS include

        LIBRARIES humotion

        #CATKIN_DEPENDS message_runtime

        #DEPENDS system_lib

    )

ENDIF (catkin_FOUND)

###########

## Build ##

###########

## Specify additional locations of header files

## Your package locations should be listed before other locations

include_directories (BEFORE ${Boost_INCLUDE_DIRS} ${REFLEXXES_INCLUDE_DIRS})

include_directories(include)

include_directories( ${catkin_INCLUDE_DIRS})

link_directories (${Boost_LIBRARY_DIRS} ${REFLEXXES_LIBRARY_DIRS} ${catkin_LIBRARY_DIRS})

file(GLOB DUMMY_HEADER_LIST RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} include/*.h  include/client/*.h  include/server/*.h srv/*.srv msg/*.msg etc/*)

## Declare a cpp library

add_library(humotion

    src/mouth_state.cpp

    src/gaze_state.cpp

    src/client/client.cpp

    src/client/middleware.cpp

    src/client/middleware_ros.cpp

    src/client/middleware_rsb.cpp

    src/server/server.cpp

    src/server/middleware.cpp

    src/server/middleware_ros.cpp

    src/server/middleware_rsb.cpp

    src/server/controller.cpp

    src/server/joint_interface.cpp

    src/server/motion_generator.cpp

    src/server/gaze_motion_generator.cpp

    src/server/reflexxes_motion_generator.cpp

    src/server/mouth_motion_generator.cpp

    src/server/eye_motion_generator.cpp

    src/server/eyelid_motion_generator.cpp

    src/server/eyebrow_motion_generator.cpp

    src/server/neck_motion_generator.cpp

    src/timestamped_list.cpp

    ${DUMMY_HEADER_LIST}

)

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

## Add cmake target dependencies of the executable/library

## as an example, message headers may need to be generated before nodes

add_dependencies(humotion ${catkin_EXPORTED_TARGETS} humotion_gencpp)

## Specify libraries to link a library or executable target against

target_link_libraries(humotion

			${Boost_LIBRARIES} 

			${catkin_LIBRARIES}

                        ${REFLEXXES_LIBRARIES}

                        ${RST_LIBRARIES}

                        ${RSB_LIBRARIES}

)

set_property(TARGET humotion PROPERTY INSTALL_RPATH_USE_LINK_PATH TRUE)

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

## Install ##

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

# all install targets should use catkin DESTINATION variables

# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation

## in contrast to setup.py, you can choose the destination

# install(PROGRAMS

#   scripts/my_python_script

#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

# )

## Mark executables and/or libraries for installation

IF (catkin_FOUND)

    install(TARGETS humotion

        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

    )

    ## Mark cpp header files for installation

    install(DIRECTORY include/

        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}

        PATTERN ".svn" EXCLUDE

    )

ENDIF (catkin_FOUND)

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

## Testing ##

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

IF (catkin_FOUND)

    ## Add gtest based cpp test target and link libraries

    catkin_add_gtest(${PROJECT_NAME}-test-server test/server.cpp)

    if(TARGET ${PROJECT_NAME}-test-server)

    target_link_libraries(${PROJECT_NAME}-test-server ${PROJECT_NAME})

    endif()

    catkin_add_gtest(${PROJECT_NAME}-test-client test/client.cpp)

    if(TARGET ${PROJECT_NAME}-test-client)

    target_link_libraries(${PROJECT_NAME}-test-client ${PROJECT_NAME})

    endif()

ENDIF (catkin_FOUND)

### Add folders to be run by python nosetests

# catkin_add_nosetests(test)

                   GNU LESSER GENERAL PUBLIC LICENSE

                       Version 3, 29 June 2007

 Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>

 Everyone is permitted to copy and distribute verbatim copies

 of this license document, but changing it is not allowed.

  This version of the GNU Lesser General Public License incorporates

the terms and conditions of version 3 of the GNU General Public

License, supplemented by the additional permissions listed below.

  0. Additional Definitions.

  As used herein, "this License" refers to version 3 of the GNU Lesser

General Public License, and the "GNU GPL" refers to version 3 of the GNU

General Public License.

  "The Library" refers to a covered work governed by this License,

other than an Application or a Combined Work as defined below.

  An "Application" is any work that makes use of an interface provided

by the Library, but which is not otherwise based on the Library.

Defining a subclass of a class defined by the Library is deemed a mode

of using an interface provided by the Library.

  A "Combined Work" is a work produced by combining or linking an

Application with the Library.  The particular version of the Library

with which the Combined Work was made is also called the "Linked

Version".

  The "Minimal Corresponding Source" for a Combined Work means the

Corresponding Source for the Combined Work, excluding any source code

for portions of the Combined Work that, considered in isolation, are

based on the Application, and not on the Linked Version.

  The "Corresponding Application Code" for a Combined Work means the

object code and/or source code for the Application, including any data

and utility programs needed for reproducing the Combined Work from the

Application, but excluding the System Libraries of the Combined Work.

  1. Exception to Section 3 of the GNU GPL.

  You may convey a covered work under sections 3 and 4 of this License

without being bound by section 3 of the GNU GPL.

  2. Conveying Modified Versions.

  If you modify a copy of the Library, and, in your modifications, a

facility refers to a function or data to be supplied by an Application

that uses the facility (other than as an argument passed when the

facility is invoked), then you may convey a copy of the modified

version:

   a) under this License, provided that you make a good faith effort to

   ensure that, in the event an Application does not supply the

   function or data, the facility still operates, and performs

   whatever part of its purpose remains meaningful, or

   b) under the GNU GPL, with none of the additional permissions of

   this License applicable to that copy.

  3. Object Code Incorporating Material from Library Header Files.

  The object code form of an Application may incorporate material from

a header file that is part of the Library.  You may convey such object

code under terms of your choice, provided that, if the incorporated

material is not limited to numerical parameters, data structure

layouts and accessors, or small macros, inline functions and templates

(ten or fewer lines in length), you do both of the following:

   a) Give prominent notice with each copy of the object code that the

   Library is used in it and that the Library and its use are

   covered by this License.

   b) Accompany the object code with a copy of the GNU GPL and this license

   document.

  4. Combined Works.

  You may convey a Combined Work under terms of your choice that,

taken together, effectively do not restrict modification of the

portions of the Library contained in the Combined Work and reverse

engineering for debugging such modifications, if you also do each of

the following:

   a) Give prominent notice with each copy of the Combined Work that

   the Library is used in it and that the Library and its use are

   covered by this License.

   b) Accompany the Combined Work with a copy of the GNU GPL and this license

   document.

   c) For a Combined Work that displays copyright notices during

   execution, include the copyright notice for the Library among

   these notices, as well as a reference directing the user to the

   copies of the GNU GPL and this license document.

   d) Do one of the following:

       0) Convey the Minimal Corresponding Source under the terms of this

       License, and the Corresponding Application Code in a form

       suitable for, and under terms that permit, the user to

       recombine or relink the Application with a modified version of

       the Linked Version to produce a modified Combined Work, in the

       manner specified by section 6 of the GNU GPL for conveying

       Corresponding Source.

       1) Use a suitable shared library mechanism for linking with the

       Library.  A suitable mechanism is one that (a) uses at run time

       a copy of the Library already present on the user's computer

       system, and (b) will operate properly with a modified version

       of the Library that is interface-compatible with the Linked

       Version.

   e) Provide Installation Information, but only if you would otherwise

   be required to provide such information under section 6 of the

   GNU GPL, and only to the extent that such information is

   necessary to install and execute a modified version of the

   Combined Work produced by recombining or relinking the

   Application with a modified version of the Linked Version. (If

   you use option 4d0, the Installation Information must accompany

   the Minimal Corresponding Source and Corresponding Application

   Code. If you use option 4d1, you must provide the Installation

   Information in the manner specified by section 6 of the GNU GPL

   for conveying Corresponding Source.)

  5. Combined Libraries.

  You may place library facilities that are a work based on the

Library side by side in a single library together with other library

facilities that are not Applications and are not covered by this

License, and convey such a combined library under terms of your

choice, if you do both of the following:

   a) Accompany the combined library with a copy of the same work based

   on the Library, uncombined with any other library facilities,

   conveyed under the terms of this License.

   b) Give prominent notice with the combined library that part of it

   is a work based on the Library, and explaining where to find the

   accompanying uncombined form of the same work.

  6. Revised Versions of the GNU Lesser General Public License.

  The Free Software Foundation may publish revised and/or new versions

of the GNU Lesser General Public License from time to time. Such new

versions will be similar in spirit to the present version, but may

differ in detail to address new problems or concerns.

  Each version is given a distinguishing version number. If the

Library as you received it specifies that a certain numbered version

of the GNU Lesser General Public License "or any later version"

applies to it, you have the option of following the terms and

conditions either of that published version or of any later version

published by the Free Software Foundation. If the Library as you

received it does not specify a version number of the GNU Lesser

General Public License, you may choose any version of the GNU Lesser

General Public License ever published by the Free Software Foundation.

  If the Library as you received it specifies that a proxy can decide

whether future versions of the GNU Lesser General Public License shall

apply, that proxy's public statement of acceptance of any version is

permanent authorization for you to choose that version for the

Library.

# [hu]man [motion] low-level robot ontrol library

this is the low level robot control software to allow

human like motion generation on a humanoid robotic head

PROJECT(icub_humotion)

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

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

<!DOCTYPE QtCreatorProject>

<!-- Written by QtCreator 3.0.1, 2015-03-12T13:07: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">{3152ed58-41d1-426c-b17f-dbe12171436a}</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="ProjectExplorer.BuildConfiguration.BuildDirectory">/homes/sschulz/src/flobidev.core/humotion/examples/yarp_icub/qbuild</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">-1</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">false</value>

    <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</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">{ccede451-c1b6-4b92-a13d-14bd6a65dd35}</value>

 </data>

 <data>

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

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

 </data>

</qtcreator>

#pragma once

#include <humotion/server/joint_interface.h>

#include <boost/bimap.hpp>

#include <yarp/dev/PolyDriver.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/dev/ControlBoardInterfaces.h>

#include "icub_jointinterface.h"

//forward declaration to solve loop

class iCubJointInterface;

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

public:

    iCubDataReceiver(int period, yarp::dev::IEncoders *_iencs, iCubJointInterface *_icub_jointinterface);

    bool threadInit();

    void threadRelease();

    void run();

private:

    double get_timestamp_ms();

    yarp::sig::Vector positions;

    yarp::sig::Vector velocities;

    yarp::sig::Vector commands;

    yarp::dev::IEncoders *iencs;

    iCubJointInterface *icub_jointinterface;

};

#pragma once

#include <humotion/server/joint_interface.h>

#include <boost/bimap.hpp>

#include <yarp/dev/PolyDriver.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 iCubJointInterface : public humotion::server::JointInterface{

public:

    iCubJointInterface(std::string scope);

    ~iCubJointInterface();

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

    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,

        //the following ids are used for remapping:

        ICUB_ID_EYES_LEFT_LR,

        ICUB_ID_EYES_RIGHT_LR,

        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

    };

    static const int MAIN_LOOP_FREQUENCY = 50;

protected:

    void disable_joint(int e);

    void publish_target_position(int e);

    void enable_joint(int e);

    void execute_motion();

private:

    double get_timestamp_ms();

    double target_angle[ICUB_JOINT_ID_ENUM_SIZE];

    double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];

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

    yarp::dev::PolyDriver dd;

    yarp::sig::Vector positions;

    yarp::sig::Vector velocities;

    yarp::sig::Vector commands;

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

    yarp::dev::IVelocityControl *ivel;

    yarp::dev::IPositionControl *ipos;

    yarp::dev::IEncoders *iencs;

    yarp::dev::IControlLimits *ilimits;

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

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

};

#include "icub_data_receiver.h"

#include <yarp/os/Property.h>

using namespace yarp::dev;

using namespace yarp::sig;

using namespace yarp::os;

using namespace std;

iCubDataReceiver::iCubDataReceiver(int period, IEncoders *_iencs, iCubJointInterface *_icub_jointinterface):RateThread(period){

    iencs = _iencs;

    icub_jointinterface = _icub_jointinterface;

    int joints;

    iencs->getAxes(&joints);

    positions.resize(joints);

    velocities.resize(joints);

}

bool iCubDataReceiver::threadInit(){ return true; }

void iCubDataReceiver::threadRelease(){ }

double iCubDataReceiver::get_timestamp_ms(){

    struct timespec spec;

    clock_gettime(CLOCK_REALTIME, &spec);

    return spec.tv_sec+spec.tv_nsec/1000000000.0;

}

void iCubDataReceiver::run(){

    //grab pos+vel data:

    iencs->getEncoders(positions.data());

    iencs->getEncoderSpeeds(velocities.data());

    double timestamp = get_timestamp_ms();

    //publish data to humotion

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

        icub_jointinterface->fetch_position(i, positions[i], timestamp);

        icub_jointinterface->fetch_speed(i, velocities[i], timestamp);

    }

    //tell humotion to update lid angle (hack)

    icub_jointinterface->fetch_position(100, 0.0, timestamp);

    //fixme: use real id

}

#include "icub_jointinterface.h"

#include <yarp/os/Property.h>

using namespace yarp::dev;

using namespace yarp::sig;

using namespace yarp::os;

using namespace std;

/*running:

/media/local_data/sschulz/iros15/icub-nightly/share/iCub/contexts/simConfig:> iCub_SIM

/media/local_data/sschulz/iros15/icub-nightly/share/iCub/contexts/simFaceExpressions:> ../../../../bin/simFaceExpressions

yarp connect /face/eyelids /icubSim/face/eyelids

yarp connect /face/image/out /icubSim/texture/face

TEST: yarp write /writer /icubSim/face/raw/in

http://wiki.icub.org/wiki/Motor_control

*/

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

#define POSITION_CONTROL 1

//! constructor

iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointInterface(){

    scope = _scope;

    //add mapping from ids to enums:

    //this might look strange at the first sight but we need to have a generic

    //way to acces joints from libhumotion. therefore the lib uses its enum with ID_* enum ids

    //to access the joints. now we need to define a mapping to map those to our motor ids.

    //this is what we use the enum bimap for (convertion fro/to motorid is handled

    //by \sa convert_enum_to_motorid() and \sa convert_motorid_to_enum() lateron

    //MOUTH

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_UPPER,   ID_LIP_LEFT_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_LOWER,   ID_LIP_LEFT_LOWER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_UPPER, ID_LIP_CENTER_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_LOWER, ID_LIP_CENTER_LOWER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_UPPER,  ID_LIP_RIGHT_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_LOWER,  ID_LIP_RIGHT_LOWER));

    //NECK

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_PAN,    ID_NECK_PAN));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_TILT,   ID_NECK_TILT));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_ROLL,   ID_NECK_ROLL));

    //EYES

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LR,   ID_EYES_LEFT_LR));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LR,   ID_EYES_RIGHT_LR));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_BOTH_UD,   ID_EYES_BOTH_UD));

    //EYELIDS

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_LOWER, ID_EYES_LEFT_LID_LOWER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_UPPER, ID_EYES_LEFT_LID_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_BROW, ID_EYES_LEFT_BROW));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_LOWER, ID_EYES_RIGHT_LID_LOWER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_UPPER,ID_EYES_RIGHT_LID_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW));

    Property options;

    options.put("device", "remote_controlboard");

    options.put("local", "/local/head");

    options.put("remote", scope+"/head");

    dd.open(options);

    //fetch views:

    dd.view(iencs);

    dd.view(ipos);

    dd.view(ivel);

    dd.view(ilimits);

    if ( (!iencs) || (!ipos) || (!ilimits) || (!ivel)){

        printf("> ERROR: failed to open icub views\n");

        exit(EXIT_FAILURE);

    }

    int joints;

    //tell humotion about min/max joint values:

    init_joints();

    iencs->getAxes(&joints);

    positions.resize(joints);

    velocities.resize(joints);

    commands.resize(joints);

    //set position mode:

    if (POSITION_CONTROL){

        commands=200000.0;

        ipos->setRefAccelerations(commands.data());

        ipos->setPositionMode();

    }else{

        ivel->setVelocityMode();

        commands=1000000;

        ivel->setRefAccelerations(commands.data());

    }

    //attach to facial expressions:

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

    printf("> opening connection to %s\n", emotion_scope.c_str());

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

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

        string emotion_port_out = "/emotionwriter" + to_string(i);

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

            printf("> ERROR: failed to open to %s\n",emotion_port_out.c_str());

            exit(EXIT_FAILURE);

        }

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

            printf("> ERROR: failed to connect emotion ports\n");

            exit(EXIT_FAILURE);

        }

    }

}

//! destructor

iCubJointInterface::~iCubJointInterface(){

}

//! conversion table for humotion motor ids to our ids:

//! \param enum from JointInterface::JOINT_ID_ENUM

//! \return int value of motor id

int iCubJointInterface::convert_enum_to_motorid(int e){

    enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(e);

    if(it == enum_id_bimap.right.end()) {

        //key does not exists, we are not interested in that dataset, return -1

        return -1;

    }

    return it->second;

}

//! conversion table for our ids to humotion motor ids:

//! \param  int value of motor id

//! \return enum from JointInterface::JOINT_ID_ENUM

int iCubJointInterface::convert_motorid_to_enum(int id){

    enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(id);

    if(it == enum_id_bimap.left.end()) {

        //key does not exists, we are not interested in that dataset, return -1

        return -1;

    }

    return it->second;

}

//! special command to set eyelid angle

//! \param angle in degrees

void iCubJointInterface::set_eyelid_angle(double 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 = min(48, max(24, opening));

        if (opening == lid_opening_previous){

            //no update necessary

            return;

        }

        lid_angle = angle;

        lid_opening_previous = opening;

        char buf[20];

        sprintf(buf, "S%2d", opening);

        //printf("> SETTING EYELID '%s' (%f -> %d)\n",buf,angle,opening);

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

        cmd.clear();

        cmd.addString(buf);

        emotion_port[0].write();

    }else{

        printf("> 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 iCubJointInterface::set_eyebrow_angle(int id){

    int port_id;

    if (id == 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:

        if (id==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;

        string cmd_s;

        if (id==ICUB_ID_EYES_LEFT_BROW){

            cmd_s = "L0" + to_string(icub_val);

        }else{

            cmd_s = "R0" + to_string(icub_val);

        }

        printf("> SETTING EYEBROW %d (%f -> %s)\n",id,angle,cmd_s.c_str());

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

        cmd.clear();

        cmd.addString(cmd_s);

        emotion_port[port_id].write();

    }else{

        printf("> ERROR: no icub emotion output\n");

        exit(EXIT_FAILURE);

    }

}

void iCubJointInterface::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 iCubJointInterface::publish_target_position(int e){

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

    }

}

//! set the target position of a joint

//! \param id of joint

//! \param float value of position

void iCubJointInterface::store_joint(int id, float value){

    //printf("> set joint %d = %f\n",id,value);

    target_angle[id] = value;

    //ipos->positionMove(id, value);

}

//! execute a move in position mode

//! \param id of joint

//! \param angle

void iCubJointInterface::set_target_in_positionmode(int id, double value){

    if (id>ICUB_ID_EYES_VERGENCE){

        printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,value);

        return;

    }

#if 0

    //set speed cacluated as in velocity + set position -> replicates smoothmotion from flobi?!

    //first: calculate necessary speed to reach the given target within the next clock tick:

    double distance = value - target_angle_previous[id];

    //make the motion smooth: we want to reach 85% of the target in the next iteration:

    //calculate speed for that:

    double speed = 0.85 * distance * ((double)MAIN_LOOP_FREQUENCY);

    //set up speed for controller:

    ipos->setRefSpeed(id, speed);

#endif

    //execute motion

    ipos->positionMove(id, value);

}

//! execute a move in velocity mode

//! \param id of joint

//! \param angle

void iCubJointInterface::set_target_in_velocitymode(int id, double value){

    //first: calculate necessary speed to reach the given target within the next clock tick:

    double distance = value - target_angle_previous[id];

    //make the motion smooth: we want to reach 85% of the target in the next iteration:

    distance = 0.85 * distance;

    //calculate speed

    double speed = distance * ((double)MAIN_LOOP_FREQUENCY);

    //execute:

    ivel->velocityMove(id, speed);

    //if (id == ICUB_ID_NECK_PAN) printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],value,distance,speed);

    target_angle_previous[id] = get_ts_position(convert_motorid_to_enum(id)).get_newest_value();

}

//! actually execute the scheduled motion commands

void iCubJointInterface::execute_motion(){

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

}

void iCubJointInterface::set_mouth(){

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

    int led_value = 0;

    //fetch center opening:

    double center_opening = target_angle[ICUB_ID_LIP_CENTER_LOWER] - target_angle[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[ICUB_ID_LIP_CENTER_LOWER] + target_angle[ICUB_ID_LIP_CENTER_UPPER])/2.0;

    double left_avg   = (target_angle[ICUB_ID_LIP_LEFT_LOWER] + target_angle[ICUB_ID_LIP_LEFT_UPPER])/2.0;

    double right_avg  = (target_angle[ICUB_ID_LIP_RIGHT_LOWER] + target_angle[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;

            }

        }

    }else if (diff < -3.0){

        if (mouth_open){

            led_value = 0x06;

        }else{

            led_value = 0x18;

        }

    }else if (diff < -2.0){

        if (mouth_open){

            led_value = 0x04; //0x25;

        }else{

            led_value = 0x08;

        }

    }else{

        if (mouth_open){

            led_value = 0x16;

        }else{

            led_value = 0x08;

        }

    }

    if (led_value == previous_mouth_state){

        //no update necessary

        return;

    }

    previous_mouth_state = led_value;

    //convert to string:

    char buf[10];

    sprintf(buf, "M%02X",led_value);

    /*printf("> sending mouth '%s'\n",buf);

    printf("> mouth angles: %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_UPPER],target_angle[ICUB_ID_LIP_CENTER_UPPER],target_angle[ICUB_ID_LIP_RIGHT_UPPER]);

    printf("  mouth         %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_LOWER],target_angle[ICUB_ID_LIP_CENTER_LOWER],target_angle[ICUB_ID_LIP_RIGHT_LOWER]);

    printf("  mouth  open=%3.2f diff=%3.2f\n", center_opening, diff);*/

    //add mouth:

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

    cmd.clear();

    cmd.addString(buf);

    emotion_port[3].write();

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

    double timestamp = get_timestamp_ms();

    JointInterface::store_incoming_position(ID_LIP_LEFT_UPPER,   target_angle[ICUB_ID_LIP_LEFT_UPPER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_LEFT_LOWER,   target_angle[ICUB_ID_LIP_LEFT_LOWER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_CENTER_UPPER, target_angle[ICUB_ID_LIP_CENTER_UPPER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_CENTER_LOWER, target_angle[ICUB_ID_LIP_CENTER_LOWER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_RIGHT_UPPER,  target_angle[ICUB_ID_LIP_RIGHT_UPPER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_RIGHT_LOWER,  target_angle[ICUB_ID_LIP_RIGHT_LOWER], timestamp);

}

double iCubJointInterface::get_timestamp_ms(){

    struct timespec spec;

    clock_gettime(CLOCK_REALTIME, &spec);

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

}

//! set the current position of a joint

//! \param id of joint

//! \param float value of position

//! \param double timestamp

void iCubJointInterface::fetch_position(int id, double value, double timestamp){

    //store joint based on id:

    switch(id){

        default:

            printf("> ERROR: unhandled joint id %d\n",id);

            return;

        case(100):

            JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp);

            break;

        case(2):

            //PAN is inverted!

            JointInterface::store_incoming_position(ID_NECK_PAN, -value, timestamp);

            break;

        case(0):

            JointInterface::store_incoming_position(ID_NECK_TILT, value, timestamp);

            break;

        case(1):

            JointInterface::store_incoming_position(ID_NECK_ROLL, value, timestamp);

            break;

        case(3):

            JointInterface::store_incoming_position(ID_EYES_BOTH_UD, value, timestamp);

            break;

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

        case(4): {//pan

            last_pos_eye_pan = value;

            float left  = last_pos_eye_pan + last_pos_eye_vergence/2.0;

            float right = last_pos_eye_pan - last_pos_eye_vergence/2.0;

            //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);

            JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp);

            JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp);

            break;

        }

        case(5): { //vergence

            last_pos_eye_vergence = value;

            float left  = last_pos_eye_pan + last_pos_eye_vergence/2.0;

            float right = last_pos_eye_pan - last_pos_eye_vergence/2.0;

            //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);

            JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp);

            JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp);

            break;

        }

    }

}

//! set the current speed of a joint

//! \param enum id of joint

//! \param float value of speed

//! \param double timestamp

void iCubJointInterface::fetch_speed(int id, double value, double timestamp){

    switch(id){

        default:

            printf("> ERROR: unhandled joint id %d\n",id);

            return;

        case(2):

            JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp);

            break;

        case(0):

            JointInterface::store_incoming_speed(ID_NECK_TILT, value, timestamp);

            break;

        case(1):

            JointInterface::store_incoming_speed(ID_NECK_ROLL, value, timestamp);

            break;

        case(3):

            JointInterface::store_incoming_speed(ID_EYES_BOTH_UD, value, timestamp);

            break;

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

        case(4): {//pan

            last_vel_eye_pan = value;

            float left  = last_vel_eye_pan + last_vel_eye_vergence/2.0;

            float right = last_vel_eye_pan - last_vel_eye_vergence/2.0;

            //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);

            JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp);

            JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp);

            break;

        }

        case(5): { //vergence

            last_vel_eye_pan = value;

            float left  = last_vel_eye_pan + last_vel_eye_vergence/2.0;

            float right = last_vel_eye_pan - last_vel_eye_vergence/2.0;

            //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);

            JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp);

            JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp);

            break;

        }

    }

/*

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

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

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

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

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

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

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

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

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

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

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

    JointInterface::store_incoming_speed(ID_EYES_RIGHT_BROW, 0.0, timestamp);*/

    /*

    //fetch enum id:

    int e = convert_motorid_to_enum(device->get_device_id());

    if (e == -1){

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

    }

    JointInterface::store_incoming_speed(e, device->get_register(XSC3_REGISTER_MOTORSPEED), timestamp);*/

}

/*

//! conversion table for humotion motor ids to our ids:

//! \param enum from JointInterface::JOINT_ID_ENUM

//! \return int value of motor id

int HumotionJointInterface::convert_enum_to_motorid(int e){

    enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(e);

    if(it == enum_id_bimap.right.end()) {

        //key does not exists, we are not interested in that dataset, return -1

        return -1;

    }

    return it->second;

}

//! conversion table for our ids to humotion motor ids:

//! \param  int value of motor id

//! \return enum from JointInterface::JOINT_ID_ENUM

int HumotionJointInterface::convert_motorid_to_enum(int id){

    enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(id);

    if(it == enum_id_bimap.left.end()) {

        //key does not exists, we are not interested in that dataset, return -1

        return -1;

    }

    return it->second;

}

*/

//! 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 iCubJointInterface::enable_joint(int e){

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