Set control parameters from Simulink block

[gabrielenava]

Currently, the control parameters such as gains and regularization terms are stored in matlab functions like this one.

The autogenerated C++ code from Simulink ( https://github.com/robotology-playground/autogenerated-whole-body-controllers) will have hard-coded gains that cannot be modified without editing the source code.

Therefore it may be useful to change the way Simulink loads these parameters, namely there should be a Simulink block that reads these values from a configuration file, that can be read both from the Simulink code and from the autogenerated c++ code.

cc @diegoferigo

[diegoferigo]

This is a challenging problem and finding a clean solution will not be easy. Let's focus on the gains since they are the more complex structure of our controllers.

Simulink models need to find the gains stored into the workspace, otherwise the values couldn't be accessed by blocks directly (e.g. Gain) and neither by blocks implementing a Matlab function. We cannot avoid this.

Generally speaking we can store values in 3 ways:

1. In the workspace, populated by a .m initialization script (current status)
2. In a file read from the filesystem by a block (e.g. using yarp's ResourceFinder to locate it)
3. In a new block that contains all the gains

It would be nice finding a way to have a single point where these values can be stored, accessible from either simulink and the autogenerated code.

I see the following problems:

1. The gains are hardcoded into the autogenerated code. Structs from the workspace become C structs (which is nice) and they can be edited manually directly from the autogenerated code, maybe also from the main by including the proper header (this would require knowing in advance the fields of the struct, but worst case scenario the code does not compile).
2. This would be probably the best and more generic choice, but the problem is how to load the content of this file in the workspace right before the simulation starts. Even if we can parse the file from a Matlab block function and populate the workspace, those values are anyway stored in the same structs of point 1.
3. This requires the support of changing parameters of wb-toolbox's blocks from the C++ code. This it to be done at the current stage, and it is challenging as well. The problem here is how to properly point to the right block (there might be more blocks with the same name in different subsystems, so the full path from the model root is needed). This would translate to a small user code from the main if a single parameter of a block need to be changes, but this new big gains block would need some helper functions.
4. Convert parameters to signals and feed them where needed. It might be done with Simulink.Bus or similar, I'm not an expert on this. I think to remember that these kind of custom types create additional headers containing structs. A new block can parse the gains from a file and convert them to this new custom signal.

@gabrielenava @S-Dafarra @traversaro @DanielePucci Do you have any other idea? I just came up with option 4, and it is what I like more. It is the more general and (of course) also the one that in my opinion needs more investigation time.

[S-Dafarra]

Probably I have not fully understood how the parameters are parsed inside the autogenerated code. Anyway, maybe we can hit two pigeons with a stone if we use a matlab script which fills a .ini file in a desired structure starting from the usual .m files.

I think it is necessary to have a separate script to generate the .ini file since we may need to evaluate some matlab related functions (like zeros). In addition, some gains are modified several times in the configuration files (and @gabrielenava knows I hate this 😂 ). Thus it is better to load the parameters from the matlab workspace directly (and this is why it has to be a matlab script).

Assuming then to have the autogenerated code inside a RFModule, at configure time, we can use ResourceFinder to locate the generated .ini and modify the C structure containing the parameters (which is automatically generated by matlab right?). This allows to avoid recompiling the code, while we can still perform checks on the provided options, eventually throwing errors.

[S-Dafarra]

Another possibility could be also to save the matlab workspace directly and use matio inside the generated code. The C structure is then modified by matching field names with the matlab variables name saved in the .mat file.

[diegoferigo]

I started working on this task and would like to reach soon a basic support for parametric controllers. Particularly, I want to add the support of performing gain tuning even from the automatically generated code. These kind of parameters are now stored in a .m script loaded into the workspace before the start of the simulation.

My idea is to move these parameters in a new xml which will be shared from the simulink controller and the automatically generated one. For what concern Simulink, the exposed parameters will be exposed as signals, and this means that we need to add a new block with the following functionalities:

• Accept from the mask a string with the name of the file
• Accept from the mask the name of the parameter to read (either scalar or vector or matrix)
• Load the xml and parses it
• Get the required data
• Forward this data to its output signal

It should not be too difficult. The only downside I see here is that we need to add a single block for every parameter we want to read. Furthermore, only numeric parameters will be supported.

For what concern the automatically generated code, the support will come for free since this new parameters block will be treated as all the other ones.

[diegoferigo]

cc @traversaro

[traversaro]

It should not be too difficult. The only downside I see here is that we need to add a single block for every parameter we want to read. Furthermore, only numeric parameters will be supported.

@gabrielenava can you check with our Simulink users that this is reasonable and not too much cumbersome?

General comment:
In the FMI world, the xml file that @diegoferigo is describing is similar to the ssv file as defined in the System Structure and Parameterization (SSP) standard , see slide 8 of https://github.com/modelica/ssp-standard.org/blob/master/publications/2018_American_Modelica_Conference/2018_Usermeeting_SSP-StatusandPlans.pdf and section 3 of http://www.ep.liu.se/ecp/124/005/ecp16124005.pdf . Unfortunately the standard still needs to be published, so it may not be feasible to think to use it directly (I do not even know if the current draft support vector/matrix parameters), but as there are some draft XSD schema of how the file should look ( https://github.com/CATIA-Systems/FMPy/blob/master/fmpy/ssp/schema/SystemStructureParameterValues.xsd ) it would be nice to look at them to get an inspiration of a possible structure.

[diegoferigo]

I spoke with @gabrielenava @S-Dafarra @FabioBergonti and we reached the following conclusions:

• XML in not the right format since often we do gain tuning on these parameters, and performing math operations on them (e.g. multiply a vector parameter by 10) would not be possible
• The parameters will be stored in a binary .mat file (not commited)
• The .mat file is generated by a .m script, runnable from either matlab or octave. This is a text file and can be committed having history
• WBToolbox will read from c++ the .mat file with matio and will provide a block that outputs a scalar / vector value
• If needed, the logic to group them in e.g. bus (in order to use them as struct in the MATLAB Function block) will be performed by the controller developer

In this way, gain tuning from the autogenerated code is still possible without having Matlab / Simulink. The gain tuning is performed on the .m that generates the .mat. The autogenerated class loads this .mat during load time as it happens in Simulink and since parameters are mapped to signals no further operations should be required.

cc @traversaro @DanielePucci @aikolina @Yeshasvitvs

[traversaro]

Cool.

A few comments:

• I like the .m --> .mat process. If in the future .ssv becomes an interesting format for storing paramers, we can always substitute the .mat with the .ssv, and just have a .m --> .ssv step.
• Change parameters + Rerun the controller in simulink probably takes 5 seconds, and is one of the most common operations when working with a controller. It is possible to make sure that the .m --> .mat conversion is done automatically before the controller is running?
• How do handle robot-specific configuration files? Via the logic in the .m --> .mat script?
• If I am not wrong, in the existing ".m"-file configuration files we also have logic related to gain scheduling (

  whole-body-controllers/torque-controllers/momentum-based-standup/app/robots/iCubGenova02/initCoordinator.m

  Line 28 in 7ab9d86

  if sum(Config.LEFT_RIGHT_FOOT_IN_CONTACT) == 2

  ) i.e. parameters that change based on the state of the controller. How do you think of handling those?
• Maintaining a script that runs both on Matlab and on Octave, even if it is as simple as a conf script, it is challenging. It would be great to have a CI to verify that those configuration scripts run on Octave without errors.

[diegoferigo]

Change parameters + Rerun the controller in simulink probably takes 5 seconds, and is one of the most common operations when working with a controller. It is possible to make sure that the .m --> .mat conversion is done automatically before the controller is running?

Yes, my idea is to run the script as a pre-run callback and place the .mat in the same folder of the mdl. I still have to understand how to pass the folder to the controller. Since this block will be stored in the WBToolbox and not BlockFactory, we can consider using ResourceFinder at least in the beginning.

How do handle robot-specific configuration files? Via the logic in the .m --> .mat script?

Instead of running directly the .m in the callback, we can have a switch (or matlab function with an argument) that loads the right robot-specific .m

existing ".m"-file configuration files we also have logic related to gain scheduling

If I understood what you mean, since the script is executed before the controller starts, all the logic would be applied. The line you reported is read only once, right @gabrielenava?

Maintaining a script that runs both on Matlab and on Octave, even if it is as simple as a conf script, it is challenging. It would be great to have a CI to verify that those configuration scripts run on Octave without errors.

I don't have extensive experience with octave, but I assumed that simple calculations as this case would represent trivial scripts that should not be affected by the challenges you mentioned. Anyway, CI is always a good idea.

[gabrielenava]

If I understood what you mean, since the script is executed before the controller starts, all the logic would be applied. The line you reported is read only once, right @gabrielenava?

Yes, this line is read only once.

[traversaro]

@gabrielenava so where is the logic for gain scheduling handled?

[gabrielenava]

@gabrielenava so where is the logic for gain scheduling handled?

Gain scheduling is performed only during the yoga (or equivalently, the standup) demo. In the initStateMachineYoga.m file, for each gain matrix we create a corresponding "gain scheduling" matrix whose rows contain the diagonal elements of the gain matrix for each state of the state machine (it is assumed that all the gains matrices are diagonal). Then, the correct row of the "gain scheduling matrices" is selected according to the current state inside the stateMachineYoga. The transition from one set of gains to the other is performed by means of the minimum jerk trajectory generator Simulink block.

[traversaro]

Perfect, so it should work fine with the proposed method, right?

[gabrielenava]

If I understood correctly it should work, as we do not update the gain matrices real time, but rather we load since the beginning all the gains for all the states of the state machine, and there is a switch inside Simulink that select the correct gain matrices according to the state.

[traversaro]

In the FMI world, the xml file that @diegoferigo is describing is similar to the ssv file as defined in the System Structure and Parameterization (SSP) standard

The draft of the SSP standard is available at https://ssp-standard.org/publications/SSP10RC1/SystemStructureAndParameterization10RC1.pdf . The relevant section is section 6, System Structure Parameter Values (SSV).

[traversaro]

I checked the draft, and apparently 1.0 will not support vector/matrix parameters, so it is quite useless at the moment for our use case. I guess vector parameters will come with version 1.1, currently planned for the end of 2019/beginning of 2020 (see slide 12 of https://ssp-standard.org/publications/2018_American_Modelica_Conference/2018_Usermeeting_SSP-StatusandPlans.pdf).

[gabrielenava]

the activity is not going to be addressed in the near future, closing the issue for now