fjandrad commented 4 years ago

This PR ports the functionality of considering temperature coefficients in the FT sensors and allows the user to enable if he wants to use an FT offset estimated offline.

It was orignally in codyco-modules and now ported to this repo.

fjandrad commented 4 years ago

The relevant changes in the configuration files are missing in this PR?

It is perfectly back-compatible so no change is required in the configuration file. What needs to be done is to add the configuration file with the required variables in robot configuration and then we may put a link in the PR. what do you think @prashanthr05

prashanthr05 commented 4 years ago

What needs to be done is to add the configuration file with the required variables in robot configuration and then we may put a link in the PR

Agreed.

But should we also add such a configuration file in the app folder of the wholebodydynamics device in this repository?

fjandrad commented 4 years ago

But should we also add such a configuration file in the app folder of the wholebodydynamics device in this repository?

I do not know what is the best policy here. As a possible user of these features I would say it is nice to have a default or dummy with already the variables added so that I can modify them later on. It could also make other users aware that those functionalities exist by looking at the parameters there. But it could also make noise since they are not strictly required parameters and a new user might think they are because they are there. @traversaro what do you think?

traversaro commented 4 years ago

What needs to be done is to add the configuration file with the required variables in robot configuration and then we may put a link in the PR

Agreed.

But should we also add such a configuration file in the app folder of the wholebodydynamics device in this repository?

I think we should start to deprecate the configuration files in the app folder, and just leave there the one for simulated robots. If we keep the configuration files in robots-configuration, then we should avoid that the same files are duplicated here. So I think we can avoid updating the files in app .

fjandrad commented 4 years ago

Testing this PR

The original testing idea was to compare if it was useful to add the temperature and in that case if we could use the offset estimated with it. The conclusion of such experiments was that yes we benefit from the temperature and more over it enables to use an offset estimated offline since main drift cause ( the temperature ) is taken care of by the temperature coefficients. Results were reported in Six-Axis Force Torque Sensor Model-Based In Situ Calibration Method and Its Impact in Floating-Based Robot Dynamic Performance.

So to test this PR what we would need to do is insert the temperature coefficients and offset candidate in the configuration file.

An example of combination of uses enabled by this PR are here temperature_conf_files.zip. They are prepared to launch an external whole-body-dynamics device for testing without risking the actual robot.

:warning: if we compare the offset recently estimated using calib all vs the pre-estimated offset we might see a small benefit from calib all in comparison to the offline offset. On the long run, the estimated offset will keep its good level of performance while the performance of the offset estimated with calib all can drop by a big amount. So the use of this feature has two main scenarios in which it has a good advantage:

When immediatly starting the robot, since it warms up fast the first time changing the offset 'fast'
When we schedule tests that will last a long time or plan to use the robot for extended periods of time ( more than 1 hour).

Once validated, we should replace the current wholebodydynamics.xml by one with the variables required for the temperature compensation with offline estimated offsets. The changes will be inserted in robots-configuration (actual file still pending)

Testing scenarios

The idea is that even when using the robot for a long period of time the offset does not need to be estimated again.

Reasonable values

launch yarprobotinterface with default setup
launch an external wholebodydynamics (wbd) device with the temperature coefficients and offset
for the externald wbd device use the rpc usePreEstimatedOffset function.
verify the values are reasonable in the external device by putting robot on the ground in two feet
verify the values are reasonable in single support ( there should be some default home positions in yarpmotorgui )

Performance verification

Start the robot, when the robot has not been used in a long time and motors were off. ( best scenario is early morning ).
Launch external wbd device with the temperature coefficients and offset
For the externald wbd device use the rpc usePreEstimatedOffset function.
Run a couple of experiments like Yoga or walking ( 2 yogas one for each foot is prefered ) while logging the experiment with yarp data dumper both the local and external wbd force information ( filtered ft port) and required information to estimate offline the expected wrenches ( joint positions, imu , ft sensor information ).
Wait with the robot on for 20 min then launch other set experiments.
Repeat previous step 4 or 5 times
Use iDyntree scripts to verify what was the expected value when standing on single support

It is expected to see that the one with temperature compensation has much better performance in th e later experiments than without compensation.

