Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Feature/icub device impl #16

Closed
wants to merge 43 commits into from
Closed

Feature/icub device impl #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
43 commits
Select commit Hold shift + click to select a range
4018710
Icub device initial commit
yeshasvitirupachuri Oct 4, 2018
862d72e
Implement sensor methods
yeshasvitirupachuri Oct 4, 2018
f35fc91
Implement getTimeStamp method
yeshasvitirupachuri Oct 4, 2018
0965fc3
Update CMakeLists.txt
yeshasvitirupachuri Oct 4, 2018
8e0fe6c
Update timestamp implementation
yeshasvitirupachuri Oct 5, 2018
144b2e8
Update CMakeLists and add config file
yeshasvitirupachuri Oct 5, 2018
47334b4
Implement getLastInputStamp method
yeshasvitirupachuri Oct 5, 2018
23c5bb5
Implement getStatus method
yeshasvitirupachuri Oct 5, 2018
d5a3166
Fix sensor type
yeshasvitirupachuri Oct 5, 2018
42b2142
Update wrapper details in config
yeshasvitirupachuri Oct 5, 2018
9ce2cbe
Update status handling
yeshasvitirupachuri Oct 5, 2018
b776173
Implement bufferport callback
yeshasvitirupachuri Oct 8, 2018
c8a58eb
Improve status update logic
yeshasvitirupachuri Oct 9, 2018
6de5a1c
Remode debug code
yeshasvitirupachuri Oct 9, 2018
15f975e
Update wrench handling
yeshasvitirupachuri Oct 10, 2018
310766a
Add mutex to wrench handling
yeshasvitirupachuri Oct 16, 2018
0b78858
Add IAnalogToIWear device config files for FTShoes
yeshasvitirupachuri Nov 23, 2018
b29785e
Update IWearWrapper port names
yeshasvitirupachuri Nov 23, 2018
ffcf276
Update IWearWrapper rpcPortName
yeshasvitirupachuri Nov 23, 2018
f7035a6
Add ICub config file
yeshasvitirupachuri Dec 4, 2018
1c243c8
Update PR changes
yeshasvitirupachuri Jan 3, 2019
c58e258
Cleanup open configuration
yeshasvitirupachuri Jan 4, 2019
b051044
Update port connection
yeshasvitirupachuri Jan 4, 2019
b4d49b7
Update separator usage
yeshasvitirupachuri Jan 4, 2019
2a5e4fb
remove Yarp Init check in ICub
lrapetti Jan 15, 2019
d4153a2
Add ground reaction FT flag handling
yeshasvitirupachuri Jan 16, 2019
285456f
Merge pull request #1 from lrapetti/feature/ICub-device-impl
yeshasvitirupachuri Jan 21, 2019
1f510d4
Update PR changes
yeshasvitirupachuri Jan 21, 2019
4ad9c08
Beautify using wearables clang format file
yeshasvitirupachuri Jan 21, 2019
5b643b3
Fix CMakeLists with YARP_FORCE_DYNAMIC_PLUGINS
lrapetti Jan 22, 2019
c1da269
Implement skin sensor and add config file
yeshasvitirupachuri Jan 28, 2019
f3aedf1
rpc port can be enabled or disabled in IWearRemapper with wearableRPC…
lrapetti Jan 22, 2019
27b5364
Add PreciselyTimed interface to IWearRemapper
lrapetti Feb 1, 2019
0bf5f0b
Merge pull request #2 from lrapetti/rpc-port-optional
yeshasvitirupachuri Feb 1, 2019
eb3a0fe
Merge pull request #3 from Yeshasvitvs/feature/iAnalogToIWear_groundR…
yeshasvitirupachuri Feb 1, 2019
af8a567
Merge pull request #4 from Yeshasvitvs/feature/Skin-device
yeshasvitirupachuri Feb 1, 2019
20e6244
update config file names and add yarpmanager files
yeshasvitirupachuri Feb 3, 2019
3386f3c
Add use RPC parameter
lrapetti Feb 4, 2019
8aa47b8
fix backcompatibility with YARP master
lrapetti Feb 5, 2019
147fdf0
Merge pull request #1 from lrapetti/fix-compatibility-YARP-master
lrapetti Feb 5, 2019
4bc67c8
Merge pull request #5 from lrapetti/add-useRPC-parameter
yeshasvitirupachuri Feb 6, 2019
6d99590
Fix Xsens Calibrator device
lrapetti Feb 7, 2019
d5fc81d
Merge pull request #6 from lrapetti/fix-xsens-calibrator
yeshasvitirupachuri Feb 7, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 5 additions & 7 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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)
14 changes: 4 additions & 10 deletions XSensMVN/src/XSensMVNCalibrator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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
Expand Down Expand Up @@ -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();
Expand Down
32 changes: 32 additions & 0 deletions app/Wearables-yarpmanager.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
148 changes: 148 additions & 0 deletions app/xml/FTShoeLeftWearableDevice.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,148 @@
<?xml version="1.0" encoding="UTF-8" ?>
<robot name="ftshoesleft_ianalogsensor_to_iwear" build=0 portprefix="">

