Friction model coulomb + viscous cannot be used without rebuilding the firmware

[isorrentino]

Hi all, I started to use the coulomb + viscous model for the friction compensation on ergoCub. In order to use it I had to rebuild the firmware and flash the boards of the robot. It would be nice to have the possibility to choose it without rebuilding the firmware.

cc @DanielePucci @sgiraz @marcoaccame @pattacini

[pattacini]

@sgiraz, would you mind taking care of this?

[sgiraz]

Hi guys,

Ok, I can start working on it in the next sprint (Monday 24th)

[pattacini]

A quick update on this activity.

As we aim to streamline the code and get only one FW capable of dealing with the entire pool of robots, we foresee to:

• Remove the define USE_VISCOUS_COULOMB. As a result, the current law will be replaced by the latest policy outright. We verified that the current law can be subsumed by the new law when default params are employed.
• Make threshold configurable from XML files. While doing so, we check if we can stick to icubdeg/s or deg/s units.
• Update ergoCub XML files to set threshold equal to the value that is currently hand-coded in the FW.
• Update the robots that make use of Kbemf (there are a few) so that they'll rely on the new friction parameters.

This activity will commence this week and will be most likely completed the next sprint.

[sgiraz]

Hi guys,

I have some updates about this issue.

Configuration file changes

Here is a snippet example of the configuration file I used to test the new threshold parameter on a single joint setup (aea2-setup) + mc4plus. Note that I removed the kbemf parameter

    <!-- custom PIDs: begin -->
     <group name="TRQ_PID_OUTPUT_CURR">
        <param name="controlLaw">          torque           </param>
        <param name="outputType">          current          </param>
        <param name="fbkControlUnits">     metric_units     </param>
        <param name="outputControlUnits">  machine_units    </param>
        <param name="kp">                  0                </param>
        <param name="kd">                  0                </param>
        <param name="ki">                  0                </param>
        <param name="maxOutput">           2500             </param>
        <param name="maxInt">              200              </param>
        <param name="ko">                  0                </param>
        <param name="stictionUp">          0                </param>
        <param name="stictionDown">        0                </param>
        <param name="viscousPos">          96                </param>
        <param name="viscousNeg">          97                </param>
        <param name="coulombPos">          98                </param>
        <param name="coulombNeg">          99                </param>
+        <param name="threshold">           11                </param>
        <param name="kff">                 1                </param>
-        <param name="kbemf">               0.00             </param>
        <param name="filterType">          0                </param>
        <param name="ktau">               50.0              </param>
    </group>

icub-main: eomcParser

After the required changes, as we can see from the image below the eomcParser is now able to parse the new threshold parameter.

icub-firmware-shared:

A new threshold parameter has been added to the eOmc_FrictionParams_t structure. This required 4 Bytes more in the ROP size.

typedef struct
{
    float32_t               viscous_pos_val;
    float32_t               viscous_neg_val;
    float32_t               coulomb_pos_val;
    float32_t               coulomb_neg_val;
+    float32_t               threshold_val;
} eOmc_FrictionParams_t;    EO_VERIFYsizeof(eOmc_FrictionParams_t, 20)

icub-firmware:

The new parameter reaches correctly the FW level:

the USE_VISCOUS_COULOMB macros have been removed and the body of the PID_do_friction_comp has been improved:

float PID_do_friction_comp(PID *o, float32_t vel_fbk, float32_t trq_ref)
{    
    const float32_t MICRO { 1000000.0 };
    const float32_t coulomb_pos_converted = o->coulomb_pos_val * MICRO;
    const float32_t coulomb_neg_converted = o->coulomb_neg_val * MICRO;
 
    const float32_t viscous_neg = o->viscous_neg_val * vel_fbk;
    const float32_t viscous_pos = o->viscous_pos_val * vel_fbk;
    
    if (vel_fbk <= -o->threshold_val)
    {
        return o->Ktau*(-coulomb_neg_converted + viscous_neg + o->Kff*trq_ref);
    }
    else if (vel_fbk > o->threshold_val)
    {
        return o->Ktau*(coulomb_pos_converted + viscous_pos + o->Kff*trq_ref);
    }
    else
    {
        return o->Ktau*(o->Kff*trq_ref);
    }
}