:exclamation: Is important to remember not to do calib all again at any time to have a clear difference in performance.

prashanthr05 commented 4 years ago

@fjandrad have you already pushed your changes to the remote? because I do not see them. Also, I wonder why the CI is not starting in this PR?

fjandrad commented 4 years ago

@prashanthr05 not yet got distracted with ISAAC stuff. No idea about the CI, its so clean that it believes it does not need to check :laughing: :rofl:

traversaro commented 4 years ago

Also, I wonder why the CI is not starting in this PR?

I guess because the PR is based on an old devel branch without CI. I suggest to rebase on the latest devel to get CI.

fjandrad commented 4 years ago

It's starting now. Is it possible the PR was created before we added CI functionalities to the repo? Note that I did not rebase for the CI to start.

fjandrad commented 4 years ago

It stopped the others because Windows failed. Is there a way to allow it to check it anyway?

prashanthr05 commented 4 years ago

It stopped the others because Windows failed. Is there a way to allow it to check it anyway?

According to this answer, this is due to a fail-fast behavior and by default it is set to true. I will open a PR.

traversaro commented 4 years ago

It stopped the others because Windows failed. Is there a way to allow it to check it anyway?

The Windows failure is something affecting several other repos, see https://github.com/dic-iit/bipedal-locomotion-controllers/issues/30 .

prashanthr05 commented 4 years ago

It stopped the others because Windows failed. Is there a way to allow it to check it anyway?

According to this answer, this is due to a fail-fast behavior and by default it is set to true. I will open a PR.

Relevant PR opened in https://github.com/robotology/whole-body-estimators/pull/52

fjandrad commented 4 years ago

For testing this PR, a branch has been opened https://github.com/dic-iit/robots-configuration/tree/testTemperatureOffset . At the moment, it contains only the main wholebodydyinamcs.xml with the variables that gave best results for the paper. No files for comparison where directly added, but they are anyway contained here.

fjandrad commented 4 years ago

I will leave this in ice-box until we can to the test in the lab with the real robot. Next iteration hopefully.

fjandrad commented 4 years ago

I tried testing the PR, sadly it failed:

I first tried the rebased version of the PR, but the yarp of the computer and robot is not updated. So when building and installing I faced:

To avoid updating everything since I had no time I tried with the non rebased version. I followed the steps in https://github.com/dic-iit/component_wholebody-teleoperation/issues/250#issue-578145346

Unfortunately is not working. I think it might be a problem on the names of the sensors configuration files, but unfortunately do not have time to verify it since I need to go.

The error that I could find in the logger is here:

A length problem just after reading the sensors. So after this line https://github.com/robotology/whole-body-estimators/blob/ed3bd4c1c06bd0afa885cc9b5c3bfbffd84c17d5/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp#L1241

I'm sorry I could not fix this before leaving. @prashanthr05 @traversaro If there is any doubt I can help with later let me know. I guess I wont be able to test this in the robot anymore.

prashanthr05 commented 4 years ago

I rebased the branch over the latest devel after fixing a couple of merge conflicts. We can schedule testing this PR in this iteration.

prashanthr05 commented 4 years ago

oops, instead of pressing comment, I pressed close and comment :D!

prashanthr05 commented 4 years ago

@HosameldinMohamed and I were testing this PR today. It was failing in the attachAllFTs() phase.

We identified the problems to be the following,

Here we parse through the ftDeviceNames which includes both MAS FTs and analog FT names (size up to 10 for iCubGenova04), however, the for loop iterates over ftList which is a vector of analog sensors only (6 in number). So there is a mismatch in checking the sensor names between the added FT sensors and those in the URDF model.
a missing return false here (however, this did not cause any problem, so far)

However, even fixing these did not allow us to successfully launch the wholebodydynamics device. This needs to be investigated further, possibly need to run a yarprobotinterface with valgrind to understand the root cause of failure, because this could not be interpreted from the logs. The yarprobotinterface just goes into a "freeze" state and then crashes after a while. Possibly maintaining single vector of strings ftDeviceNames to handle both MAS FTs and analog FTs is causing the problem and the consequential segmentation faults.

While, we were able to run YOGA successfully with the master branch code.

traversaro commented 4 years ago

