Implementation of ICub device

app/Wearables-yarpmanager.ini

@@ -0,0 +1,32 @@
+# Path to the folder which contains application description files
+apppath =  "xml/applications"
+load_subfolders = yes
+
+# Path to the folder which contains module description files
+#modpath =  "xml/modules"
+
+# Path to the folder which contains module description files
+#respath =  "xml/resources"
+
+# Fault tolerance
+# parameters: yes|No
+watchdog = no
+
+# Module failure awareness (if watchdog is enabled)
+# parameters: Ignore|terminate|prompt|recover
+module_failure = prompt
+
+# Connection failure awareness (if watchdog is enabled)
+# parameters: Ignore|terminate|prompt
+connection_failure = prompt
+
+# Automatically establish all connections
+# parameters: Yes|no
+auto_connect = no
+
+# Appearance (for yarpmanager)
+# parameters: No|dark|light
+color_theme = dark
+
+# External editor (e.g. gedit, notepad)
+external_editor = gvim

app/xml/ICubWearableDevice.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<robot name="ICub">
+    <device type="icub_wearable_device" name="ICubWearableDevice">
+        <param name="urdf">model.urdf</param>
+        <param name="floatingBase">root_link</param>
+        <group name="ft-sensors">
+            <param name="leftHandWBDFTSensor">/wholeBodyDynamics/left_arm/endEffectorWrench:o</param>
+            <param name="rightHandWBDFTSensor">/wholeBodyDynamics/right_arm/endEffectorWrench:o</param>
+            <param name="LeftFootWBDFTSensor">/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o</param>
+            <param name="rightFootWBDFTSensor">/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o</param>
+        </group>
+        <group name="joint-sensors">
+            <param name="controlBoardsList">(head torso left_arm right_arm left_leg right_leg)</param>
+            <param name="remotePrefix">/icubSim</param>
+            <param name="localPrefix">/ICubWearableRemoteClient</param>
+        </group>
+    </device>
+
+    <device type="iwear_wrapper" name="ICubWearableDeviceWrapper">
+        <param name="period">0.01</param>
+        <param name="dataPortName">/ICub/WearableData/data:o</param>
+        <param name="rpcPortName">/ICub/WearableData/metadataRpc:o</param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <elem name="ICubWearableDeviceWrapper"> ICubWearableDevice </elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="5" type="detach"/>
+    </device>
+</robot>

app/xml/applications/WearableDevices.xml

@@ -0,0 +1,50 @@
+<application>
+  <name>WearableDevices</name>
+  <description>An application for running wearable devices</description>
+
+  <!--yarprobotinterface for xsens suit wearable device-->
+  <module>
+    <name>yarprobotinterface</name>
+    <parameters>--config XsensSuitWearableDevice.xml</parameters>
+    <environment>YARP_FORWARD_LOG_ENABLE=1</environment>
+    <description>Run xsens suit wearable device</description>
+    <node>localhost</node>
+  </module>
+
+  <!--yarprobotinterface for left ftshoe wearable device-->
+  <module>
+    <name>yarprobotinterface</name>
+    <parameters>--config FTShoeLeftWearableDevice.xml</parameters>
+    <environment>YARP_FORWARD_LOG_ENABLE=1</environment>
+    <description>Run left ftshoe wearable device</description>
+    <node>localhost</node>
+  </module>
+
+  <!--yarprobotinterface for right ftshoe wearable device-->
+  <module>
+    <name>yarprobotinterface</name>
+    <parameters>--config FTShoeRightWearableDevice.xml</parameters>
+    <environment>YARP_FORWARD_LOG_ENABLE=1</environment>
+    <description>Run right ftshoe wearable device</description>
+    <node>localhost</node>
+  </module>
+
+  <!--yarprobotinterface for skin insoles wearable device-->
+  <module>
+    <name>yarprobotinterface</name>
+    <parameters>--config SkinInsolesWearableDevice.xml</parameters>
+    <environment>YARP_FORWARD_LOG_ENABLE=1</environment>
+    <description>Run skin insoles wearable device</description>
+    <node>localhost</node>
+  </module>
+
+  <!--yarprobotinterface for icub wearable device-->
+  <module>
+    <name>yarprobotinterface</name>
+    <parameters>--config ICubWearableDevice.xml</parameters>
+    <environment>YARP_FORWARD_LOG_ENABLE=1</environment>
+    <description>Run icub wearable device</description>
+    <node>localhost</node>
+  </module>
+
+</application>

devices/CMakeLists.txt

@@ -2,9 +2,16 @@
 # This software may be modified and distributed under the terms of the
 # GNU Lesser General Public License v2.1 or any later version.
 
