Replace `embObjInertial` w/ `embObjIMU` – Stint 2 #595

Nicogene · 2023-11-22T14:01:33Z

This is the follow-up of #420

Since now inertials3 service and embObjIMU covers all the functionalities of inertials2 and embObjInertials respectively we can:

Remove all inertials2 references in icub-firmware
Remove all inertials2 references in icub-firmware-shared
Remove embObjInertial from icub-main
Test inertial3 service
Test FT and STRAIN services

PRs

cc @marcoaccame @pattacini

The text was updated successfully, but these errors were encountered:

Nicogene · 2023-11-27T13:38:39Z

I started from the easiest part, i.e. the icub-main part:

Nicogene · 2023-11-27T16:00:59Z

I moved to the most intricate part, in fact probably I will have to double check the huge amount of changes w/ @marcoaccame.

I managed to remove all the inertials2 structures and classes both from icub-firmware and icub-firmware-shared.

I tried to compile the firmware of ems, mc4plus and mc2plus and it works fine, the only part that I am not convinced about are the MEMS

(I updated the OP w/ all the opened PRs)

Nicogene · 2023-11-28T09:06:56Z

I tried to test all the above changes running yarprobotinterface on my setup w/ an ems, an rfe and mtb boards, the yarprobotinterface starts fine but either from the can boards and mems I get this data published:

((0.0 0.0 0.0) -9.2559631349317831e+61)

It means that something broke removing the inertials2 structures

cc @marcoaccame

Nicogene · 2023-11-28T11:59:54Z

After a f2f alignment w/ @marcoaccame, it came out that instead of simply removing the enums not used anymore, we need to remove them and then shift the others in order to maintain them ordered.

Moreover, I had to change the CanMapping accordingly, and after these two commits:

Now I can instantiate correctly the inertials3 service and read both from legacy and "new" imus.

(((-0.125 -0.1875 -0.0625) 1701172202.18483) ((-63.0 -3.0 6.0) 1701172202.17504)) (((0.040000000000000001 1.3300000000000001 9.3900000000000006) 1701172202.1848295) ((0.0 0.0 0.0) 1701172202.2099471)) (((2.3875000000000001e-05 2.1875e-05 1.5562499999999999e-05) 1701172202.1848297)) (((8.1875 0.0 -5.375) 1701172202.1848302)) () () () () () ()

In order to be sure that these changes does not break other services, I will try to run:

The THERMO service on mtb4
The FT service on strain2

I am still not 100% convinced about this implementation:

extern uint8_t eocanmap_posOfEPEN(eOprotEndpoint_t ep, eOprotEntity_t en)
{
    enum { // keep in magic numbers. it is a check if any of the eoprot_xxx value changes
        s0 = 4, // eoprot_endpoints_numberof
        s1 = 8  // eoprot_entities_maxnumberofsupported
    };
    
    static const uint8_t pos[s0][s1] = // pos[eoprot_endpoints_numberof][eoprot_entities_maxnumberofsupported] = 
    {
        {0xf, 0xf, 0xf, 0xf, 0xf, 0xf, 0xf, 0xf , 0xf},     // ep->management [none, ...]
        {0,     1, 0xf, 0xf, 0xf, 0xf, 0xf, 0xf , 0xf},     // ep->mc [joi, mot, none, ...]
        {2,     3,   4,   5,   6,   7,   8,   9 , 0xf},     // ep->as [str, mai, tem, ine3, psc, pos, ft, bs]
        {10,  0xf, 0xf, 0xf, 0xf, 0xf, 0xf, 0xf , 0xf}      // ep->sk [sk, none]
    }; EO_VERIFYsizeof(pos, sizeof(const uint8_t)*(eoprot_endpoints_numberof)*(eoprot_entities_maxnumberofsupported))

    // the order is joint-motor-strain-mais-temperature-inertial-inertial3-psc-pos-ft-bs-skin
    if((ep >= s0) || (en >= (s1)))
    {
        return 0x0f;
    }
    return pos[ep][en];    
}