---

What to do now:

• Test the SW+FW on a complete setup (ems+2foc + bldc motor) / real robot, where we can try to switch in torque-control-mode and see if it works as expected.
• Update the configuration files of the robots paying attention the the ones that may use the kbemf parameter
• Draft all the PRs:
  • yarp
  • icub-firmware-shared
  • icub-main
  • icub-firmware
  • icub-firmware-build
  • robots-configuration
• update ergoCub 1.0 S/N:000

cc @marcoaccame @pattacini @isorrentino

[pattacini]

Nice!

Now that I look at the table of parameters I feel that threshold may be a too anonymous name. Better off something like velocityThres. What do you think @sgiraz?

Also, we should check the units of the new param ahead of the tests.

[sgiraz]

Hi @pattacini,

I agree with you, I just changed the name of the variable to verlocityThres.
Looking at the caller function we can see that the units for the new parameter must be icubdeg/s.

Now, I proceed to remove the old kbemf parameter from all the robots in robots-configuration, introducing the new velocityThresh.

cc @isorrentino

[pattacini]

Looking at the caller function we can see that the units for the new parameter must be icubdeg/s.

This doesn't necessarily imply that we must stick to icubdeg/s.
For example, we can switch to deg/s units in the conf files and then do the conversion in the FW.
Of course, switching to deg/s is only helpful if it allows us to keep homogeneity with other parameters, for instance. Is this the case?

Then, when undecided or when there are no clear benefits, it's always better to go for more standard units.

Important

Also, once we pick the units, we need to check whether typical user values for this parameter can be transmitted with the least loss of precision.

In this latter respect, icubdeg/s requires higher values than deg/s for the same configuration, if I'm not mistaken. Therefore, choosing units has an impact also on the precision we can convey on the net.

[sgiraz]

Important
Also, once we pick the units, we need to check whether typical user values for this parameter can be transmitted with the least loss of precision.

Hi @isorrentino,

Do you believe that We can use deg/s instead of icubdeg/s considering the note highlighted by @pattacini?

Note
Consider that the velocityThresh parameter is declared as float32_t. Therefore, the FLT_EPSILON = 1.192093e-07.

cc @Nicogene

[pattacini]

Note
Consider that the velocityThresh parameter is declared as float32_t. Therefore, the FLT_EPSILON = 1.192093e-07.

Then, I'd say that there's no problem with the transmitted precision of icubdeg/s.

Also, deg/s is definitely more readable than icubdeg/s but to keep consistency with the other params, we can take a look at viscousPos and viscousNeg, which should be given probably in terms of Nm/icubdeg/s, judging from this formula.

If you confirm the latter, then there's a solid reason to stick to icubdeg/s.

[valegagge]

Hi @sgiraz ,
just a question: did you update the icub-templates files accordingly with your changes?

Remember that if your parameter is not mandatory, you should not increment the version.

Thanks!

[sgiraz]

Hi @valegagge,

I haven't changed the robots-configuration yet, but I'll take into account your suggestions.
As of now, I identified the following conditions:

Not in Icebox	In Icebox
robot condition of kbemf param ergoCubSN000 ⚠️‼️ ergoCubSN001 ⚠️‼️ iCubEdinburg01 ✅ iCubErzelli01 ✅ iCubErzelli02 ✅ iCubGenova02 ⚠️❗ iCubGenova07 ⚠️ iCubGenova08 ⚠️ iCubGenova10 ⚠️ iCubGenova11 ⚠️ iCubLisboa01 ⚠️ iCubNancy01 ✅ iCubPargue01 ⚠️ iCubShanghai01 ⚠️ iCubShenzhen01 ⚠️ iCubSingapore01 ✅ iCubTemplates ✅ iCubValparaiso01 ⚠️ iCubWaterloo01 ⚠️ iCubZagreb01 ⚠️	robot condition of kbemf param iCubBarcelona ✅ iCubChemnitz ⚠️❗ iCubDarmstadt ✅ iCubGenova04 ⚠️‼️ iCubGenova09 ⚠️‼️ iCubHeidelberg01 ✅ iCubHongKong01 ⚠️ iCubLausanne02 ✅ iCubMoscow01 ⚠️ iCubNottingham01 ✅ iCubShieffield01 ✅ iCubTwente01 ✅

