iCubGenova09: Add FT sensor to right upper leg

[mfussi66]

As per title, this PR adds the FT sensor on the right leg of iCub3 to reflect the addition on the real robot.

Its transformation relative to the root link is the following:

-0.866025  -0.5        0
-0.5        0.866025   0
 0          0         -1

Which corresponds to the transform in the CAD, and therefore it corresponds to the way it is mounted:

cc @S-Dafarra @pattacini

[Nicogene]

@mfussi66 thanks for the PR, the CI is failing because there is a mismatch between the expected number of FT and the actual number since you added two sensors :

icub-model-test : odd number of F/T sensor found in the model
Could you please fix the test accordingly?

[Nicogene]

Actually the problem is that the left leg sensor is missing:

icub-models-generator/tests/icub-model-test.cpp

Lines 375 to 379 in a57abd3

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

[mfussi66]

Actually the problem is that the left leg sensor is missing:

Yep @Nicogene , only the right one was mounted on the robot. I guess changing the test to check against zero should suffice.

[Nicogene]

Yep @Nicogene , only the right one was mounted on the robot. I guess changing the test to check against zero should suffice.

Yes, then if it is intentional it is fine to change the test

[mfussi66]

Additionally, I think I'll need to change the check on the correct orientation of the sensor:

icub-models-generator/tests/icub-model-test.cpp

Lines 440 to 465 in a57abd3

	bool checkFTSensorsAreCorrectlyOrientedV3(iDynTree::KinDynComputations & comp)
	{
	iDynTree::Rotation rootLink_R_sensorFrameLeftArmExpected =
	iDynTree::Rotation(-0.267012, -0.961047, 0.0713662,
	-0.960459, 0.271447, 0.0619303,
	-0.0788901, -0.0520081, -0.995526);
	iDynTree::Rotation rootLink_R_sensorFrameRightArmExpected =
	iDynTree::Rotation(-0.267012, 0.961047, 0.0713662,
	0.960459, 0.271447, -0.0619303,
	-0.0788901, 0.0520081, -0.995526);
	iDynTree::Rotation rootLink_R_sensorFrameExpectedFoot =
	iDynTree::Rotation(-0.5, 0.866025, 0,
	-0.866025, -0.5, 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_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;
	ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_front_ft_sensor") && ok;
	return ok;
	}

[GiulioRomualdi]

cc @isorrentino

[traversaro]

@mfussi66 are we waiting for something or can we merge?

[mfussi66]

@mfussi66 are we waiting for something or can we merge?

I was waiting for the approval from @Nicogene, on my side I think we can merge.

[Nicogene]

LGTM !

[pattacini]

Shall we merge?