diff --git a/CMakeLists.txt b/CMakeLists.txt
index 313e8935..5cdaa960 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -21,12 +21,9 @@ endif()
# Libraries type
if(MSVC)
- option(BUILD_SHARED_LIBS "Compile WBToolbox as a shared library" OFF)
+ option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" OFF)
else()
- option(BUILD_SHARED_LIBS "Compile WBToolbox as a shared library" ON)
-endif()
-if(NOT BUILD_SHARED_LIBS)
- set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+ option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON)
endif()
# Control where binaries and libraries are placed in the build folder.
@@ -63,10 +60,11 @@ add_subdirectory(impl)
# Prepare YARP wrapper and devices
find_package(YARP 3.0 REQUIRED)
-set(BUILD_SHARED_LIBS ON)
-set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+set(YARP_FORCE_DYNAMIC_PLUGINS ON)
yarp_configure_plugins_installation(wearable)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
add_subdirectory(msgs)
add_subdirectory(devices)
add_subdirectory(wrappers)
diff --git a/XSensMVN/src/XSensMVNCalibrator.cpp b/XSensMVN/src/XSensMVNCalibrator.cpp
index e6bc7be2..70c1c989 100644
--- a/XSensMVN/src/XSensMVNCalibrator.cpp
+++ b/XSensMVN/src/XSensMVNCalibrator.cpp
@@ -169,7 +169,7 @@ namespace xsensmvn {
}
// Wait for the discard operation to be completed
- while (!m_operationCompleted && !m_calibrationAborted) {
+ while (m_suitsConnector.isCalibrationPerformed(calibrationType) && !m_calibrationAborted) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
if (m_calibrationAborted) {
@@ -183,8 +183,9 @@ namespace xsensmvn {
// Get the phases of the selected calibration type
XsIntArray calibPhases = m_suitsConnector.calibrationPhaseList();
- // Start the calibration data collection
+ // Start the calibration data collection (Wait three seconds to give the subject enough time to take position)
xsInfo << "Starting " << calibrationType << " calibration" << std::endl;
+ std::this_thread::sleep_for(std::chrono::milliseconds(3000));
m_suitsConnector.startCalibration();
// Follow step-by-step the calibration phases of the selected type
@@ -270,14 +271,7 @@ namespace xsensmvn {
}
else {
// Notify the user the calibration can be applied
- xsInfo << "Ready to apply the obtained calibration. Stand still in starting position. "
- "Applying in: ";
-
- // Wait three seconds to give the subject enough time to take position
- for (int s = 5; s >= 0; --s) {
- xsInfo << s << " s";
- std::this_thread::sleep_for(std::chrono::milliseconds(1000));
- }
+ xsInfo << "Ready to apply the obtained calibration";
// Apply the calibration to the MVN Engine and wait for a positive feedback
m_suitsConnector.finalizeCalibration();
diff --git a/app/Wearables-yarpmanager.ini b/app/Wearables-yarpmanager.ini
new file mode 100644
index 00000000..c163290c
--- /dev/null
+++ b/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
diff --git a/app/xml/FTShoeLeftWearableDevice.xml b/app/xml/FTShoeLeftWearableDevice.xml
new file mode 100644
index 00000000..f78ecc8a
--- /dev/null
+++ b/app/xml/FTShoeLeftWearableDevice.xml
@@ -0,0 +1,148 @@
+
+
+
+
+
+ "ecan"
+ "ecan"
+ 0
+ 0x01
+ 16
+ 6
+ 5
+ 1
+ 1
+
+
+ "ecan"
+ "ecan"
+ 0
+ 0x02
+ 16
+ 6
+ 5
+ 1
+ 1
+
+
+
+
+ 10
+ /ft/ftShoe_Left/analog:o
+
+
+
+ (-0.181101, 0.0, 0.0)
+
+
+ ( -1.0, 0.0, 0.0,
+ 0.0, -1.0, 0.0,
+ 0.0, 0.0, 1.0
+ )
+
+
+
+
+
+
+ (
+ -1.0, 0.0, 0.0,
+ 0.0, 1.0, 0.0,
+ 0.0, 0.0, -1.0
+ )
+
+
+ false
+
+
+ (
+ 0.938857303261460 -0.099987001538724 -0.031719474716931 0.934262757200198 -2.081128504120452 1.116222891312317
+ -0.008782994221006 0.755393650114154 0.045656043724651 -1.655858713166938 -0.627809636881184 -1.815251879665972
+ -0.013675176859862 0.062209239624336 0.991900304766940 -0.285473412550362 0.672237690815685 0.094924727111362
+ -0.003522241315859 0.031293495376460 0.005788286130503 0.909299295313223 -0.034622536733099 -0.026646251320573
+ -0.029397178956236 0.014719174094312 0.001425795437255 -0.055596915544064 1.100253681658064 0.017844947080158
+ -0.003939180755661 0.010569279599475 0.000530902451101 -0.026442188019439 0.021906211171418 1.087258387321892
+ )
+
+ (
+ 0.826424294040360 -0.091357422730653 0.029217798555402 0.413143077503409 -3.003143284075124 -0.734326709982322
+ -0.043527467867752 0.945992379875415 -0.013974374847989 -2.158570031731984 -0.364385509664276 0.178660582281101
+ 0.018456480855675 -0.027680967629762 0.995373889642780 -0.378801203422740 0.405083974855969 -0.399854054343818
+ -0.001858452062923 0.028548921877088 -0.001296050687382 0.882390054944027 -0.021267049818161 0.012534000056372
+ -0.024912614354884 -0.002610851373780 -0.001454598169520 -0.013007004521544 1.198536139399159 0.056633344983102
+ -0.001526052208884 -0.008395096178476 -0.000888111548024 0.027328873291096 -0.022322328373427 0.963011521138837
+ )
+
+
+
+
+ ftShoe_Left_Front
+ ftShoe_Left_Rear
+
+
+
+
+
+
+
+
+ 10
+ /ft/ftShoe_Left/analog:o
+
+
+ ftShoe_l
+
+
+
+
+
+
+
+ 10
+ /ft/ftShoe_Left_Front/analog:o
+
+
+ ftShoe_Left_Front
+
+
+
+
+
+ 10
+ /ft/ftShoe_Left_Rear/analog:o
+
+
+ ftShoe_Left_Rear
+
+
+
+
+
+
+ FTShoeLeftFTSensors
+ FTShoeLeft
+ 6
+ 0
+ ForceTorque6DSensor
+ true
+
+
+ ftShoe_l
+
+
+
+
+
+
+ 0.01
+ /FTShoeLeft/WearableData/data:o
+ /FTShoeLeft/WearableData/metadataRpc:o
+
+
+ FTShoeLeftIAnalogSensorToIWear
+
+
+
+
+
+
diff --git a/app/xml/FTShoeRightWearableDevice.xml b/app/xml/FTShoeRightWearableDevice.xml
new file mode 100644
index 00000000..aab0a1c3
--- /dev/null
+++ b/app/xml/FTShoeRightWearableDevice.xml
@@ -0,0 +1,148 @@
+
+
+
+
+
+ "ecan"
+ "ecan"
+ 0
+ 0x03
+ 16
+ 6
+ 5
+ 1
+ 1
+
+
+ "ecan"
+ "ecan"
+ 0
+ 0x04
+ 16
+ 6
+ 5
+ 1
+ 1
+
+
+
+
+ 10
+ /ft/ftShoe_Right/analog:o
+
+
+
+ (-0.181101, 0.0, 0.0)
+
+
+ ( -1.0, 0.0, 0.0,
+ 0.0, -1.0, 0.0,
+ 0.0, 0.0, 1.0
+ )
+
+
+
+
+
+
+ (
+ -1.0, 0.0, 0.0,
+ 0.0, 1.0, 0.0,
+ 0.0, 0.0, -1.0
+ )
+
+
+ false
+
+
+ (
+ 0.927495744506558 0.041016654619440 -0.020768066468872 -0.686739129220131 -1.700341714921413 -0.655844186466448
+ 0.032691498454271 0.905387122196879 -0.062060254943741 -2.528433638522609 1.936698940397620 -1.261426627647161
+ 0.030307518024746 -0.042708402432872 0.990834772663811 -0.771255812731485 1.379976452711553 1.338383628433477
+ 0.003417328018857 0.027192538668070 0.000903874352365 0.934072112166630 0.018298809861597 0.009727347237814
+ -0.029683885285561 -0.009657076123040 -0.001319794460849 0.070540847374860 1.112394249787454 -0.081039701093926
+ -0.003720738063755 0.002403350518587 -0.001256203878104 -0.004909951055368 0.004652419860706 1.023620297977032
+ )
+
+ (
+ 0.752606509808956 0.128485214807810 0.037816833951859 -1.189506447082422 -3.528696946955530 0.367956429295563
+ 0.032994788492599 0.947861991843522 0.014131047867024 -2.503874602191483 -0.306564401641517 0.033900728916422
+ 0.010291417081055 -0.016786848400983 0.997235043288604 0.044337750405427 0.628784109489714 0.608479361767708
+ -0.000534324857619 0.039993343127271 -0.000046443326033 0.846610774589901 -0.114938934447660 -0.164828223499561
+ -0.021558854024374 0.008200634539163 -0.003084470512841 0.036116559584761 1.220596115437195 -0.067513275649460
+ -0.000434930362439 0.001589856391163 -0.000514737521434 -0.009663262776501 0.025349107573698 1.107985249217049
+ )
+
+
+
+
+ ftShoe_Right_Front
+ ftShoe_Right_Rear
+
+
+
+
+
+
+
+
+ 10
+ /ft/ftShoe_Right/analog:o
+
+
+ ftShoe_r
+
+
+
+
+
+
+
+ 10
+ /ft/ftShoe_Right_Front/analog:o
+
+
+ ftShoe_Right_Front
+
+
+
+
+
+ 10
+ /ft/ftShoe_Right_Rear/analog:o
+
+
+ ftShoe_Right_Rear
+
+
+
+
+
+
+ FTShoeRightFTSensors
+ FTShoeRight
+ 6
+ 0
+ ForceTorque6DSensor
+ true
+
+
+ ftShoe_r
+
+
+
+
+
+
+ 0.01
+ /FTShoeRight/WearableData/data:o
+ /FTShoeRight/WearableData/metadataRpc:o
+
+
+ FTShoeRightIAnalogSensorToIWear
+
+
+
+
+
+
diff --git a/app/xml/ICubWearableDevice.xml b/app/xml/ICubWearableDevice.xml
new file mode 100644
index 00000000..34eb7b86
--- /dev/null
+++ b/app/xml/ICubWearableDevice.xml
@@ -0,0 +1,21 @@
+
+
+
+
+ /wholeBodyDynamics/left_arm/endEffectorWrench:o
+ /wholeBodyDynamics/right_arm/endEffectorWrench:o
+
+
+
+
+ 0.01
+ /ICub/WearableData/data:o
+ /ICub/WearableData/metadataRpc:o
+
+
+ ICubWearableDevice
+
+
+
+
+
diff --git a/app/xml/SkinInsolesWearableDevice.xml b/app/xml/SkinInsolesWearableDevice.xml
new file mode 100644
index 00000000..33e2dcf8
--- /dev/null
+++ b/app/xml/SkinInsolesWearableDevice.xml
@@ -0,0 +1,70 @@
+
+
+
+
+
+ 0
+ 5 6 7 8 9 10
+ 20
+ socketcan
+
+
+
+ 50
+ 0
+ 0xff
+ false
+
+
+ true
+ 3
+ 0x2200
+
+
+
+
+
+ 20
+ 1152
+ skinWrapper
+
+ 0 1151 0 1151
+
+
+
+
+ skin_insoles
+
+
+
+
+
+
+ SkinInsolesSensor
+ SkinInsoles
+ 1152
+ 0
+ SkinSensor
+
+
+ skin_insoles
+
+
+
+
+
+
+ 0.01
+ /SkinInsoles/WearableData/data:o
+ /SkinInsoles/WearableData/metadataRpc:o
+
+
+ SkinInsolesIAnalogSensorToIWear
+
+
+
+
+
+
diff --git a/conf/Xsens/xsens_windows_all_wrappers.xml b/app/xml/XsensSuitWearableDevice.xml
similarity index 98%
rename from conf/Xsens/xsens_windows_all_wrappers.xml
rename to app/xml/XsensSuitWearableDevice.xml
index 3378bcef..5b4149e7 100644
--- a/conf/Xsens/xsens_windows_all_wrappers.xml
+++ b/app/xml/XsensSuitWearableDevice.xml
@@ -28,7 +28,7 @@
true
true
- false
+ true
diff --git a/app/xml/applications/WearableDevices.xml b/app/xml/applications/WearableDevices.xml
new file mode 100644
index 00000000..0585e241
--- /dev/null
+++ b/app/xml/applications/WearableDevices.xml
@@ -0,0 +1,50 @@
+
+ WearableDevices
+ An application for running wearable devices
+
+
+
+ yarprobotinterface
+ --config XsensSuitWearableDevice.xml
+ YARP_FORWARD_LOG_ENABLE=1
+ Run xsens suit wearable device
+ localhost
+
+
+
+
+ yarprobotinterface
+ --config FTShoeLeftWearableDevice.xml
+ YARP_FORWARD_LOG_ENABLE=1
+ Run left ftshoe wearable device
+ localhost
+
+
+
+
+ yarprobotinterface
+ --config FTShoeRightWearableDevice.xml
+ YARP_FORWARD_LOG_ENABLE=1
+ Run right ftshoe wearable device
+ localhost
+
+
+
+
+ yarprobotinterface
+ --config SkinInsolesWearableDevice.xml
+ YARP_FORWARD_LOG_ENABLE=1
+ Run skin insoles wearable device
+ localhost
+
+
+
+
+ yarprobotinterface
+ --config ICubWearableDevice.xml
+ YARP_FORWARD_LOG_ENABLE=1
+ Run icub wearable device
+ localhost
+
+
+
diff --git a/devices/CMakeLists.txt b/devices/CMakeLists.txt
index 74b62a65..b341afba 100644
--- a/devices/CMakeLists.txt
+++ b/devices/CMakeLists.txt
@@ -8,3 +8,5 @@ add_subdirectory(IAnalogSensorToIWear)
if(XSENS_MVN_USE_SDK)
add_subdirectory(XsensSuit)
endif()
+
+add_subdirectory(ICub)
diff --git a/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp b/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp
index 015d9a07..4d42ac86 100644
--- a/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp
+++ b/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp
@@ -120,6 +120,14 @@ static class IAnalogSensorHandler
std::copy(buffer.data() + offset, buffer.data() + offset + 4, array.data());
return true;
}
+
+ // This is for skin data
+ bool getData(std::vector& vector, const size_t offset = 0)
+ {
+ std::copy(buffer.data() + offset, buffer.data() + buffer.size() + offset, vector.data());
+ return true;
+ }
+
} handler;
// ================================
@@ -130,6 +138,7 @@ class ForceTorque6DSensor : public IForceTorque6DSensor
{
public:
unsigned offset = 0;
+ bool groundReactionFT;
ForceTorque6DSensor(SensorName name, SensorStatus status = SensorStatus::Unknown)
: IForceTorque6DSensor(name, status)
@@ -149,7 +158,17 @@ class ForceTorque6DSensor : public IForceTorque6DSensor
// TODO: The positions of force and torques are hardcoded. Forces should be the first
// triplet of elements of the read vector and torques the second one.
- return handler.getData(force3D, offset) && handler.getData(torque3D, offset + 3);
+ bool ok = handler.getData(force3D, offset) && handler.getData(torque3D, offset + 3);
+ if (groundReactionFT) {
+ force3D[0] = -1 * force3D[0];
+ force3D[1] = -1 * force3D[1];
+ force3D[2] = -1 * force3D[2];
+
+ torque3D[0] = -1 * torque3D[0];
+ torque3D[1] = -1 * torque3D[1];
+ torque3D[2] = -1 * torque3D[2];
+ }
+ return ok;
}
};
@@ -228,6 +247,31 @@ class TemperatureSensor : public ITemperatureSensor
}
};
+class SkinSensor : public ISkinSensor
+{
+public:
+ unsigned offset = 0;
+
+ SkinSensor(SensorName name, SensorStatus status = SensorStatus::Unknown)
+ : ISkinSensor(name, status)
+ {}
+
+ void setStatus(const SensorStatus status) { m_status = status; }
+
+ bool getPressure(std::vector& pressure) const override
+ {
+ if (!handler.readData()) {
+ return false;
+ }
+
+ // Dirty workaround to set the status from a const method
+ auto nonConstThis = const_cast(this);
+ nonConstThis->setStatus(handler.getStatus());
+
+ return handler.getData(pressure, offset);
+ }
+};
+
// TODO: implement all the other sensors
// ====================
@@ -281,6 +325,8 @@ struct ParsedOptions
size_t numberOfChannels;
size_t channelOffset;
+
+ bool getGroundReactionFT;
};
class IAnalogSensorToIWear::Impl
@@ -337,6 +383,11 @@ bool IAnalogSensorToIWear::open(yarp::os::Searchable& config)
return false;
}
+ if (!(config.check("getGroundReactionFT") && config.find("getGroundReactionFT").isBool())) {
+ yError() << LogPrefix << "Parameter 'getGroundReactionFT' missing or invalid";
+ return false;
+ }
+
// ===============
// READ PARAMETERS
// ===============
@@ -346,6 +397,7 @@ bool IAnalogSensorToIWear::open(yarp::os::Searchable& config)
pImpl->options.numberOfChannels = config.find("numberOfChannels").asInt();
pImpl->options.channelOffset = config.find("channelOffset").asInt();
std::string sensorType = config.find("wearableSensorType").asString();
+ pImpl->options.getGroundReactionFT = config.find("getGroundReactionFT").asBool();
pImpl->options.wearableSensorType = sensorTypeFromString(sensorType);
yInfo() << LogPrefix << "*** ====================";
@@ -354,6 +406,7 @@ bool IAnalogSensorToIWear::open(yarp::os::Searchable& config)
yInfo() << LogPrefix << "*** Wearable name :" << pImpl->options.wearableName;
yInfo() << LogPrefix << "*** Number of channels :" << pImpl->options.numberOfChannels;
yInfo() << LogPrefix << "*** Channel offset :" << pImpl->options.channelOffset;
+ yInfo() << LogPrefix << "*** Ground reaction FT :" << pImpl->options.getGroundReactionFT;
yInfo() << LogPrefix << "*** ====================";
// =====================
@@ -393,6 +446,7 @@ bool IAnalogSensorToIWear::Impl::allocateSensor(const wearable::sensor::SensorTy
case wearable::sensor::SensorType::ForceTorque6DSensor: {
auto sensor = std::make_shared(name, SensorStatus::Ok);
sensor->offset = options.channelOffset;
+ sensor->groundReactionFT = options.getGroundReactionFT;
iSensor = std::dynamic_pointer_cast(sensor);
break;
}
@@ -408,6 +462,12 @@ bool IAnalogSensorToIWear::Impl::allocateSensor(const wearable::sensor::SensorTy
iSensor = std::dynamic_pointer_cast(sensor);
break;
}
+ case wearable::sensor::SensorType::SkinSensor: {
+ auto sensor = std::make_shared(name, SensorStatus::Ok);
+ sensor->offset = options.channelOffset;
+ iSensor = std::dynamic_pointer_cast(sensor);
+ break;
+ }
default:
// TODO: implement the remaining sensors
return false;
@@ -641,9 +701,14 @@ IAnalogSensorToIWear::getPositionSensor(const wearable::sensor::SensorName /*nam
}
wearable::SensorPtr
-IAnalogSensorToIWear::getSkinSensor(const wearable::sensor::SensorName /*name*/) const
+IAnalogSensorToIWear::getSkinSensor(const wearable::sensor::SensorName name) const
{
- return nullptr;
+ if (!(pImpl->iSensor && (pImpl->iSensor->getSensorName() == name))) {
+ yError() << LogPrefix << "Failed to get sensor" << name;
+ return nullptr;
+ }
+
+ return std::dynamic_pointer_cast(pImpl->iSensor);
}
wearable::SensorPtr
diff --git a/devices/ICub/CMakeLists.txt b/devices/ICub/CMakeLists.txt
new file mode 100644
index 00000000..dbddecf5
--- /dev/null
+++ b/devices/ICub/CMakeLists.txt
@@ -0,0 +1,31 @@
+# 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
+ $)
+
+target_link_libraries(ICub PUBLIC
+ Wearable::IWear YARP::YARP_dev)
+
+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})
+
diff --git a/devices/ICub/conf/ICub.xml b/devices/ICub/conf/ICub.xml
new file mode 100644
index 00000000..d87f6fad
--- /dev/null
+++ b/devices/ICub/conf/ICub.xml
@@ -0,0 +1,22 @@
+
+
+
+
+ 2
+ /wholeBodyDynamics/left_arm/endEffectorWrench:o
+ /wholeBodyDynamics/right_arm/endEffectorWrench:o
+
+
+
+
+ 0.01
+ /ICub/WearableData/data:o
+ /ICub/WearableData/metadataRpc:o
+
+
+ ICubWearableDevice
+
+
+
+
+
diff --git a/devices/ICub/icub_wearable_device.ini b/devices/ICub/icub_wearable_device.ini
new file mode 100644
index 00000000..0b5505d3
--- /dev/null
+++ b/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
diff --git a/devices/ICub/include/ICub.h b/devices/ICub/include/ICub.h
new file mode 100644
index 00000000..6ac6b0d8
--- /dev/null
+++ b/devices/ICub/include/ICub.h
@@ -0,0 +1,208 @@
+/*
+ * 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.
+ */
+
+#ifndef ICUB_H
+#define ICUB_H
+
+#include "Wearable/IWear/IWear.h"
+
+#include
+#include
+
+#include
+
+namespace wearable {
+ namespace devices {
+ class ICub;
+ } // namespace devices
+} // namespace wearable
+
+class wearable::devices::ICub final
+ : public yarp::dev::DeviceDriver
+ , public yarp::dev::IPreciselyTimed
+ , public wearable::IWear
+{
+private:
+ class ICubImpl;
+ std::unique_ptr pImpl;
+
+public:
+ ICub();
+ ~ICub() override;
+
+ ICub(const ICub& other) = delete;
+ ICub(ICub&& other) = delete;
+ ICub& operator=(const ICub& other) = delete;
+ ICub& operator=(ICub&& other) = delete;
+
+ // =============
+ // DEVICE DRIVER
+ // =============
+
+ bool open(yarp::os::Searchable& config) override;
+ bool close() override;
+
+ // ================
+ // IPRECISELY TIMED
+ // ================
+
+ yarp::os::Stamp getLastInputStamp() override;
+
+ // =====
+ // IWEAR
+ // =====
+
+ // GENERIC
+ // -------
+
+ WearableName getWearableName() const override;
+ WearStatus getStatus() const override;
+ TimeStamp getTimeStamp() const override;
+
+ SensorPtr getSensor(const sensor::SensorName name) const override;
+
+ VectorOfSensorPtr getSensors(const sensor::SensorType) const override;
+
+ // IMPLEMENTED SENSORS
+ // -------------------
+
+ SensorPtr
+ getForceTorque6DSensor(const sensor::SensorName name) const override;
+
+ // UNIMPLEMENTED SENSORS
+ // ---------------------
+
+ inline SensorPtr
+ getFreeBodyAccelerationSensor(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getMagnetometer(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getOrientationSensor(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getPoseSensor(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getPositionSensor(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getVirtualLinkKinSensor(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getVirtualSphericalJointKinSensor(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getAccelerometer(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getEmgSensor(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getForce3DSensor(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getGyroscope(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getSkinSensor(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getTemperatureSensor(const sensor::SensorName /*name*/) const override;
+
+ inline SensorPtr
+ getTorque3DSensor(const sensor::SensorName /*name*/) const override;
+
+};
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getFreeBodyAccelerationSensor(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getMagnetometer(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getOrientationSensor(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getPoseSensor(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getPositionSensor(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getVirtualLinkKinSensor(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getVirtualSphericalJointKinSensor(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getAccelerometer(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getEmgSensor(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getForce3DSensor(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getGyroscope(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getSkinSensor(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getTemperatureSensor(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+inline wearable::SensorPtr
+wearable::devices::ICub::getTorque3DSensor(const sensor::SensorName /*name*/) const
+{
+ return nullptr;
+}
+
+#endif // ICUB_H
diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp
new file mode 100644
index 00000000..0951cab1
--- /dev/null
+++ b/devices/ICub/src/ICub.cpp
@@ -0,0 +1,345 @@
+/*
+ * 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.
+ */
+
+#include "ICub.h"
+
+#include
+#include
+#include
+
+#include
+#include