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

iCubGenova09: Add FT sensor to right upper leg #218

Merged
merged 5 commits into from
Feb 17, 2022
Merged

iCubGenova09: Add FT sensor to right upper leg #218

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
b6aa6c5
Add FT sensor to right upper leg of iCubGenova09
mfussi66 Feb 11, 2022
e25ddf9
add right leg ft on yaml.in file
mfussi66 Feb 11, 2022
23c57c0
Changed test accounting for an odd number of FTs
mfussi66 Feb 11, 2022
2bdc9ce
Compare to even number of n Ft sensors if icub2 as before
mfussi66 Feb 11, 2022
0e98268
Update test with rotation matrix of FT sensor on right upper leg
mfussi66 Feb 14, 2022
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
20 changes: 9 additions & 11 deletions simmechanics/data/icub3/ICUB_3_all_options.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -267,15 +267,15 @@ forceTorqueSensors:
<yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_foot_rear_ft.ini</yarpConfigurationFile>
</plugin>
# right leg
#- jointName: r_leg_ft_sensor
# directionChildToParent: Yes
# frame: sensor
# frameName: SCSYS_R_HIP_2_FT
# sensorBlobs:
# - |
# <plugin name="right_leg_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
# <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_leg_ft.ini</yarpConfigurationFile>
# </plugin>
- jointName: r_leg_ft_sensor
directionChildToParent: Yes
frame: sensor
frameName: SCSYS_R_HIP_2_FT
sensorBlobs:
- |
<plugin name="right_leg_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_leg_ft.ini</yarpConfigurationFile>
</plugin>
- jointName: r_foot_front_ft_sensor
directionChildToParent: No
frame: sensor
Expand Down Expand Up @@ -831,5 +831,3 @@ XMLBlobs:
<gazebo reference="r_wrist_yaw">
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>


21 changes: 9 additions & 12 deletions simmechanics/data/icub3/ICUB_3_all_options_gazebo.yaml.in
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -264,15 +264,15 @@ forceTorqueSensors:
<yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_foot_rear_ft.ini</yarpConfigurationFile>
</plugin>
# right leg
#- jointName: r_leg_ft_sensor
# directionChildToParent: Yes
# frame: sensor
# frameName: SCSYS_R_HIP_2_FT
# sensorBlobs:
# - |
# <plugin name="right_leg_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
# <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_leg_ft.ini</yarpConfigurationFile>
# </plugin>
- jointName: r_leg_ft_sensor
directionChildToParent: Yes
frame: sensor
frameName: SCSYS_R_HIP_2_FT
sensorBlobs:
- |
<plugin name="right_leg_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_leg_ft.ini</yarpConfigurationFile>
</plugin>
- jointName: r_foot_front_ft_sensor
directionChildToParent: No
frame: sensor
Expand Down Expand Up @@ -919,6 +919,3 @@ XMLBlobs:
<gazebo reference="r_wrist_yaw">
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>



47 changes: 41 additions & 6 deletions tests/icub-model-test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -359,6 +359,29 @@ bool checkAxisDirectionsV3(iDynTree::KinDynComputations & comp)
return true;
}

/**
* All the iCub have a odd and not null number of F/T sensors.
*/
bool checkFTSensorsAreOddAndNotNull(iDynTree::ModelLoader & mdlLoader)
{
int nrOfFTSensors = mdlLoader.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE);

if( nrOfFTSensors == 0 )
{
std::cerr << "icub-model-test error: no F/T sensor found in the model" << std::endl;
return false;
}

if( nrOfFTSensors % 2 == 0 )
{
std::cerr << "icub-model-test : even number of F/T sensor found in the model" << std::endl;
return false;
}


return true;
}

/**
* All the iCub have a even and not null number of F/T sensors.
*/
Expand All @@ -382,6 +405,7 @@ bool checkFTSensorsAreEvenAndNotNull(iDynTree::ModelLoader & mdlLoader)
return true;
}


bool checkFTSensorIsCorrectlyOriented(iDynTree::KinDynComputations & comp,
const iDynTree::Rotation& expected,
const std::string& sensorName)
Expand Down Expand Up @@ -454,9 +478,14 @@ bool checkFTSensorsAreCorrectlyOrientedV3(iDynTree::KinDynComputations & comp)
-0.866025, -0.5, 0,
0, 0, 1);

iDynTree::Rotation rootLink_R_sensorFrameExpectedLeg =
iDynTree::Rotation(-0.866025, -0.5, 0,
-0.5, 0.866025, 0,
0, 0, -1);

bool ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameLeftArmExpected, "l_arm_ft_sensor");
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameRightArmExpected, "r_arm_ft_sensor") && ok;
// l_leg_ft_sensor and r_leg_ft_sensor frames seems to be wrong(see https://github.com/robotology/icub-models/issues/71)
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedLeg, "r_leg_ft_sensor") && ok;
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_rear_ft_sensor") && ok;
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_rear_ft_sensor") && ok;
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_front_ft_sensor") && ok;
Expand Down Expand Up @@ -535,21 +564,27 @@ int main(int argc, char ** argv)
}

// Now some test that test the sensors
if( !checkFTSensorsAreEvenAndNotNull(mdlLoader) )
{
return EXIT_FAILURE;
}

// The ft sensors orientation respect to the root_link are different to iCubV2 and they are under investigation.
if (modelPath.find("Genova09") != std::string::npos ||
modelPath.find("GazeboV3") != std::string::npos) {

if( !checkFTSensorsAreOddAndNotNull(mdlLoader) )
{
return EXIT_FAILURE;
}

if (!checkFTSensorsAreCorrectlyOrientedV3(comp))
{
return EXIT_FAILURE;
}
}
else
{
if( !checkFTSensorsAreEvenAndNotNull(mdlLoader) )
{
return EXIT_FAILURE;
}

if (!checkFTSensorsAreCorrectlyOrientedV2(comp))
{
return EXIT_FAILURE;
Expand Down