Is the calibration working correctly or it is disabled?

prashanthr05 commented 4 years ago

Is the calibration working correctly or it is disabled?

The calibration was running. We were able to see almost zero end-effector wrenches at the feet even without running a calib all 300 through the rpc (not sure if we should be surprised about this, because we usually see a ~20 N offset on forces most times on the robot).

However, we still need to test the pre-estimated offset and temperature compensation. We couldn't fully understand the details in this comment. Maybe @fjandrad is available sometime, we could have a quick call to discuss the procedure?

fjandrad commented 4 years ago

We identified the problems to be the following,

  -   Here we parse through the ftDeviceNames which includes both MAS FTs and analog FT names (size up to 10 for iCubGenova04), however, the for loop iterates over ftList which is a vector of analog sensors only (6 in number). So there is a mismatch in checking the sensor names between the added FT sensors and those in the URDF model.
  -     a missing return false here (however, this did not cause any problem, so far)

This was on purpose since the MAS have both the interface for analog and MAS so when looking for analog sensors the MAS sensors will also appear in the analog list and are as of now the default mode for the FT sensors. The name in the overlapping analog sensors and MAS should be the same. But the name of the MAS is actually read from the sensors while for the analog they do not really matter. When testing my impression was that the name of the sensors in the hardware conf files might be not the ones expected of the urdf, but could not check further. I am available if you want to have a call.

prashanthr05 commented 4 years ago

This was on purpose since the MAS have both the interface for analog and MAS so when looking for analog sensors the MAS sensors will also appear in the analog list and are as of now the default mode for the FT sensors. The name in the overlapping analog sensors and MAS should be the same

Eventhough they might be from the same device, we populate the vector of names by checking the interfaces which are different (ISixAxisForceTorqueSensors and IAnalogSensors). So in that case, it is also ok to maintain two separate vectors, because the compatibility check with the sensors present in the URDF model is done using the list populated by analog sensor interfaces.

traversaro commented 4 years ago

This was on purpose since the MAS have both the interface for analog and MAS so when looking for analog sensors the MAS sensors will also appear in the analog list and are as of now the default mode for the FT sensors. The name in the overlapping analog sensors and MAS should be the same

Eventhough they might be from the same device, we populate the vector of names by checking the interfaces which are different (ISixAxisForceTorqueSensors and IAnalogSensors). So in that case, it is also ok to maintain two separate vectors, because the compatibility check with the sensors present in the URDF model is done using the list populated by analog sensor interfaces.

I think that is the case now, but if we could report this problem now will permit to fix them at the robot configuration level, so that in the future we will be able to migrate away from IAnalogSensors .

prashanthr05 commented 4 years ago

In that case, the MAS wrappers for the arm FT sensors are clearly missing here.

https://github.com/robotology/robots-configuration/tree/master/iCubGenova04/wrappers/FT

and maybe boils down to the fact that

the arm FT sensors are not strain2 sensors? https://github.com/robotology/robots-configuration/tree/master/iCubGenova04/hardware/FT

or

The sensor ids in seem to be id_l_upper_arm_strain instead of the expected l_arm_ft_sensor. https://github.com/robotology/robots-configuration/blob/c3c02dd7cc16644d9d950b63bd85e72d080876cf/iCubGenova04/hardware/FT/left_arm-eb1-j0_3-strain.xml#L33

traversaro commented 4 years ago

and maybe boils down to the fact that the arm FT sensors are not strain2 sensors?

I am afraid you are right, all strain1 sensors uses the embObjStrain devices that at the moment does not support MAS interfaces.

fjandrad commented 4 years ago

I am afraid you are right, all strain1 sensors uses the embObjStrain devices that at the moment does not support MAS interfaces.

Having only strain 2 sensors would have been much easier also in the code.

traversaro commented 4 years ago

I am afraid you are right, all strain1 sensors uses the embObjStrain devices that at the moment does not support MAS interfaces.

Having only strain 2 sensors would have been much easier also in the code.

Having only strain 2 sensors on all iCub in the world is a bit impossible for the few next years for logistical reasons, but making sure that all strain sensors also expose a MAS interface is much easier, I will open appropriate issues for that.

HosameldinMohamed commented 4 years ago

