Refactor and wrap part of the walking logic as a BlockFactory block?

[traversaro]

As most of walking-controllers developers probably know I have a secret plan for streamlining our software infrastructure, ensuring that the software we develop can be composed easily with no runtime additional latency due to the component composition, and to be able to have reproducible simulation regardless of the computational load of the simulating machine.

One thing that I think would definitely boost the re-usability of the awesome walking algorithm(s) contained in this repo is to wrap its core "Walking" logic in a discrete system form, to simplify the following use cases:

• Run the walking algorithm as part of complex simulation scheme made up by smaller discrete systems, think for example a Simulink model, a complex Adams simulation or a Creo simulation for a co-design purpouse.
• Programmatically run simulations of the walking algorithm coupled with a simulated robot and environment, for performing automatic gain tuning or to exploit the latest fancy reinforcement learning algorithm on the top of the existing walking infrastructure.

To wrap an existing C++ code as a discrete system, several possible interfaces exists, such as Drake, FMI, Simulink S-Functions . However, I think the most suitable option is to use our actively developed BlockFactory library, mainly because we control it, and so we can eventually modify it to suit any kind of crazy requirement could emerge from our research, because is extremely lightweight (no dependencies), and because we add new targets in the future to it, see for example robotology/blockfactory#5 for a discussion on adding FMI export support to it (TL;DR: we need to wait for FMIv3 that will support the features that we need).
An example of using BlockFactory to wrap an instantaneous system can be found in https://robotology.github.io/blockfactory/mkdocs/create_new_library/ . The issue discussing support for discrete time systems and the workaround currently used to implement them can be found in robotology/blockfactory#8 .

At this point you may think:
but porting the walking controller to BlockFactory will probably means that we need to rewrite it, and we cannot used it anymore as a normal YARP module!
based on my preliminary assessment of the code, this is probably not true, mostly due to the awesome work of everyone involved in writing the controller (kudos everyone, really).

The only small refactoring steps that I think are necessary are the following:

1. separate the logic of the controller when in it is "Walking" state, from the rest of the WalkingModule logic. The logic of switching control modes, reaching the initial position with the position control, checkMotionDone, etc etc is IMHO too complex/different to wrap as a discrete system, but the juicy part is the one of the Walking state, that is more and less what is described in a block diagram such as the one reported in the README.
   All that logic should be refactored in a different class (something like WalkingDiscreteSystem, any better name suggestion is welcome) together with most of the attributes of WalkingModule that are just used/accessed in the Walking state. Ideally, the outcome of this refactor would be that the part of the updateModule method related to the Walking state would be simplified to:

else if(m_robotState == WalkingFSM::Walking)
{
    m_walkingDiscreteSystem->advance(); // This is tipically step, but we agreed that it is not the best name in a walking context
}

2. Refactor the classes that handle the interaction with the rest of the YARP infrastructure (that are already confined in RobotHelper and WalkingPIDHandler ❤️) in inheriting from a series of C++ interface classes (i.e. a pure virtual classes), and just use a pointer to those interfaces inside WalkingDiscreteSystem. The implementation of this interfaces to communicate
   with YARP (the existing RobotHelper and WalkingPIDHandler, that it would make sense to rename YARPRobotHelper and YARPWalkingPIDHandler) will need to be created inside the WalkingModule class, and passed to the WalkingDiscreteSystem just as pointers to the generic interface, to ensure that no network related logic is contained in WalkingDiscreteSystem.

