Document that some block parameters need to be coherent with the Simulink fixed time-step

[traversaro]

Summary

Document that some block parameters need to be coherent with the Simulink fixed time-step. In the DiscreteFilter and MinimumJerkTrajectoryGenerator, the parameters Ts and SampleTime respectively need to be coherent with the simulation timestep of the Simulink model.

Note that this would not be necessary if this models would be represented as discrete systems with an explicit state, instead of instantaneous system with an hidden state whose evolution is triggered by an evaluation of the output function.

cc @gabrielenava

Motivation

I remember that several users (in particular @FabioBergonti) were changing the simulation time-step of Simulink in some simulation, but I am not sure that they were aware of the need of changing the parameters of the MinimumJerkTrajectoryGenerator.

[diegoferigo]

In our normal workflow for the controllers we have built on top of wb-toolbox there is always a Ts variable that is used to propagate this information. However, for external users of this toolbox this is never mentioned anywhere.

I am not sure how to make the state explicit while using our filter classes, that contain internally the information about previous state they need.

Instead, for what concern the sample time, we should definitely try to read it directly from Simulink. This would require modifying the public APIs of blockfactory::core::BlockInformation with a new method, and it should be done before the 1.0 release.

[traversaro]

In our normal workflow for the controllers we have built on top of wb-toolbox there is always a Ts variable that is used to propagate this information. However, for external users of this toolbox this is never mentioned anywhere.

Do you copy @FabioBergonti? Do you agree?

I am not sure how to make the state explicit while using our filter classes, that contain internally the information about previous state they need.

To be honest I really think that explicitly modelling discrete system is only viable way to achieve important features in the long term for our controllers/estimator, such the possibility to rollback in time (useful also for some RL algorithms) or to enable the use of variable step integrators for hybrid systems (hybrid in the sense that the controller is discrete, while the plan is continuous) . I think it may be doable to modify those classes to make sure that they expose an interface such as:

size_t getInternalStateSize() const;  
bool getInternalState(yarp::sig::Vector & stateBuffer) const;
bool setInternalState(yarp::sig::Vector & stateBuffer);

but for anything related to ctrlLib we should before check with @pattacini . See https://github.com/prashanthr05/idyntree/commit/a67d881087d9cbcd2549d1967a844cc7f30af3aa#r31755418 for a related discussion

Instead, for what concern the sample time, we should definitely try to read it directly from Simulink. This would require modifying the public APIs of blockfactory::core::BlockInformation with a new method, and it should be done before the 1.0 release.

To be honest, I would really like to avoid that, due to the following limitations:

• In a hybrid system setting, it perfectly makes sense to run the filter discrete system at a fixed period of 10 or 1 ms (think for example if you are simulating a filter implemented in a firmware) while changing the fixed timestep to a lower value to decrease the numerical error.
• In a hybrid system scenario, assuming a fixed timestep will prevent the use of variable step error-controlled integrators.

[diegoferigo]

If I understood what you mean, and I am not really sure, this:

In a hybrid system setting, it perfectly makes sense to run the filter discrete system at a fixed period of 10 or 1 ms (think for example if you are simulating a filter implemented in a firmware) while changing the fixed timestep to a lower value to decrease the numerical error.

considering the current status, is in contrast with this:

Note that this would not be necessary if this models would be represented as discrete systems with an explicit state, instead of instantaneous system with an hidden state whose evolution is triggered by an evaluation of the output function.

because even if the state was explicit, the output of the filter would be evaluated at the same rate of the simulation. Moreover, it is not clear to me how we should handle these classes that require a fixed step in a variable step scenario.

Is what you propose related to have the possibility to run the filter at a higher frequency than the simulation's one?

[traversaro]

because even if the state was explicit, the output of the filter would be evaluated at the same rate of the simulation

At the moment, it is not possible with the existing blockfactory/WB-Toolbox functionality to make the (discrete) state explicit, as there is no way to set or get the discrete state in the BlockInformation. If this support is implemented, as described for example in https://github.com/robotology/blockfactory/issues/8 , then it would be possible to specify for a discrete system an explicit period that is different from the simulink timestep, see the comment in https://github.com/robotology/blockfactory/issues/8#issuecomment-449880237 .

[FabioBergonti]

@traversaro

I remember that several users (in particular @FabioBergonti) were changing the simulation time-step of Simulink in some simulation, but I am not sure that they were aware of the need of changing the parameters of the MinimumJerkTrajectoryGenerator

I was aware of that

Do you copy @FabioBergonti? Do you agree?

In our normal workflow for the controllers we have built on top of wb-toolbox there is always a Ts variable that is used to propagate this information. However, for external users of this toolbox this is never mentioned anywhere.

Yes, I know the presence of Ts. I acted on it to modify the simulink frequency.

I think my problems were more related to the frequency that simulink received the informations from gazebo

[diegoferigo]

At the moment, it is not possible with the existing blockfactory/WB-Toolbox functionality to make the (discrete) state explicit, as there is no way to set or get the discrete state in the BlockInformation. If this support is implemented, as described for example in robotology/blockfactory#8 , then it would be possible to specify for a discrete system an explicit period that is different from the simulink timestep, see the comment in robotology/blockfactory#8 (comment) .

Ok if we are talking about multirate S-Function blocks we are accessing an entire new world ;)

[traversaro]

Ok if we are talking about multirate S-Function blocks we are accessing an entire new world ;)

~~Indeed it is a brand new world, but I do not think there is any other way to model discrete system with explicit state.~~ Sorry, see the next comment.

[traversaro]

Sorry, I just noticed that the link was pointing to the multi rate section of Simulink. You can specify the period of the discrete block even if it is a single rate block, sorry for the confusion.

[diegoferigo]

Ok, the right Simulik documentation is this: https://it.mathworks.com/help/simulink/sfg/sample-times-cpp.html.

[traversaro]

Exactly!

[traversaro]

Related to https://github.com/robotology/wb-toolbox/issues/165#issuecomment-451145638, as an example, a possible not optimized implementation of those three methods for the iCub::ctrll::Filter (https://github.com/robotology/icub-main/blob/c3ad642ed0aacdb3216f692cc73916936079714b/src/libraries/ctrlLib/src/filters.cpp#L26) is the following:

size_t Filter::getInternalStateSize() const
{
    return ((m-1)+(n-1))*y.length();
} 

bool getInternalState(yarp::sig::Vector & stateBuffer) const
{
    size_t idx = 0;
    stateBuffer.resize(this->getInternalStateSize());
    for (size_t i=1; i<m; i++) {
        for (size_t j=0; j<y.length(); j++) {
            stateBuffer[idx] = uold[i-1][j];
            idx++;
        }
    }
    
    for (size_t i=1; i<n; i++) {
        for (size_t j=0; j<y.length(); j++) {
            stateBuffer[idx] = yold[i-1][j];
            idx++;
        }
    } 
     return true;
}

bool setInternalState(const yarp::sig::Vector & stateBuffer) 
{
    size_t idx = 0;
    if (stateBuffer.size() != this->getInternalStateSize())) { 
       return false;
    }
    for (size_t i=1; i<m; i++) {
        for (size_t j=0; j<y.length(); j++) {
            uold[i-1][j] = stateBuffer[idx];
            idx++;
        }
    }
    
    for (size_t i=1; i<n; i++) {
        for (size_t j=0; j<y.length(); j++) {
            yold[i-1][j] = stateBuffer[idx];
            idx++;
        }
    } 
    return true;
}

At least for retrieving, a similar method was already added in https://github.com/robotology/icub-main/commit/295ced9e8347a9ad02d2d0cb3d36c6157d1a6d54 .