<!-- First level devices section - force-torque-sensors -->
<device type="canBusAnalogSensor" name="ftShoe_Left_Front">
<param name="canbusDevice"> "ecan" </param>
<param name="physDevice"> "ecan" </param>
<param name="canDeviceNum"> 0 </param>
<param name="canAddress"> 0x01 </param>
<param name="format"> 16 </param>
<param name="channels"> 6 </param>
<param name="period"> 5 </param>
<param name="useCalibration"> 1 </param>
<param name="diagnostic"> 1 </param>
</device>
<device type="canBusAnalogSensor" name="ftShoe_Left_Rear">
<param name="canbusDevice"> "ecan" </param>
<param name="physDevice"> "ecan" </param>
<param name="canDeviceNum"> 0 </param>
<param name="canAddress"> 0x02 </param>
<param name="format"> 16 </param>
<param name="channels"> 6 </param>
<param name="period"> 5 </param>
<param name="useCalibration"> 1 </param>
<param name="diagnostic"> 1 </param>
</device>

<!-- Second level devices section - ftshoe -->
<device name="ftShoe_l" type="ftshoe">
<param name="period"> 10 </param>
<param name="name"> /ft/ftShoe_Left/analog:o </param>
<!------------------Transformation from Front to Rear---------------------->
<!--TRANSLATION-->
<!--fts_offset: the origin of Front ft SoR expressed in Rear ft SoR [m]-->
<param name="fts_offset">(-0.181101, 0.0, 0.0)</param>
<!--ROTATION-->
<!--fts_orientation_R: rotation matrix to transform values (expressed w.r.t. the Front ft SoR) into Rear ft SoR, (i.e., Rear_R_Front)-->
<param name="fts_orientation_R">( -1.0, 0.0, 0.0,
0.0, -1.0, 0.0,
0.0, 0.0, 1.0
)
<!------------Transformation from Rear to final ftShoe Output-------------->
</param>
<!--ROTATION-->
<!--rear_fts_to_out_R: rotation matrix to transform values (expressed w.r.t. the Rear ft SoR) into the final output SoR-->
<!--The final output SoR is located at the origin of the Rear ft SoR (no translation) with the following orientation:-->
<!--Z pointing up, X pointing forward (along the line that connects the origins of Rear ft SoR and the Front ft SoR) and Y following right-hand convention-->
<param name="rear_fts_to_out_R">(
-1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0
)
</param>
<!--inSitu calibration matrices.-->
<param name="useInSituCalibration"> false </param>
<!--This matrices applies to the calibrated fts signals, than be sure to set to 1 the useCalibration parameter of both devices-->
<group name="inSituMatrices">
<param name="front_fts"> (
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
)
</param>
<param name="rear_fts"> (
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
)
</param>
</group>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="front_ft"> ftShoe_Left_Front </elem>
<elem name="rear_ft"> ftShoe_Left_Rear </elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

<!-- Analog wrappers sections, required to forward devices data on dedicated yarp ports -->
<!-- Analog wrappers sections - ftshoe devices wrappers -->
<device name="ftSensWrapper" type="analogServer">
<param name="period"> 10 </param>
<param name="name"> /ft/ftShoe_Left/analog:o </param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="FirstStrain"> ftShoe_l </elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

<!-- Analog wrappers sections - ftSensors devices wrappers -->
<device name="ftSensWrapper" type="analogServer">
<param name="period"> 10 </param>
<param name="name"> /ft/ftShoe_Left_Front/analog:o </param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="FirstStrain"> ftShoe_Left_Front </elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>
<device name="ftSensWrapper" type="analogServer">
<param name="period"> 10 </param>
<param name="name"> /ft/ftShoe_Left_Rear/analog:o </param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="FirstStrain"> ftShoe_Left_Rear </elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

<device type="ianalogsensor_to_iwear" name="FTShoeLeftIAnalogSensorToIWear">
<param name="sensorName">FTShoeLeftFTSensors</param>
<param name="wearableName">FTShoeLeft</param>
<param name="numberOfChannels">6</param>
<param name="channelOffset">0</param>
<param name="wearableSensorType">ForceTorque6DSensor</param>
<param name="getGroundReactionFT">true</param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="FTShoeLeftIAnalogSensorToIWearLabel">ftShoe_l</elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

<device type="iwear_wrapper" name="FTShoeLeftIWearWrapper">
<param name="period">0.01</param>
<param name="dataPortName">/FTShoeLeft/WearableData/data:o</param>
<param name="rpcPortName">/FTShoeLeft/WearableData/metadataRpc:o</param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="FTShoeLeftIWearWrapperLabel">FTShoeLeftIAnalogSensorToIWear</elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