3. Once WalkingDiscreteSystem will be YARP-network-free, it will be possible to use it also inside a BlockFactory block. To modify at least as possible
   its internal structure (to share it with the YARP module, that will continue to work as usual) the trick would be to reimplement the RobotHelper and WalkingPIDHandler interfaces to
   interact with BlockFactory's ports. For example, getFeedbacksRaw will not read anymore from the YARP network, but from some BlockFactory ports that indicate the "measured joint positions" and
   "measured joint velocities", using the getInputPortSignal methods of BlockFactory (see https://github.com/robotology/blockfactory/blob/master/example/src/SignalMath.cpp#L133). Similarly,
   setDirectPositionReferences will set some value on some output port. Non-relevant methods (such as checkMotionDone) can be simply left not implemented, as long as they are not called
   inside WalkingDiscreteSystem.

4. Profit!

One cool thing is that it is not necessary to change is the nice configuration system that is currently in place: the WalkingDiscreteSystem can be configure with a yarp::os::ResourceFinder object, and the only additional that will be necessary to have in the BlockFactory's Block will be a string, to indicate the .ini configuration file that contains all the parameters (that could be shared with no problems between the YARP module and the BlockFactory's block.

[traversaro]

cc @S-Dafarra @GiulioRomualdi @prashanthr05

[S-Dafarra]

I like the idea of having the walking as a single block to be used in a bigger infrastructure! I really think this is the way to go for achieving high level goals/tasks, while make it easier for new people to focus only on a single part.
Back in time I started an activity of refactoring of the old walking controller, aimed at making it "modular". Thus I see the walking controller as a sort of subsystem, not a single block, to make it easy to change for example planner, add or remove intermediate loops, so on and so forth. This activity started, but then I moved task before reaching a final solution.
The code is here https://github.com/S-Dafarra/capture-point-walking/tree/zmpRefactored/modules/WalkingControllersComponents (it was more or less complete, while the controller was only a draft, see https://github.com/S-Dafarra/capture-point-walking/tree/zmpRefactored/modules/ZMPPreviewWalkingController). I don't know, maybe it can be a starting point.

[diegoferigo]

Programmatically run simulations of the walking algorithm coupled with a simulated robot and environment, for performing automatic gain tuning or to exploit the latest fancy reinforcement learning algorithm on the top of the existing walking infrastructure.

Just to outline some nice things to have even more in the future, if the gains to tune would be exposed as input signals it would drastically simplify the integration of an high-level system that automatically tune them. I see that there are really (really) many parameters, and being able to access them (all or just a subset) directly making them the resulting action of the policy under training would be great, even tho in the beginning this kind of interface could be done by a .ini template and automatic file creation associated to a restart of the walking device.

Ack for everything else, let me know if you have any blockfactory-related curiosity.

[traversaro]

if the gains to tune would be exposed as input signals it would drastically simplify the integration of an high-level system that automatically tune them. I see that there are really (really) many parameters, and being able to access them (all or just a subset) directly making them the resulting action of the policy under training would be great

Totally agree that eventually it would be great to be able to programmatically change the gain even during a simulation. However, I think the right way to expose them would not be as input variables, but rather "tunable parameter" (in the FMI mathematical formalism term). The difference between the two is ( https://github.com/modelica/fmi-standard/blame/44e7a9fbd2cab711e39051b73c0186da91ceff6c/docs/2_2_common_schema.adoc#L1028, rendered version: https://fmi-standard.org/docs/2.0.1-develop/#_definition_of_model_variables_modelvariables ):

How to treat tunable variables:

A parameter p is a variable that does not change its value during simulation, in other words, dp/dt = 0.
If the parameter "p" is changing, then Dirac impulses are introduced since dp/dt of a discontinuous constant variable "p" is a Dirac impulse.
Even if this Dirac impulse would be modeled correctly by the modeling environment, it would introduce unwanted "vibrations".
Furthermore, in many cases the model equations are derived under the assumption of a constant value (like mass or capacity), and the model equations would be different if "p" would be time varying.

FMI for Model Exchange: +
Therefore, "tuning a parameter" during simulation does not mean to "change the parameter online" during simulation.
Instead, this is a short hand notation for:

. Stop the simulation at an event instant (usually, a step event, in other words, after a successful integration step).

. Change the values of the tunable parameters.

. Compute all parameters that depend on the tunable parameters.

. Newly start the simulation using as initial values the current values of all previous variables and the new values of the parameters.

Basically this means that a new simulation run is started from the previous FMU state with changed parameter values.
With this interpretation, changing parameters online is "clean", as long as these changes appear at an event instant.

FMI for Co-Simulation:
Changing of tunable parameters is allowed before an fmi3DoStep call (so, whenever an input can be set with fmi3SetXXX) and before fmi3ExitInitializationMode is called (that is before and during Initialization Mode).
The FMU internally carries out event handling if necessary.]

Not really sure what the "tunable parameters" equivalent is in Simulink, but I suspect this is the right formalism to use for gains (even if for a system with just discrete time state probably there is not a big difference).

[traversaro]

Given his experiences with Chorenoid and OpenRTM components (if I am not wrong) perhaps also @MiladShafiee. Is interested in this discussion.

[diegoferigo]

Totally agree that eventually it would be great to be able to programmatically change the gain even during a simulation. However, I think the right way to expose them would not be as input variables, but rather "tunable parameter" (in the FMI mathematical formalism term). The difference between the two is ( https://github.com/modelica/fmi-standard/blame/44e7a9fbd2cab711e39051b73c0186da91ceff6c/docs/2_2_common_schema.adoc#L1028, rendered version: https://fmi-standard.org/docs/2.0.1-develop/#_definition_of_model_variables_modelvariables ):

Simulink has tunable parameters, and we already have them enabled, but at the moment they are not easily accessible since they are hardcoded in the sources and getting an index to their right location is not possible (at least, I still haven't found a way). The new block I developed (discussion: robotology/whole-body-controllers#26) and will finalize soon is a workaround to this.

[traversaro]

I see. Simulix seems to be supporting tunable parameters in FMU-blocks generated from code generation of Simulink models in https://github.com/Kvixen/Simulix/blob/cc317f89fe7c03ed83a218813d0588d0d80ac57b/templates/Simulix_exemain_template.c#L128, but I am no sure how that is related to concrete problem that we were tackling in robotology/whole-body-controllers#26 .

[diegoferigo]

See:

https://github.com/robotology/blockfactory/blob/a05e6d0d192274ebe4f4fae57d8987c9dfa3cde0/example/matlab/AutogenerationExample_grt_rtw/AutogenerationExample.h#L75-L80

And:

https://github.com/robotology/blockfactory/blob/a05e6d0d192274ebe4f4fae57d8987c9dfa3cde0/example/matlab/AutogenerationExample_grt_rtw/AutogenerationExample.cpp#L390-L400

They are stored in a static struct, but when there is more than one they are stored sequentially. It would be nice having a map or similar, but it's not the case.

[MiladShafiee]

Given his experiences with Chorenoid and OpenRTM components (if I am not wrong) perhaps also @MiladShafiee. Is interested in this discussion.

I'm interested in this topic, however, I didn't use the OpenRTM components. The Choreonoid has a simpleController plugin that allow us to implement the motion planning and control module! In this way we can receive sensors data and we can command the motors torque based on our implemented controller! Every new things can be implemented by developing a new plugin in Choreonoid!

[traversaro]

@diegoferigo that is super interesting, what do you think is the best issue in which to discuss this? We probably already have a few.

@MiladShafiee good to know, thanks! I think a Chorenoid plugin (simular to a Gazebo plugin, see http://gazebosim.org/tutorials/?tut=plugins_hello_world) would be another use case for which the proposed refactor would be useful.

Back in time I started an activity of refactoring of the old walking controller, aimed at making it "modular".

@S-Dafarra As discussed f2f, this is an extremly interesting direction, more complete but also probably much more complex w.r.t. to the effort of decoupling WalkingModule and WalkingDiscreteSystem classes while minimizing the source code changes.

[diegoferigo]

Moved discussion about parameters in the autogenerated code in robotology/blockfactory#42

[traversaro]

I see. Simulix seems to be supporting tunable parameters in FMU-blocks generated from code generation of Simulink models in https://github.com/Kvixen/Simulix/blob/cc317f89fe7c03ed83a218813d0588d0d80ac57b/templates/Simulix_exemain_template.c#L128, but I am no sure how that is related to concrete problem that we were tackling in robotology/whole-body-controllers#26 .

FMIKit-Simulink is another related project that was open sourced recently that it may be worth to check out.