+find_package(iDynTree)
+
 add_subdirectory(IWearRemapper)
 add_subdirectory(IAnalogSensorToIWear)
 
 if(XSENS_MVN_USE_SDK)
   add_subdirectory(XsensSuit)
 endif()
+
+if(iDynTree_FOUND)
+  add_subdirectory(ICub)
+endif()
+

devices/IAnalogSensorToIWear/include/IAnalogSensorToIWear.h

@@ -121,6 +121,9 @@ class wearable::devices::IAnalogSensorToIWear
     SensorPtr<const sensor::IVirtualLinkKinSensor>
     getVirtualLinkKinSensor(const sensor::SensorName name) const override;
 
+    SensorPtr<const sensor::IVirtualJointKinSensor>
+    getVirtualJointKinSensor(const sensor::SensorName name) const override;
+
     SensorPtr<const sensor::IVirtualSphericalJointKinSensor>
     getVirtualSphericalJointKinSensor(const sensor::SensorName name) const override;
 };

devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp

@@ -298,6 +298,8 @@ wearable::sensor::SensorType sensorTypeFromString(std::string sensorTypeString)
         return wearable::sensor::SensorType::Torque3DSensor;
     else if (sensorTypeString == "VirtualLinkKinSensor")
         return wearable::sensor::SensorType::VirtualLinkKinSensor;
+    else if (sensorTypeString == "VirtualJointKinSensor")
+        return wearable::sensor::SensorType::VirtualJointKinSensor;
     else if (sensorTypeString == "VirtualSphericalJointKinSensor")
         return wearable::sensor::SensorType::VirtualSphericalJointKinSensor;
     else {
@@ -707,6 +709,13 @@ IAnalogSensorToIWear::getVirtualLinkKinSensor(const wearable::sensor::SensorName
     return nullptr;
 }
 
+wearable::SensorPtr<const wearable::sensor::IVirtualJointKinSensor>
+IAnalogSensorToIWear::getVirtualJointKinSensor(
+    const wearable::sensor::SensorName /*name*/) const
+{
+    return nullptr;
+}
+
 wearable::SensorPtr<const wearable::sensor::IVirtualSphericalJointKinSensor>
 IAnalogSensorToIWear::getVirtualSphericalJointKinSensor(
     const wearable::sensor::SensorName /*name*/) const

devices/ICub/CMakeLists.txt

@@ -0,0 +1,37 @@
+# Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+yarp_prepare_plugin(icub_wearable_device
+    TYPE wearable::devices::ICub
+    INCLUDE ICub.h
+    CATEGORY device
+    ADVANCED
+    DEFAULT ON)
+
+yarp_add_plugin(ICub
+    src/ICub.cpp
+    include/ICub.h)
+
+target_include_directories(ICub PRIVATE
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
+
+target_link_libraries(ICub
+    PUBLIC
+        Wearable::IWear
+        YARP::YARP_dev
+        YARP::YARP_init
+    PRIVATE
+        iDynTree::idyntree-model
+        iDynTree::idyntree-high-level)
+
+yarp_install(
+    TARGETS ICub
+    COMPONENT runtime
+    LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
+    ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR})
+
+yarp_install(
+    FILES "${CMAKE_CURRENT_SOURCE_DIR}/icub_wearable_device.ini"
+    DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR})
+

devices/ICub/conf/ICub.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<robot name="ICub">
+    <device type="icub_wearable_device" name="ICubWearableDevice">
+        <param name="urdf">model.urdf</param>
+        <param name="floatingBase">root_link</param>
+        <group name="ft-sensors">
+            <param name="leftHandWBDFTSensor">/wholeBodyDynamics/left_arm/endEffectorWrench:o</param>
+            <param name="rightHandWBDFTSensor">/wholeBodyDynamics/right_arm/endEffectorWrench:o</param>
+            <param name="LeftFootWBDFTSensor">/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o</param>
+            <param name="rightFootWBDFTSensor">/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o</param>
+        </group>
+        <group name="joint-sensors">
+            <param name="controlBoardsList">(head torso left_arm right_arm left_leg right_leg)</param>
+            <param name="remotePrefix">/icubSim</param>
+            <param name="localPrefix">/ICubWearableRemoteClient</param>
+        </group>
+    </device>
+
+    <device type="iwear_wrapper" name="ICubWearableDeviceWrapper">
+        <param name="period">0.01</param>
+        <param name="dataPortName">/ICub/WearableData/data:o</param>
+        <param name="rpcPortName">/ICub/WearableData/metadataRpc:o</param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <elem name="ICubWearableDeviceWrapper"> ICubWearableDevice </elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="5" type="detach"/>
+    </device>
+</robot>

devices/ICub/icub_wearable_device.ini

@@ -0,0 +1,5 @@
+[plugin icub_wearable_device]
+type device
+name icub_wearable_device
+library ICub
+wrapper iwear_wrapper