</robot>
148 changes: 148 additions & 0 deletions app/xml/FTShoeRightWearableDevice.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,148 @@
<?xml version="1.0" encoding="UTF-8" ?>
<robot name="ftshoes_ianalogsensor_to_iwear" build=0 portprefix="">

<!-- First level devices section - force-torque-sensors -->
<device type="canBusAnalogSensor" name="ftShoe_Right_Front">
<param name="canbusDevice"> "ecan" </param>
<param name="physDevice"> "ecan" </param>
<param name="canDeviceNum"> 0 </param>
<param name="canAddress"> 0x03 </param>
<param name="format"> 16 </param>
<param name="channels"> 6 </param>
<param name="period"> 5 </param>
<param name="useCalibration"> 1 </param>
<param name="diagnostic"> 1 </param>
</device>
<device type="canBusAnalogSensor" name="ftShoe_Right_Rear">
<param name="canbusDevice"> "ecan" </param>
<param name="physDevice"> "ecan" </param>
<param name="canDeviceNum"> 0 </param>
<param name="canAddress"> 0x04 </param>
<param name="format"> 16 </param>
<param name="channels"> 6 </param>
<param name="period"> 5 </param>
<param name="useCalibration"> 1 </param>
<param name="diagnostic"> 1 </param>
</device>

<!-- Second level devices section - ftshoe -->
<device name="ftShoe_r" type="ftshoe">
<param name="period"> 10 </param>
<param name="name"> /ft/ftShoe_Right/analog:o </param>
<!------------------Transformation from Front to Rear---------------------->
<!--TRANSLATION-->
<!--fts_offset: the origin of Front ft SoR expressed in Rear ft SoR [m]-->
<param name="fts_offset">(-0.181101, 0.0, 0.0)</param>
<!--ROTATION-->
<!--fts_orientation_R: rotation matrix to transform values (expressed w.r.t. the Front ft SoR) into Rear ft SoR, (i.e., Rear_R_Front)-->
<param name="fts_orientation_R">( -1.0, 0.0, 0.0,
0.0, -1.0, 0.0,
0.0, 0.0, 1.0
)
<!------------Transformation from Rear to final ftShoe Output-------------->
</param>
<!--ROTATION-->
<!--rear_fts_to_out_R: rotation matrix to transform values (expressed w.r.t. the Rear ft SoR) into the final output SoR-->
<!--The final output SoR is located at the origin of the Rear ft SoR (no translation) with the following orientation:-->
<!--Z pointing up, X pointing forward (along the line that connects the origins of Rear ft SoR and the Front ft SoR) and Y following right-hand convention-->
<param name="rear_fts_to_out_R">(
-1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0
)
</param>
<!--inSitu calibration matrices.-->
<param name="useInSituCalibration"> false </param>
<!--This matrices applies to the calibrated fts signals, than be sure to set to 1 the useCalibration parameter of both devices-->
<group name="inSituMatrices">
<param name="front_fts"> (
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
)
</param>
<param name="rear_fts"> (
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
)
</param>
</group>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="front_ft"> ftShoe_Right_Front </elem>
<elem name="rear_ft"> ftShoe_Right_Rear </elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

<!-- Analog wrappers sections, required to forward devices data on dedicated yarp ports -->
<!-- Analog wrappers sections - ftshoe devices wrappers -->
<device name="ftSensWrapper" type="analogServer">
<param name="period"> 10 </param>
<param name="name"> /ft/ftShoe_Right/analog:o </param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="FirstStrain"> ftShoe_r </elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

<!-- Analog wrappers sections - ftSensors devices wrappers -->
<device name="ftSensWrapper" type="analogServer">
<param name="period"> 10 </param>
<param name="name"> /ft/ftShoe_Right_Front/analog:o </param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="FirstStrain"> ftShoe_Right_Front </elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>
<device name="ftSensWrapper" type="analogServer">
<param name="period"> 10 </param>
<param name="name"> /ft/ftShoe_Right_Rear/analog:o </param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="FirstStrain"> ftShoe_Right_Rear </elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

<device type="ianalogsensor_to_iwear" name="FTShoeRightIAnalogSensorToIWear">
<param name="sensorName">FTShoeRightFTSensors</param>
<param name="wearableName">FTShoeRight</param>
<param name="numberOfChannels">6</param>
<param name="channelOffset">0</param>
<param name="wearableSensorType">ForceTorque6DSensor</param>
<param name="getGroundReactionFT">true</param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="FTShoeRightIAnalogSensorToIWearLabel">ftShoe_r</elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

<device type="iwear_wrapper" name="FTShoeRightIWearWrapper">
<param name="period">0.01</param>
<param name="dataPortName">/FTShoeRight/WearableData/data:o</param>
<param name="rpcPortName">/FTShoeRight/WearableData/metadataRpc:o</param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="FTShoeRightIWearWrapperLabel">FTShoeRightIAnalogSensorToIWear</elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

</robot>
Loading