Where:

• ✅ means that the kbmef exists but is 0 on all the joints
• ⚠️ means that the kbmef exists and is not 0 on the following joints: left_arm-eb1-j0_3, right_arm-eb3-j0_3, torso-eb5-j0_2
• ⚠️❗ means that the kbmef exists but is not 0 on the following joints: left_arm-eb1-j0_3, right_arm-eb3-j0_3, torso-eb5-j0_2, left_leg-eb6-j0_3, right_leg-eb8-j0_3
• ⚠️‼️ means that the kbmef exists but is not 0 on a significant number of joints

Note

• All the robots with the kbemf parameter in the (⚠️) state have the same values on the same joints. As a result, I guess most of them are cut-and-pasted.
• When kbemf is 0, it's like it doesn't exist in the friction compensation control law
• Surely, the new parameter needs to be fine-tuned for ergoCub robots

cc @isorrentino @pattacini @Nicogene

[sgiraz]

PRs drafted:

• Add velocityThres param for torque control yarp#3010
• Update eomcParser and embObjMotionControl to support the new velocityThresh param icub-main#903
• Update EoMotionControl to support the new velocityThres parameter icub-firmware-shared#86
• Add velocityThres parameter for friction compensation #410
• amc v108.9 + ems v3.73 + mc2plus v3.55 + mc4plus v3.76 icub-firmware-build#100

TODO:

• < robots-configuration >

cc @isorrentino @pattacini

[sgiraz]

Since we decided to add the new velocityThres parameter without remove the old kbemf one it is not necessary to update the robots-configurations for all the robots.

Before merging all the repos, I would ask to @isorrentino to give a try on ergoCub1.0 S/N:000 providing us some feedbacks.

cc @Nicogene @pattacini @marcoaccame

[pattacini]

Since we decided to add the new velocityThres parameter without remove the old kbemf one it is not necessary to update the robots-configurations for all the robots.

kbemf remains in YARP but the FW no longer employs it, I think. Therefore, we still need to update the robot conf files to use the new param.

Remember that robots located in icebox will always stay unchanged.

[sgiraz]

/remind 18 Sep Let's try the new parameter on ergoCub1.0 S/N:000 (cc @isorrentino @Nicogene @pattacini)

[octo-reminder]

⏰ Reminder
Monday, September 18, 2023 10:00 AM (GMT+02:00)

Let's try the new parameter on ergoCub1.0 S/N:000 (cc @isorrentino @Nicogene @pattacini)

[valegagge]

Since we decided to add the new velocityThres parameter without remove the old kbemf one it is not necessary to update the robots-configurations for all the robots.

kbemf remains in YARP but the FW no longer employs it, I think. Therefore, we still need to update the robot conf files to use the new param.

Remember that robots located in icebox will always stay unchanged.

I think that leaving the old parameter might be confusing on the user side...
if all the sw constraints for back compatibility are respected, I suppose that we should not face any problems....

cc @sgiraz @pattacini

[pattacini]

I think that leaving the old parameter might be confusing on the user side...

I agree 👍🏻
We ought to update robots' conf files to the use of the new param straight away, even though YARP will still contain the machinery to deal with the old param.

[valegagge]

@sgiraz
Please also update the icub-templates config file with the new param.
If you need more information please ask me. 😄

[octo-reminder]

🔔 @sgiraz

Let's try the new parameter on ergoCub1.0 S/N:000 (cc @isorrentino @Nicogene @pattacini)

[pattacini]

Most likely to postpone to next week as ergoCub is going to show off during the IIT anniversary tomorrow.

/remind September 26 Let's try the new parameter on ergoCub1.0 S/N:000 (cc @isorrentino @sgiraz @Nicogene)

[octo-reminder]

⏰ Reminder
Tuesday, September 26, 2023 10:00 AM (GMT+02:00)

Let's try the new parameter on ergoCub1.0 S/N:000 (cc @isorrentino @sgiraz @Nicogene)

[octo-reminder]

🔔 @pattacini