if fact I am getting these warning from the compiler

compiling EoCANprotSKperiodic.c...
../../../../../embobj/plus/can/EOtheCANmapping.c(774): warning: excess elements in array initializer [-Wexcess-initializers]
        {0xf, 0xf, 0xf, 0xf, 0xf, 0xf, 0xf, 0xf , 0xf},     // ep->management [none, ...]
                                                  ^~~
../../../../../embobj/plus/can/EOtheCANmapping.c(775): warning: excess elements in array initializer [-Wexcess-initializers]
        {0,     1, 0xf, 0xf, 0xf, 0xf, 0xf, 0xf , 0xf},     // ep->mc [joi, mot, none, ...]
                                                  ^~~
../../../../../embobj/plus/can/EOtheCANmapping.c(776): warning: excess elements in array initializer [-Wexcess-initializers]
        {2,     3,   4,   5,   6,   7,   8,   9 , 0xf},     // ep->as [str, mai, tem, ine3, psc, pos, ft, bs]
                                                  ^~~
../../../../../embobj/plus/can/EOtheCANmapping.c(777): warning: excess elements in array initializer [-Wexcess-initializers]
        {10,  0xf, 0xf, 0xf, 0xf, 0xf, 0xf, 0xf , 0xf}      // ep->sk [sk, none]

Nicogene · 2023-11-28T13:24:59Z

I am still not 100% convinced about this implementation:

In fact since now the map is a 4x8 not a 4x9 I removed the last column and compiles without warnings and works fine:

robotology/icub-firmware@fb3134a

Nicogene · 2023-11-28T14:43:56Z

I tried to validate my changes w/ the following setup:

ems
strain2
rfe

I ran the inertials3 service for getting the imu data from these 3 boards and alternatively the strain(legacy) and ft service for getting the ft data.

Here the list of ports created:

registration name /icub/left_hand/FT/measures:o ip 10.0.1.104 port 10009 type tcp
registration name /icub/left_hand/FT/rpc:o ip 10.0.1.104 port 10010 type tcp
registration name /icub/left_hand/inertialFT/measures:o ip 10.0.1.104 port 10007 type tcp
registration name /icub/left_hand/inertialFT/rpc:o ip 10.0.1.104 port 10008 type tcp
registration name /icub/left_hand/inertialMTB/measures:o ip 10.0.1.104 port 10003 type tcp
registration name /icub/left_hand/inertialMTB/rpc:o ip 10.0.1.104 port 10004 type tcp

And here is what I am reading:

✅ `/icub/left_hand/FT` -> FT + temperature (service `strain`(legacy))

() () () () (((31.199999999999999) 1701181101.5865707)) (((-1.75634765625 -1.54022216796875 -5.8397216796875 -0.059844970703125 -0.015380859375 0.005218505859375) 1701181101.9764657)) () () () ()

❌ `/icub/left_hand/FT` -> FT + temperature (service `ft`)

[ERROR] ServiceParser::check_analog() has found wrong SERVICE.type =  16 it is  eomn_serv_AS_ft it must be   TODO : tostring() function

We are configuring eomn_serv_AS_ft that is 16 but this check is failing:

https://github.com/robotology/icub-main/blob/23f59fe284d3cbdf526cf2ca43d5dd6948c06547/src/libraries/icubmod/embObjLib/serviceParser.cpp#L591-L595

Because type is strangely eomn_serv_AS_strain

❌ `/icub/left_hand/inertialFT` -> imu of the `strain2` (with another inertial running)

(((0.0 0.0 0.0) -9.2559631349317831e+61)) (((0.0 0.0 0.0) -9.2559631349317831e+61)) (((0.0 0.0 0.0) -9.2559631349317831e+61)) (((0.0 0.0 0.0) -9.2559631349317831e+61)) () () () () () ()

❌ `/icub/left_hand/inertialMTB` -> imu of rfe + gyro of the ems. (with another inertial running)