Is the calibration working correctly or it is disabled?

The calibration was running. We were able to see almost zero end-effector wrenches at the feet even without running a calib all 300 through the rpc (not sure if we should be surprised about this, because we usually see a ~20 N offset on forces most times on the robot).

@traversaro considering this line inside attachAll which is automatically called before calib all https://github.com/robotology/whole-body-estimators/blob/772be87fc71798eb5ca4f3eb15dd4df49b1acf82/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp#L1387

should we disable also that while testing the preEstimatedOffset?

traversaro commented 4 years ago

should we disable also that while testing the preEstimatedOffset?

Exactly, if we want to use the preEstimatedOffset we don't want to compute them online via the calibration. Furthermore, you must not call calib all if you want to test with preEstimatedOffset, otherwise the calib all will overwrite the offset that you specified with preEstimatedOffset.

HosameldinMohamed commented 4 years ago

@prashanthr05 and I tested the first part of the https://github.com/robotology/whole-body-estimators/pull/45#issuecomment-601255577 launching an external device using the wholebodydynamics-temp-offset.xml config file attached in the comment. In addition to the usual device launched in iCub-head.

The FT_OFFSET, FT_TEMPERATURE_COEFFICIENTS And FT_SECONDARY_CALIBRATION parameters are the same in the config file mentioned above.
The performed activities are:

Reasonable values

launch yarprobotinterface with default setup

launch an external wholebodydynamics (wbd) device with the temperature coefficients and offset

for the externald wbd device use the rpc usePreEstimatedOffset function.

verify the values are reasonable in the external device by putting robot on the ground in two feet

verify the values are reasonable in single support ( there should be some default home positions in yarpmotorgui )

The robot was ON for a long time before recording the data (>1 hour).
For standing on two feet, the dumped ports are:
Left leg FT sensor:

Left leg End effector wrench:

@prashanthr05 We expect that the difference between filteredFT data of the external WBD device (pre-estimated offset) and the raw FT data should be the configured value24.8009 , -6.2369 , -62.0044 , -0.0588 , -0.2425 , 0.1253?

And we expect the end effector wrench of the default device to be all of values zero?

Could it be because we kept the robot running for a long time before dumping the data. So the estimates of both devices drifted a bit?

fjandrad commented 4 years ago

We expect that the difference between filteredFT data of the external WBD device (pre-estimated offset) and the raw FT data should be the configured value24.8009 , -6.2369 , -62.0044 , -0.0588 , -0.2425 , 0.1253?

Assuming that by raw data you mean one of the analog or measures ports, the difference might not be exactly that because besides the offset there is the secondary calibration matrix. If you want to verify exactly only the offset, then you can put an identity matrix as secondary matrix and a pre-estimated offset or no secondary matrix at all. This would only verify that indeed the pre-estimated offset function is doing what is meant to be.

And we expect the end effector wrench of the default device to be all of values zero?

If you are checking at the left leg and not the foot then yes it should be zero since there is no force there. Verify that in the settings of the wbd file the frame is lower or upper left leg instead of one of the soles or ankle frames.

Could it be because we kept the robot running for a long time before dumping the data. So the estimates of both devices drifted a bit?

That is expected since one should be affected by temperature while the other not. The best comparison though is to get something when the robot is cold and recently started and then another one after some time to compare if one stayed around the same values ( the one with temperature compensation ) while the other drifted a bit. It might be worth doing this check when it is on one foot since all the weight will be only on one leg. It should help for repeatability. While in two feet the distribution of forces might be slightly different every time depending on the landing of the robot on the ground.

HosameldinMohamed commented 4 years ago

Assuming that by raw data you mean one of the analog or measures ports, the difference might not be exactly that because besides the offset there is the secondary calibration matrix. If you want to verify exactly only the offset, then you can put an identity matrix as secondary matrix and a pre-estimated offset or no secondary matrix at all. This would only verify that indeed the pre-estimated offset function is doing what is meant to be.

We commented the configuration groups FT_SECONDARY_CALIBRATION and FT_TEMPERATURE_COEFFICIENTS to use the default value of identity and 0 respectively. In this case we were able to observe the differences between the filteredFT port and the analog:o port when setting usePreEstimatedOffset, and they differences are equal to the values set in the config group FT_OFFSET.