Let's try the new parameter on ergoCub1.0 S/N:000 (cc @isorrentino @sgiraz @Nicogene)

[sgiraz]

Hi @isorrentino,

When you are ready, you can start the tests on this activity.

Just FYI, in order to build everything correctly you have to set the version of YARP as follows:

# Main project
project(
  YARP
  VERSION 3.9.0
  LANGUAGES C CXX
)

This is due to the requirement imposed by icub-main:

set(YARP_REQUIRED_VERSION 3.9.0)

[isorrentino]

I tried to flash the firmware. I flashed only one board of the right leg (10.0.1.7) and I modified the configuration file according to the previous comments. Basically, I added this parameter for the two joints of the ankle:

<param name="velocityThres">     27306      27306        </param>

The value of the parameter is taken from the previous implementation (the hardcoded one) and is in icubdeg/s. I used this unit given your discussion above.

Then, I run the yarprobotinterface and I got this error:

[INFO] EthResource::askBoardVersion() found BOARD right_leg-eb7-j4_5 @ IP 10.0.1.7 of type ems4 with FW = ver 3.73 built on 2023 Aug 29 12:30
[ERROR] EthResource::verifyEPprotocol() for ep = eoprot_endpoint_motioncontrol detected: pc104.version.minor = 25 and board.version.minor = 26
[ERROR] EthResource::verifyEPprotocol() detected mismatching protocol version.minor in BOARD right_leg-eb7-j4_5 with IP 10.0.1.7  for eoprot_endpoint_motioncontrol : annot proceed any further
[ERROR] ACTION REQUIRED: BOARD right_leg-eb7-j4_5 with IP 10.0.1.7 needs a FW update to offer services for eoprot_endpoint_motioncontrol
[ERROR] embObjMotionControl: failed verifyEPprotocol. Cannot continue!
[ERROR] |yarp.dev.PolyDriver|right_leg-eb7-j4_5-mc| Driver <embObjMotionControl> was found but could not open
[WARNING] Cannot open device right_leg-eb7-j4_5-mc

How can I easily solve this error?

cc @pattacini @sgiraz @DanielePucci

[pattacini]

Hi @isorrentino

@sgiraz is sick and thus unavailable for a while. This could potentially slow down the tests, but no problem in that sense.

The message you reported seems to indicate a mismatch between the FW and the SW.
I'd need to check it w/ the help of @marcoaccame.

For confirmation, are you also using the icub-main and yarp versions reported in the list above?

[isorrentino]

For confirmation, are you also using the icub-main and yarp versions reported in the #396 (comment)?

You are right, I did not recompile yarp and icub-main. So, I do not know if recompiling yarp and icub-main things work flashing only one board.

[pattacini]

So, I do not know if recompiling yarp and icub-main things work flashing only one board.

Perhaps, the best option would be to flash the boards of one entire leg and launch that part only from the yarprobotinterface.

Just to get aligned on the things to use:

• The FW binaries need to be fetched from @sgiraz's fork (amc v108.9 + ems v3.73 + mc2plus v3.55 + mc4plus v3.76 icub-firmware-build#100) as in the meanwhile the upstream repo has certainly progressed.
• PR on YARP got merged so you can refer to the upstream repo master branch.
• By contrast, icub-main and icub-firmware-shared versions need to be fetched from @sgiraz's forks:
  • Update EoMotionControl to support the new velocityThres parameter icub-firmware-shared#86
  • Update eomcParser and embObjMotionControl to support the new velocityThresh param icub-main#903

If this test fails for whatever reason, we can wait for @sgiraz.

[isorrentino]

Thank you for the recap. I hope we already have the config file to run only one leg. I'll test this as soon as I have a new slot on the robot.

[isorrentino]

Today, I flashed e rebuilt everything reported in #396 (comment).
I used this branch for robots-configuration. The leg joints started without any error.

Nevertheless, flashing only the right leg as suggested in #396 (comment)

Perhaps, the best option would be to flash the boards of one entire leg and launch that part only from the yarprobotinterface.

does not allow to test the friction compensation. Indeed, if we do not run the full robot, we do not have WBD running and as a consequence we do not have the torque feedback.

