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

Add IAnalogToIWear device config files for FTShoes #22

Merged
merged 4 commits into from
Feb 8, 2019
Merged

Add IAnalogToIWear device config files for FTShoes #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
238c469
Add IAnalogToIWear device config files for FTShoes
yeshasvitirupachuri Nov 23, 2018
de1a5d0
Update IWearWrapper port names
yeshasvitirupachuri Nov 23, 2018
076c5a9
Update IWearWrapper rpcPortName
yeshasvitirupachuri Nov 23, 2018
d28f564
Add ground reaction FT flag handling
yeshasvitirupachuri Jan 16, 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
148 changes: 148 additions & 0 deletions conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.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 conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.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>
23 changes: 22 additions & 1 deletion devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp
View file
Open in desktop
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