Please note that the tests were done using the left-leg FT sensor only.

HosameldinMohamed commented 4 years ago

The best comparison though is to get something when the robot is cold and recently started and then another one after some time to compare if one stayed around the same values ( the one with temperature compensation ) while the other drifted a bit. It might be worth doing this check when it is on one foot since all the weight will be only on one leg.

With @prashanthr05 we recorded the data of the filteredFT/l_leg_ft_sensor port of the default wholeBodyDyanmics (without the temperature compensation - run in iCub head), and an external wholeBodyDynamics with temperature compensation run on an another machine. We recorded the data of both devices when the robot was 25°C and when temperature went up to 39°C. For the device with the temperature compensation we didn't run usePreEstimatedOffset.

Here are the data:

Force X component

Name	appx. value at 25°C [N]	appx. value at 39°C [N]	difference
Raw FT data	12.66	13.37	-0.71
WBD default	48.42	49.45	-1.03
WBD temperature compensation	24.32	23.85	0.47

Force Y component

Name	appx. value at 25°C [N]	appx. value at 39°C [N]	difference [N]
Raw FT data	26.18	22.53	3.65
WBD default	4.25	2.434	1.816
WBD temperature compensation	6.5	7.85	-1.35

Force Z component

Name	appx. value at 25°C [N]	appx. value at 39°C [N]	difference [N]
Raw FT data	-213.2	-232.5	19.3
WBD default	-285	-303.4	18.4
WBD temperature compensation	-210.8	-212.4	1.6

Torque X component

Name	appx. value at 25°C [N]	appx. value at 39°C [N]	difference [N]
Raw FT data	-0.234	-0.1236	-0.1104
WBD default	-1.569	-1.473	-0.096
WBD temperature compensation	-1.432	-1.534	0.102

Torque Y component

Name	appx. value at 25°C [N]	appx. value at 39°C [N]	difference [N]
Raw FT data	5	4.4	0.6
WBD default	4.34	3.87	0.47
WBD temperature compensation	5.09	4.67	0.42

Torque Z component

Name	appx. value at 25°C [N]	appx. value at 39°C [N]	difference [N]
Raw FT data	-0.137	-0.0128	-0.1242
WBD default	0.0953	0.122	-0.0267
WBD temperature compensation	-0.1447	-0.0414	-0.1033

fjandrad commented 4 years ago

@HosameldinMohamed @prashanthr05 nice work. The code is working as intended. It confirms the behaviour I saw in the past. The z component was always the most affected. I'm glad to see that the secondary calibration matrices also diminish oscillation in the torque values.

traversaro commented 4 years ago

Had the impression the auto generated files where no longer commited. Am I wrong? @traversaro

It depends on which CMake function are used and with which argument.

prashanthr05 commented 4 years ago

Had the impression the auto generated files where no longer commited. Am I wrong? @traversaro

It depends on which CMake function are used and with which argument.

@traversaro could you add more details to this? Maybe this could be helpful for all of us in the future dealing with IDL autogenerated files.

Also, @fjandrad @traversaro if you give a thumbs up on this PR, we could maybe finally merge this.

It must be noted that,

the experiments were done only for the left leg FT sensors using the configuration linked in this comment https://github.com/robotology/whole-body-estimators/pull/45#issuecomment-601255577. However, the functionality of the code has been tested.

traversaro commented 4 years ago

I will clarify the IDL thing when I will get in front of a real keyboard. In the meanwhile I think it is ok to merge this PR.

fjandrad commented 4 years ago

My only concern was the IDL files, so if that is not an issue it is fine for me as well.

prashanthr05 commented 4 years ago

I rebased over the latest devel and updated changelog.

We could merge this PR once the CI passes.

prashanthr05 commented 4 years ago

@fjandrad @HosameldinMohamed @traversaro Merged into devel, thanks!!

robotology / whole-body-estimators

Add support for compensating temperature in Force/Torque sensors readings and for specify user offline offset #45

Testing this PR

Testing scenarios

Reasonable values

Performance verification

Reasonable values

Force X component

Force Y component

Force Z component

Torque X component

Torque Y component

Torque Z component