Furthermore, I do not see the new parameter velocityThres from the motorgui.

I tried to flash all the EMS boards and start the robot with the usual config file but there are several errors due to incompatible firmware versions.

Here the log:
log.txt

cc @pattacini @sgiraz

[pattacini]

Hi @isorrentino

@sgiraz has recovered and can now follow up on this.

[sgiraz]

Hi @isorrentino,

Furthermore, I do not see the new parameter velocityThres from the motorgui.

The change has been applied in https://github.com/sgiraz/yarp/blob/81b7f3c0a52f574912bf6c953f927a8e2710b1c7/src/yarpmotorgui/piddlg.ui#L939-L943

Anyway, tomorrow I'll try to verify if there is some kind of mistake about it.

I tried to flash all the EMS boards and start the robot with the usual config file but there are several errors due to incompatible firmware versions.

As indicated in the log posted, there is a misalignment between the icub-fw-shared used to build the FW and the one used to build icub-main.

 2019,134514 <ERROR> EthResource::verifyEPprotocol() for ep = eoprot_endpoint_motioncontrol detected: pc104.version.minor = 26 and board.version.minor = 25
 2019,134519 <ERROR> ACTION REQUIRED: BOARD head-eb20-j0_1 with IP 10.0.1.20 needs a FW update to offer services for eoprot_endpoint_motioncontrol

To be sure, I can try to regenerate and provide you with the binaries for all the ETH boards. Meanwhile, I recommend verifying that you have the correct versions of icub-firmware-shared and icub-main installed.

[sgiraz]

Hi @isorrentino,

Today I found a bug in yarpmotorgui that was causing the crash (segmentation fault) when the PID button was clicked independently from the control mode currently set.
As you can see from the picture below, now it works as expected:

I just dropped a new PR in yarp:

• yarpmotorgui bugfix yarp#3030

You can wait the merge or use my fork for your tests.

Anyway, I also regenerated all the binaries for the ETH boards as mentioned above. See the PR updated:

• amc v108.9 + ems v3.73 + mc2plus v3.55 + mc4plus v3.76 icub-firmware-build#100

Unfortunately I cannot test the torque control mode with our setup due to a missing HW configuration.

Thanks to @MSECode for the support with 5-small-joint setup 🙏🏻.

By the way, you shouldn't have any problem on the robot. Let me know.

cc @isorrentino @pattacini @Nicogene

[pattacini]

Unfortunately I cannot test the torque control mode with our setup due to a missing HW configuration.

So, it seems that it's difficult to test the torque mode in action on a setup, while we tested that the information is delivered to the FW correctly.

By the way, you shouldn't have any problem on the robot. Let me know.

This could be problematic these days, given the robot agenda.
Probably, we need to wait for ergoCubSN001 to be ready.

[sgiraz]

Hi guys,

I want to drop here just a quick update.
After a quick F2F meet-up with @valegagge, I discovered that the error mentioned in the comment above:

is caused by a preliminary check at FW level from the ems board:

    if (WatchDog_check_expired(&o->trq_fbk_wdog))
    {
        o->trq_fbk = ZERO;
        o->trq_ref = ZERO;
        o->trq_err = ZERO;
        
        if (o->trq_control_active)
        {
            o->fault_state.bits.torque_sensor_timeout = TRUE;
            
            o->control_mode = eomc_controlmode_hwFault;
        }
    }

The watchdog on the trq_fbk_wdog is set with a timeout of 100 milliseconds in Joint.c (init function)

   WatchDog_set_base_time_msec(&o->trq_fbk_wdog, 2*DEFAULT_WATCHDOG_TIME_MSEC);

And it expires because no feedback is provided by the motion controller

void Joint_update_torque_fbk(Joint* o, CTRL_UNITS trq_fbk)
{
    o->trq_fbk = trq_fbk;
    
    WatchDog_rearm(&o->trq_fbk_wdog);
}

Please, @valegagge, feel free to correct me if I made some mistakes or if you want to add further details.

As of now, in order to use the torque control mode we need to run the yarprobotinterface on a robot with wholebodydynamics enabled.

cc @isorrentino @pattacini @Nicogene @marcoaccame