(((0.0 0.0 0.0) -9.2559631349317831e+61) ((0.0 0.0 0.0) -9.2559631349317831e+61)) (((0.0 0.0 0.0) -9.2559631349317831e+61)) (((0.0 0.0 0.0) -9.2559631349317831e+61)) (((0.0 0.0 0.0) -9.2559631349317831e+61)) () () () () () ()

The strange thing is that both the inertials3 launched on strain2 and ems/rfe standalone works, but when it is launched with another inertials3 service prints all zeros.

So the open problems are:

Make run two inertials3 services togheter
make run the eomn_serv_AS_ft service.

I am using the xml files in this branch

https://github.com/Nicogene/robots-configuration/tree/testemsmtb/experimentalSetups/ems_mtb

cc @marcoaccame @pattacini

Nicogene · 2023-11-28T14:58:17Z

❌ /icub/left_hand/FT -> FT + temperature (service ft)

I managed to make ti work replacing embObjFTsensor w/ embObjMultipleFTsensors, in fact now I am getting data from the strain2:

() () () () (((31.800000000000001) 1701183284.6826351)) (((-2.41497802734375 0.176025390625 5.8397216796875 -0.0194091796875 0.028839111328125 0.007537841796875) 1972204539.0)) () () () ()

This means that embObjFTsensor device works only with eomn_serv_AS_strain service @marcoaccame ?

Nicogene · 2023-11-28T15:50:09Z

So the open problems are:

Make run two inertials3 services togheter

After a f2f chat w/ @marcoaccame it came out that both are not actually problems. For each eth board can run only one service per type, then for running the imu of the ft sensor I have to add colums to the same xml, not spawn another embObjIMU

This means that embObjFTsensor device works only with eomn_serv_AS_strain service @marcoaccame ?

Looking here:

https://github.com/robotology/icub-main/blob/23f59fe284d3cbdf526cf2ca43d5dd6948c06547/src/libraries/icubmod/embObjLib/serviceParser.cpp#L558-L563

It seems that embObjFTsensor supports only eomn_serv_AS_strain, on the other hand eomn_serv_AS_ft is used only by embObjMultipleFTsensors, see:

https://github.com/search?q=repo%3Arobotology%2Ficub-main%20eomn_serv_AS_ft%20&type=code

Tomorrow I will do a double check and then I would say we are good to go.

cc @pattacini

Nicogene · 2023-11-29T13:28:35Z

The last test passed without problems 🎉
I run the Inertials3 service asking all this IMU data:

((l_leg_ft_gyro l_leg_ft_gyro "") (rfeimu_gyro rfeimu_gyro "") (on_eb2_gyros on_eb2_gyros "")) ((l_leg_ft_acc l_leg_ft_acc "") (rfeimu_acc rfeimu_acc "")) ((l_leg_ft_mag l_leg_ft_mag "") (rfeimu_mag rfeimu_mag "")) ((l_leg_ft_eul l_leg_ft_eul "") (rfeimu_eul rfeimu_eul "")) () () () () () ()

It is coming from ems, strain2, rfe and I am gettig this data:

(((-0.125 0.0 0.0) 1701263754.7739019) ((0.25 0.125 -0.125) 1701263754.7890337) ((-37.0 21.0 -140.0) 1701263754.7790971)) (((0.51000000000000001 0.080000000000000002 -9.4399999999999995) 1701263754.7739012) ((-1.3300000000000001 -0.60999999999999999 -9.6899999999999995) 1701263754.7890329)) (((-2.4999999999999999e-07 -3.0750000000000002e-05 0.0001206875) 1701263754.7739017) ((-6.7999999999999999e-05 -4.0624999999999996e-06 7.875e-06) 1701263754.7890334)) (((178.9375 2.5625 -212.8125) 1701263754.7739024) ((-176.4375 7.875 -280.5625) 1701263754.7890339)) () () () () () ()

Then I tried both the eomn_serv_AS_strain and eomn_serv_AS_strain and they works just fine:

() () () () (((30.300000000000001) 1701263738.2537851)) (((0.109771728515625 -0.9681396484375 4.910675048828125 -0.02587890625 -0.036529541015625 0.04522705078125) 1701263738.59881)) () () () ()

I will proceed then opening the PR containing the new FW binaries
cc @marcoaccame

Nicogene · 2023-11-29T14:22:06Z

The three PRs on firmware can be merged, I will then make as ready for review the one in icub-main

cc @pattacini @marcoaccame

Nicogene · 2023-11-30T10:35:18Z

Inertials2 has been completely removed.

We can close this issue then

…ls2 service This PR fixes a regressio of robotology/robots-configuration#595

Nicogene self-assigned this Nov 22, 2023

Nicogene changed the title ~~Replace embObjInertial w/ embObjIMU – Stint 2~~ Replace embObjInertial w/ embObjIMU – Stint 2 Nov 22, 2023

Nicogene closed this as completed Nov 30, 2023

Nicogene added a commit to Nicogene/icub-main that referenced this issue Dec 12, 2023

embObjAnalogSensor: fix compilation after the removal of EoTheInertia…

53a85cc

…ls2 service This PR fixes a regressio of robotology/robots-configuration#595

Nicogene mentioned this issue Dec 12, 2023

embObjAnalogSensor: fix compilation after the removal of EoTheInertials2 service robotology/icub-main#918

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Replace `embObjInertial` w/ `embObjIMU` – Stint 2 #595

Replace `embObjInertial` w/ `embObjIMU` – Stint 2 #595

Nicogene commented Nov 22, 2023 •

edited

Loading

Nicogene commented Nov 27, 2023

Nicogene commented Nov 27, 2023

Nicogene commented Nov 28, 2023

Nicogene commented Nov 28, 2023

Nicogene commented Nov 28, 2023

Nicogene commented Nov 28, 2023 •

edited

Loading

Nicogene commented Nov 28, 2023

Nicogene commented Nov 28, 2023 •

edited

Loading

Nicogene commented Nov 29, 2023 •

edited

Loading

Nicogene commented Nov 29, 2023

Nicogene commented Nov 30, 2023

Replace embObjInertial w/ embObjIMU – Stint 2 #595

Replace embObjInertial w/ embObjIMU – Stint 2 #595

Comments

Nicogene commented Nov 22, 2023 • edited Loading

PRs

Nicogene commented Nov 27, 2023

Nicogene commented Nov 27, 2023

Nicogene commented Nov 28, 2023

Nicogene commented Nov 28, 2023

Nicogene commented Nov 28, 2023

Nicogene commented Nov 28, 2023 • edited Loading

✅ /icub/left_hand/FT -> FT + temperature (service strain(legacy))

❌ /icub/left_hand/FT -> FT + temperature (service ft)

❌ /icub/left_hand/inertialFT -> imu of the strain2 (with another inertial running)

❌ /icub/left_hand/inertialMTB -> imu of rfe + gyro of the ems. (with another inertial running)

Nicogene commented Nov 28, 2023

Nicogene commented Nov 28, 2023 • edited Loading

Nicogene commented Nov 29, 2023 • edited Loading

Nicogene commented Nov 29, 2023

Nicogene commented Nov 30, 2023

Replace `embObjInertial` w/ `embObjIMU` – Stint 2 #595

Replace `embObjInertial` w/ `embObjIMU` – Stint 2 #595

Nicogene commented Nov 22, 2023 •

edited

Loading

Nicogene commented Nov 28, 2023 •

edited

Loading

✅ `/icub/left_hand/FT` -> FT + temperature (service `strain`(legacy))

❌ `/icub/left_hand/FT` -> FT + temperature (service `ft`)

❌ `/icub/left_hand/inertialFT` -> imu of the `strain2` (with another inertial running)

❌ `/icub/left_hand/inertialMTB` -> imu of rfe + gyro of the ems. (with another inertial running)

Nicogene commented Nov 28, 2023 •

edited

Loading

Nicogene commented Nov 29, 2023 •

edited

Loading