Skip to content

Commit

Permalink
Merge pull request #22 from robotology-playground/feature/ftshoe-IAna…
Browse files Browse the repository at this point in the history 
…logToIWear

 Add IAnalogToIWear device config files for FTShoes
  • Loading branch information
@diegoferigo
diegoferigo authored Feb 8, 2019
2 parents 43aaed7 + d28f564 commit 7e5028b
Show file tree
Hide file tree
Showing 3 changed files with 318 additions and 1 deletion.
148 changes: 148 additions & 0 deletions conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml
View file
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 conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml
View file
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>
23 changes: 22 additions & 1 deletion devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,6 +130,7 @@ class ForceTorque6DSensor : public IForceTorque6DSensor
{
public:
unsigned offset = 0;
bool groundReactionFT;

ForceTorque6DSensor(SensorName name, SensorStatus status = SensorStatus::Unknown)
: IForceTorque6DSensor(name, status)
Expand All @@ -149,7 +150,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;
}
};

Expand Down Expand Up @@ -281,6 +292,8 @@ struct ParsedOptions

size_t numberOfChannels;
size_t channelOffset;

bool getGroundReactionFT;
};

class IAnalogSensorToIWear::Impl
Expand Down Expand Up @@ -337,6 +350,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
// ===============
Expand All @@ -346,6 +364,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 << "*** ====================";
Expand All @@ -354,6 +373,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 << "*** ====================";

// =====================
Expand Down Expand Up @@ -393,6 +413,7 @@ bool IAnalogSensorToIWear::Impl::allocateSensor(const wearable::sensor::SensorTy
case wearable::sensor::SensorType::ForceTorque6DSensor: {
auto sensor = std::make_shared<ForceTorque6DSensor>(name, SensorStatus::Ok);
sensor->offset = options.channelOffset;
sensor->groundReactionFT = options.getGroundReactionFT;
iSensor = std::dynamic_pointer_cast<ISensor>(sensor);
break;
}
Expand Down

0 comments on commit 7e5028b

Please sign in to comment.