Closed fjandrad closed 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
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?
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?
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 thewholebodydynamics
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
.
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:
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)
The idea is that even when using the robot for a long period of time the offset does not need to be estimated again.
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.
@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?
@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:
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.
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.
It stopped the others because Windows failed. Is there a way to allow it to check it anyway?
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.
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 .
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
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.
I will leave this in ice-box until we can to the test in the lab with the real robot. Next iteration hopefully.
I tried testing the PR, sadly it failed:
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.
I rebased the branch over the latest devel after fixing a couple of merge conflicts. We can schedule testing this PR in this iteration.
oops, instead of pressing comment, I pressed close and comment :D!
@HosameldinMohamed and I were testing this PR today. It was failing in the attachAllFTs()
phase.
We identified the problems to be the following,
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.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.
Is the calibration working correctly or it is disabled?
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?
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.
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.
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
.
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
or
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#L33and 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.
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.
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.
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
?
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
.
@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:
@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?
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.
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.
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:
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 |
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 |
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 |
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 |
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 |
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 |
@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.
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.
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,
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.
My only concern was the IDL files, so if that is not an issue it is fine for me as well.
I rebased over the latest devel and updated changelog.
We could merge this PR once the CI passes.
@fjandrad @HosameldinMohamed @traversaro Merged into devel, thanks!!
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.