search

robotology / icub-firmware

iCub Firmware
Other
12 stars 30 forks source link

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

Closed isorrentino closed 11 months ago

isorrentino commented 1 year ago

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 commented 1 year ago

@sgiraz, would you mind taking care of this?

sgiraz commented 1 year ago

Hi guys,

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

pattacini commented 1 year ago

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:

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

sgiraz commented 1 year ago

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.

image

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:

image

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:

cc @marcoaccame @pattacini @isorrentino

pattacini commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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:

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 commented 1 year ago

PRs drafted:

TODO:

cc @isorrentino @pattacini

sgiraz commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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

octo-reminder[bot] commented 1 year ago

⏰ 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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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

octo-reminder[bot] commented 1 year ago

🔔 @sgiraz

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

pattacini commented 1 year ago

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[bot] commented 1 year ago

⏰ 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[bot] commented 1 year ago

🔔 @pattacini

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

sgiraz commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

For confirmation, are you also using the icub-main and yarp versions reported in the https://github.com/robotology/icub-firmware/issues/396#issuecomment-1697219349?

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 commented 1 year ago

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:

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

isorrentino commented 1 year ago

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 commented 1 year ago

Today, I flashed e rebuilt everything reported in https://github.com/robotology/icub-firmware/issues/396#issuecomment-1748331088. I used this branch for robots-configuration. The leg joints started without any error.

Nevertheless, flashing only the right leg as suggested in https://github.com/robotology/icub-firmware/issues/396#issuecomment-1748331088

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. image

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 commented 1 year ago

Hi @isorrentino

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

sgiraz commented 1 year ago

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 commented 1 year ago

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:

motorgui_velThresold_param

I just dropped a new PR in yarp:

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:

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

motorgui_torque_ctrl_mode

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 commented 1 year ago

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 commented 1 year ago

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:

image

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

Thanks for your confirmation @valegagge.

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

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

Branches to use per each repo:

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 commented 1 year ago

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

sgiraz commented 1 year ago

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 commented 11 months ago

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

test-friction-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 commented 11 months ago

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

sgiraz commented 11 months ago

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 https://github.com/robotology/icub-firmware/issues/396#issuecomment-1697136151.

cc @Nicogene @pattacini

sgiraz commented 11 months ago

PR is open:

cc @Nicogene @pattacini @isorrentino