[marcoaccame]

As of now, in order to use the torque control mode we need to run the yarprobotinterface on a robot with wholebodydynamics enabled.

as it is meant to be: if the board does not receive torque values cannot work in that mode. and it is wholebodydynamics that sends them

[valegagge]

Hi @sgiraz ,
you are completely right. You find exactly the watchdog of we discussed.

So, currently, you need wholebodydynamics to put the robot in torque control mode

[sgiraz]

Thanks for your confirmation @valegagge.

@isorrentino, I'm pretty sure that ergoCub1.0 S/N:000 should be available on the following dates:

• 10/11 (whole day)
• 13/11 (whole day)

Feel free to book the robot to test the new feature requested. Let me know if you need any help on this.

cc @Nicogene

[isorrentino]

I've booked the robot for Monday the 13th, from 2 pm to 5 pm. @sgiraz. if you are available let's run the tests together so that you are already there in case of any issues or questions. Thanks.

cc @DanielePucci @GiulioRomualdi

[isorrentino]

I had an offline discussion with @sgiraz. Unfortunately, he won't be available on Monday. Nevertheless, I plan to proceed with the tests independently. I've asked @sgiraz to summarize the required robot configuration details (YARP, icub-main versions, and configuration file modifications). This time I'm going to flash all the boards of ergoCubSN000 to launch 'whole-body-dynamics' with the mainyarprobotinterface. In this way, we have access to the streaming of the estimated joint torques required to enable torque control. With torque control enabled, I can test the features required in this ticket.

[isorrentino]

Branches to use per each repo:

• https://github.com/robotology/yarp.git (master)
• sgiraz/icub-firmware-shared at devel (github.com)
• sgiraz/icub-main at velocityThres (github.com)
• sgiraz/icub-firmware-build at devel (github.com)
• sgiraz/robots-configuration at devel (github.com)

With this configuration I get the following error when I run the yarprobotinterface:

[INFO] EthResource::askBoardVersion() found BOARD head-eb20-j0_1 @ IP 10.0.1.20 of type mc4plus with FW = ver 3.75 built on 2023 Jul 31 11:30
[ERROR] EthResource::verifyEPprotocol() for ep = eoprot_endpoint_motioncontrol detected: pc104.version.minor = 26 and board.version.minor = 25
[ERROR] EthResource::verifyEPprotocol() detected mismatching protocol version.minor in BOARD head-eb20-j0_1 with IP 10.0.1.20  for eoprot_endpoint_motioncontrol : annot proceed any further
[ERROR] ACTION REQUIRED: BOARD head-eb20-j0_1 with IP 10.0.1.20 needs a FW update to offer services for eoprot_endpoint_motioncontrol
[ERROR] embObjMotionControl: failed verifyEPprotocol. Cannot continue!
[ERROR] |yarp.dev.PolyDriver|head-eb20-j0_1-mc| Driver <embObjMotionControl> was found but could not open

cc @pattacini @sgiraz @DanielePucci @GiulioRomualdi

[pattacini]

To cut this short, we ought to arrange for a shared session on the robot.
cc @sgiraz

[sgiraz]

I've booked iCubGenova11 for next Friday morning (17th November). Actually, @isorrentino cannot guarantee the presence, but I'll try to do my best to complete the test successfully.

[sgiraz]

Today @isorrentino and I tested successfully both the new SW and FW on iCubGenova11.

As you can see in the last row of the PID table, there is the new velocityThres parameter which makes the friction compensation tunable at runtime.

Note

Although some quick friction compensation tests provided unexpected results, it's important to consider that these tests differed from previous tests conducted by @isorrentino on ergoCub1.0 S/N:000 in terms of the joint tested and the robot used.

cc @Nicogene

[pattacini]

Well done!
We can now proceed further with upgrading robots-configuration files of our robots to get them aligned with this development.

[sgiraz]

As agreed during the review meeting, next week I'm going to open a PR on robots-configuration@devel in order to apply the changes as described in #396 (comment).

cc @Nicogene @pattacini

[sgiraz]

PR is open:

• Add friction + coulomb + threshold parameters  robots-configuration#599

cc @Nicogene @pattacini @isorrentino