Error while testing attaching MAS FTs to wholeBodyDynamics

[HosameldinMohamed]

Here's the config file I used

config file

<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<robot name="@WBD_YARP_ROBOT_NAME">
    <!-- controlboards -->
    <device name="torso_mc" type="remote_controlboard">
        <param name="remote"> /icub/torso </param>
        <param name="local"> /wholeBodyDynamics-thrEst/torso </param>
    </device>

    <device name="left_arm_mc" type="remote_controlboard">
        <param name="remote"> /icub/left_arm </param>
        <param name="local"> /wholeBodyDynamics-thrEst/left_arm </param>
    </device>

    <device name="right_arm_mc" type="remote_controlboard">
        <param name="remote"> /icub/right_arm </param>
        <param name="local"> /wholeBodyDynamics-thrEst/right_arm </param>
    </device>

    <device name="left_leg_mc" type="remote_controlboard">
        <param name="remote"> /icub/left_leg </param>
        <param name="local"> /wholeBodyDynamics-thrEst/left_leg </param>
    </device>

    <device name="right_leg_mc" type="remote_controlboard">
        <param name="remote"> /icub/right_leg </param>
        <param name="local"> /wholeBodyDynamics-thrEst/right_leg </param>
    </device>

    <device name="head_mc" type="remote_controlboard">
        <param name="remote"> /icub/head </param>
        <param name="local"> /wholeBodyDynamics-thrEst/head </param>
    </device>


    <!-- imu -->
    <device name="inertial" type="genericSensorClient">
        <param name="remote"> /icub/inertial </param>
        <param name="local"> /wholeBodyDynamics-thrEst/imu </param>
    </device>

    <!-- FTs -->
     <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left-arm-ft-client" type="multipleanalogsensorsclient">
        <param name="remote">/icub/left_arm</param>
        <param name="local">/wholeBodyDynamics-thrEst/l_arm_ft_sensor</param>
    </device>


    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right-arm-ft-client" type="multipleanalogsensorsclient">
        <param name="remote">/icub/right_arm</param>
        <param name="local">/wholeBodyDynamics-thrEst/r_arm_ft_sensor</param>
    </device>

    <device name="left_upper_leg_strain" type="analogsensorclient">
        <param name="remote"> /icub/left_leg/analog:o </param>
        <param name="local"> /wholeBodyDynamics-thrEst/l_leg_ft_sensor </param>
    </device>

    <device name="right_upper_leg_strain" type="analogsensorclient">
        <param name="remote"> /icub/right_leg/analog:o </param>
        <param name="local"> /wholeBodyDynamics-thrEst/r_leg_ft_sensor </param>
    </device>

</robot>

<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<devices robot="@WBD_YARP_ROBOT_NAME@" build="1">

    <!--device  xmlns:xi="http://www.w3.org/2001/XInclude" name="ft-imu-mas-remapper" type="multipleanalogsensorsremapper">
            <param name="period">10</param>
                <param name="SixAxisForceTorqueSensorsNames">
                (l_arm_ft_sensor)
                </param>
                <param name="TemperatureSensorsNames">
                (l_arm_ft_sensor)
                </param>
            <action phase="startup" level="5" type="attach">
                    <paramlist name="networks">
                        <elem name="left-arm-ft-client">left-arm-ft-client</elem>
                    </paramlist>
                </action>

                <action phase="shutdown" level="5" type="detach" />
        </device-->

    <device name="wholeBodyDynamics" type="wholebodydynamics">
        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
        <param name="modelFile">model_no-feet-ft.urdf</param>
        <param name="fixedFrameGravity">(0,0,-9.81)</param>
        <param name="defaultContactFrames">(root_link,l_sole,r_sole,l_upper_leg,r_upper_leg,l_elbow_1,r_elbow_1)</param>
        <param name="overrideContactFrames">(chest_l_jet_turbine, chest_r_jet_turbine,l_sole,r_sole,l_upper_leg,r_upper_leg,r_arm_jet_turbine,l_arm_jet_turbine)</param>
        <param name="contactWrenchType">(       pure, pureKnown           ,full     ,full     ,full           ,full                ,full              ,full)</param>
        <param name="contactWrenchDirection">(0,0,1  ,0,0,1    ,0,0,1  ,0,0,1  ,0,0,1              ,0,0,1             ,0,0,1           ,0,0,1)</param>
        <param name="contactWrenchPosition">(  0,0,0  ,0,0,0     ,0,0,0 ,0,0,0 ,0,0,0             ,0,0,0             ,0,0,0           ,0,0,0)</param>
        <param name="imuFrameName">imu_frame</param>
        <param name="useJointVelocity">true</param>
        <param name="useJointAcceleration">false</param>
        <param name="streamFilteredFT">true</param>
        <param name="imuFilterCutoffInHz">3.0</param>
        <param name="forceTorqueFilterCutoffInHz">3.0</param>
        <param name="jointVelFilterCutoffInHz">3.0</param> <!-- used if useJointVelocity is set to true -->
        <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->
        <param name="additionalConsideredFixedJoints">(l_foot_ft_sensor,r_foot_ft_sensor)</param> <!-- Feet FT sensors are removed from the URDF model so these fixed joints should be added explicitly-->
        <param name="portPrefix">/wholeBodyDynamicsThrustEst</param> <!-- Not to interfere with the default one -->
        <param name="useSkinForContacts">false</param>


        <!-- map between iDynTree links (identified by a name)
             and skinDynLib links (identified by their frame name, a BodyPart enum
             and a local (to the body part) index -->
        <group name="IDYNTREE_SKINDYNLIB_LINKS">
            <param name="root_link">(root_link,1,0)</param>
            <param name="chest"> (chest,1,2)</param>
            <param name="l_upper_arm">(l_upper_arm,3,2)</param>
            <param name="l_elbow_1">(l_elbow_1, 3, 4)</param>
            <param name="r_upper_arm">(r_upper_arm,4,2)</param>
            <param name="r_elbow_1">(r_elbow_1, 4, 4)</param>
            <param name="l_upper_leg">(l_upper_leg,5,2)</param>
            <param name="l_lower_leg">(l_lower_leg,5,3)</param>
            <param name="l_ankle_1">(l_ankle_1,5,4)</param>
            <!--param name="l_foot">(l_foot_dh_frame,5,5)</param-->
            <param name="r_upper_leg">(r_upper_leg,6,2)</param>
            <param name="r_lower_leg">(r_lower_leg,6,3)</param>
            <param name="r_ankle_1">(r_ankle_1,6,4)</param>
            <!--param name="r_foot">(r_foot_dh_frame,6,5)</param-->
        </group>

        <group name="GRAVITY_COMPENSATION">
            <param name="enableGravityCompensation">true</param>
            <param name="gravityCompensationBaseLink">root_link</param>
            <param name="gravityCompensationAxesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow)</param>
        </group>

        <group name="WBD_OUTPUT_EXTERNAL_WRENCH_PORTS">
            <!--param name="/wholeBodyDynamics-thrEst/left_leg/cartesianEndEffectorWrench:o">(l_foot,l_sole,root_link)</param-->
            <!--param name="/wholeBodyDynamics-thrEst/right_leg/cartesianEndEffectorWrench:o">(r_foot,r_sole,root_link)</param-->
            <param name="/wholeBodyDynamics-thrEst/left_foot/cartesianEndEffectorWrench:o">(l_ankle_2,l_sole,l_sole)</param>
            <param name="/wholeBodyDynamics-thrEst/right_foot/cartesianEndEffectorWrench:o">(r_ankle_2,r_sole,r_sole)</param>
            <param name="/wholeBodyDynamics-thrEst/torso/cartesianEndEffectorWrench:o">(chest,chest)</param>
            <param name="/wholeBodyDynamics-thrEst/left_arm/cartesianEndEffectorWrench:o">(l_elbow_1,l_elbow_1)</param>
            <param name="/wholeBodyDynamics-thrEst/right_arm/cartesianEndEffectorWrench:o">(r_elbow_1,r_elbow_1)</param>
        </group>

        <!-- Parameter to determine the type of information publised in the group "WBD_OUTPUT_EXTERNAL_WRENCH_PORTS" -->
        <!-- "netWrench" or contactWrenches -->
        <param name="outputWrenchPortInfoType">   contactWrenches   </param>

        <action phase="startup" level="15" type="attach">
            <paramlist name="networks">
                <!-- motorcontrol -->
                <elem name="left_leg">left_leg_mc</elem>
                <elem name="right_leg">right_leg_mc</elem>
                <elem name="torso">torso_mc</elem>
                <elem name="right_arm">right_arm_mc</elem>
                <elem name="left_arm">left_arm_mc</elem>
                <elem name="head">head_mc</elem>
                <!-- imu -->
                <elem name="imu">inertial</elem>
                <!-- ft -->
                <!--elem name="ft-imu-mas-remapper">ft-imu-mas-remapper</elem-->
                <elem name="left-arm-ft-client">left-arm-ft-client</elem>
                <!--elem name="l_arm_ft_sensor">left_upper_arm_strain</elem-->
                <elem name="right-arm-ft-client">right-arm-ft-client</elem>
                <!--elem name="r_arm_ft_sensor">right_upper_arm_strain</elem-->
                <elem name="l_leg_ft_sensor">left_upper_leg_strain</elem>
                <elem name="r_leg_ft_sensor">right_upper_leg_strain</elem>
                <!--elem name="l_foot_ft_sensor">left_lower_leg_strain</elem-->
                <!--elem name="r_foot_ft_sensor">right_lower_leg_strain</elem-->
            </paramlist>
        </action>

        <action phase="shutdown" level="2" type="detach" />

    </device>
</devices>

Basically, I replaced 2 analogServer FTs with multipleanalogservers.

I am getting the following error

[ERROR] WholeBodyDynamicsDevice::attachAll Invalid number of MAS FT sensors after remapper

It happens due to this line:

whole-body-estimators/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp

Lines 1743 to 1747 in c46c185

	if (nrMASFTSensors != remappedMASInterfaces.ftMultiSensors->getNrOfSixAxisForceTorqueSensors())
	{
	yError() << "WholeBodyDynamicsDevice::attachAll Invalid number of MAS FT sensors after remapper";
	return false;
	}

Which is strange because the MAS sensors are detected based on

[DEBUG] wholeBodyDynamicsDevice :: number of ft sensors found in both ft + mas 4 where analog are  2  and mas are  2

From line

whole-body-estimators/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp

Line 1720 in c46c185

yDebug()<<"wholeBodyDynamicsDevice :: number of ft sensors found in both ft + mas"<<ftDeviceNames.size()<< "where analog are "<<ftList.size()<<" and mas are "<<ftSensorList.size();

Which means ftSensorList.size() is actually not empty.

What could I be possibly missing?

[HosameldinMohamed]

CC @traversaro

[traversaro]

If I recall correctly the current logic is that you should either have all sensors exposed via IAnalogSensors or all via ISixAxisForceTorqueSensors, you can't mix some sensors exposed via IAnalogSensors and some others via ISixAxisForceTorqueSensors.

[HosameldinMohamed]

If I recall correctly the current logic is that you should either have all sensors exposed via IAnalogSensors or all via ISixAxisForceTorqueSensors, you can't mix some sensors exposed via IAnalogSensors and some others via ISixAxisForceTorqueSensors.

I'm not sure this is the cause of this particular error, anyhow I tried with all FTs exposed via ISixAxisForceTorqueSensors, but still the same error occurs.

The error happens because attach methods in

whole-body-estimators/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp

Lines 1734 to 1735 in c46c185

	bool ok = remappedMASInterfaces.multwrap->attachAll(ftSensorList);
	ok = ok & remappedMASInterfaces.multwrap->attachAll(tempSensorList);

are executed with no errors but still return 0 devices because remappedMASInterfaces.ftMultiSensors->getNrOfSixAxisForceTorqueSensors() returns 0.

I found that I am missing the group multipleAnalogSensorsNames in the config files, which I guess should list the names of the MAS FT sensors, see this logic

whole-body-estimators/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp

Lines 772 to 795 in c46c185

	yarp::os::Bottle & propMasNames=prop.findGroup("multipleAnalogSensorsNames");
	propMASRemapper.put("device","multipleanalogsensorsremapper");
	bool ok=false;
	for (auto types=1u;types<propMasNames.size();types++){
	yarp::os::Bottle * mas_type = propMasNames.get(types).asList();
	if( mas_type->size() != 2 || mas_type->get(1).asList() == nullptr )
	{
	yError() << "wholeBodyDynamics: multipleAnalogSensorsNames group is malformed (" << mas_type->toString() << "). ";
	return false;
	}
	else {
	ok=true;
	}
	propMASRemapper.addGroup(mas_type->get(0).asString());
	yarp::os::Bottle MASnames;
	yarp::os::Bottle & MASnamesList= MASnames.addList();
	for(auto i=0u; i < mas_type->get(1).asList()->size(); i++)
	{
	MASnamesList.addString(mas_type->get(1).asList()->get(i).asString());
	}
	propMASRemapper.put(mas_type->get(0).asString(),MASnames.get(0));
	}

But I still can't get it right because I don't know how to properly add this group.
CC @CarlottaSartore @GiulioRomualdi

[HosameldinMohamed]

But I still can't get it right because I don't know how to properly add this group.

Adding this configuration worked (we are using 4 FTs in iRonCub):

<group name="multipleAnalogSensorsNames">
            <param name="SixAxisForceTorqueSensorsNames">("l_arm_ft_sensor", "r_arm_ft_sensor", "l_leg_ft_sensor", "r_leg_ft_sensor")</param>
            <param name="TemperatureSensorsNames">("l_arm_ft_sensor", "r_arm_ft_sensor", "l_leg_ft_sensor", "r_leg_ft_sensor")</param>
</group>

Probably we need to document this somewhere..

I was also able to launch the device successfully with mixing attaching multipleanalogservers and analogservers.

CC @traversaro

[HosameldinMohamed]

I think we can close this issue!