DiscreteFilter block #47

diegoferigo · 2017-08-10T09:46:28Z

This block wraps all the filters included in the iCub::ctrl namespace (filters.h):

iCub::ctrl::Filter
iCub::ctrl::FirstOrderLowPassFilter
iCub::ctrl::MedianFilter

This class already supports multidimensional input signals.
This PR needs the interface that has been discussed in robotology/icub-main#463.

Future TODOs:

This is what is currently missing and it might be included in future PRs if required:

Passing filter coefficients during run time (through an additional port)
Passing initial state from a signal

traversaro · 2017-08-10T10:18:57Z

If we are going to require the devel branch of icub-main in WB-Toolbox, I think it is convenient to create a devel branch also on icub-main.

traversaro · 2017-08-10T14:00:44Z

toolbox/src/DiscreteFilter.cpp

@@ -125,7 +132,10 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error)
            }
            return 1;
        }
-        filter = new iCub::ctrl::Filter(*num, *den);
+        for (unsigned i = 0; i < inputSignalWidth; ++i) {


The filter class support vector input/output, why are are you using a different filter for each component of the vector?

With the new commit I forced pushed now the block works with vectors and reduces dynamic allocation

traversaro · 2017-08-10T14:03:59Z

toolbox/src/DiscreteFilter.cpp

+    // Get the width of the signals
+    unsigned inputSignalWidth  = blockInfo->getInputPortWidth(INPUT_IDX_SIGNAL);
+    unsigned outputSignalWidth = blockInfo->getOutputPortWidth(OUTPUT_IDX_SIGNAL);
+    assert(inputSignalWidth == outputSignalWidth);


I am not an expert of Simulink, but what happens with this assert if the user connect two signal of different size to the input and output of the block? Simulink crashes?

It crashes. In my opinion having asserts is good even in this case. When people work on controllers (downstream) the code should be always be set in release. Debug should be enabled only when developing the block themselves. @francesco-romano

traversaro · 2017-08-11T07:29:17Z

toolbox/src/DiscreteFilter.cpp

+    s.erase(remove_if(s.begin(), s.end(), lambda_remove_chars), s.end());
+
+    // Convert the cleaned string to a yarp vector of floats
+    std::istringstream sstrm(s);


This conversion part could be affected by locale-bugs such as robotology/idyntree#288 .

Fixed in aafaf38

francesco-romano · 2017-08-11T08:31:36Z

toolbox/src/base/factory.cpp

@@ -36,7 +36,10 @@ wbt::Block* wbt::Block::instantiateBlockWithClassName(std::string blockClassName
 #ifdef WBT_USES_ICUB
    else if (blockClassName == wbt::MinimumJerkTrajectoryGenerator::ClassName) {
        block = new wbt::MinimumJerkTrajectoryGenerator();
-    } 
+    }
+    else if (blockClassName == wbt::DiscreteFilter::ClassName) {


Please keep the indentation of the else coherent with the rest of the file

What do you mean? It looks already coherent but is morning for me as well :)

Oh sorry, now I see. Inside WBT_USES_ICUB is coherent, outside no. I fix both.

Mmh consideing that the last part of the file is all with defines, and the indentation has to be changes, imo the current status looks better. Compare:

... } else if (blockClassName == wbt::GetLimits::ClassName) { block = new wbt::GetLimits(); } else if (blockClassName == wbt::CentroidalMomentum::ClassName) { block = new wbt::CentroidalMomentum(); } #ifdef WBT_USES_ICUB else if (blockClassName == wbt::MinimumJerkTrajectoryGenerator::ClassName) { block = new wbt::MinimumJerkTrajectoryGenerator(); } else if (blockClassName == wbt::DiscreteFilter::ClassName) { block = new wbt::DiscreteFilter(); } #endif #ifdef WBT_USES_IPOPT else if (blockClassName == wbt::InverseKinematics::ClassName) { block = new wbt::InverseKinematics(); } #endif

with

... } else if (blockClassName == wbt::GetLimits::ClassName) { block = new wbt::GetLimits(); } else if (blockClassName == wbt::CentroidalMomentum::ClassName) { block = new wbt::CentroidalMomentum(); } #ifdef WBT_USES_ICUB else if (blockClassName == wbt::MinimumJerkTrajectoryGenerator::ClassName) { block = new wbt::MinimumJerkTrajectoryGenerator(); } else if (blockClassName == wbt::DiscreteFilter::ClassName) { block = new wbt::DiscreteFilter(); } #endif #ifdef WBT_USES_IPOPT else if (blockClassName == wbt::InverseKinematics::ClassName) { block = new wbt::InverseKinematics(); } #endif

But as you prefer.

I prefer the second one.

francesco-romano

The comments are related to the C++ only.
Before merging the matlab part should also be tested

francesco-romano · 2017-08-11T08:39:11Z

toolbox/include/DiscreteFilter.h

+private:
+    bool firstRun;
+    iCub::ctrl::IFilter* filter;
+    yarp::sig::Vector* num;


Why putting num and den in the .h file? They are only needed in configuration and not in real time.

francesco-romano · 2017-08-11T08:39:31Z

toolbox/src/DiscreteFilter.cpp

+
+DiscreteFilter::DiscreteFilter() : firstRun(true), filter(nullptr), inputSignalVector(nullptr)
+{
+    num = new Vector(0);


see comment in the .h

francesco-romano · 2017-08-11T08:42:45Z

toolbox/src/DiscreteFilter.cpp

+    unsigned inputSignalWidth = blockInfo->getInputPortWidth(INPUT_IDX_SIGNAL);
+
+    // Allocate the memory for the input data
+    if (firstRun) {


There is also another callback (initializeInitialConditions) which is called before the first output.

Furthermore, have you tried to ask for the port width in the initialize method?

In initialize() it returns the correct width. I'll add a private member for that and move the initialization into the initializeInitialConditions callback that I didn't know existed.

if initialize returns the correct width, I advise to use it instead of initializeInitialConditions

Ok thanks, should be ok now.

francesco-romano · 2017-08-11T08:43:16Z

toolbox/src/DiscreteFilter.cpp

+
+void DiscreteFilter::stringToYarpVector(const std::string str, Vector* v)
+{
+    assert(v != nullptr);


As the pointer should always be != null, I would use a reference, not a pointer

Changed the argument type to Vector&

diegoferigo · 2017-08-11T12:56:04Z

@francesco-romano
Ready for the final review

francesco-romano · 2017-08-11T13:03:28Z

toolbox/src/DiscreteFilter.cpp

+
+    // If the initial conditions for the output are not set, allocate a properly
+    // sized vector
+    if (*y0 == Vector(0)) {


Why not asking for the size of the vector and calling a resize on it, instead of allocated twice?

francesco-romano · 2017-08-11T13:04:06Z

toolbox/src/DiscreteFilter.cpp

+
+DiscreteFilter::DiscreteFilter() : filter(nullptr), inputSignalVector(nullptr)
+{
+    y0 = new Vector(0);


As the delete are done in the terminate, move the allocation in the corresponding method, and not in the constructor.
Here simply initialise to nullptr the pointers

francesco-romano · 2017-08-11T13:08:35Z

toolbox/src/DiscreteFilter.cpp

+    // zero-gain case
+    Filter* filter_c = dynamic_cast<Filter*>(filter);
+    if (filter_c != nullptr) {
+        filter_c->init(*y0, *u0);


I don't remember what init does, but is it ok to call with an empty vector? (i.e. u0.size() == 0)

With 4a29231 the vectors that hold the optional initial values are null if the parameters are not specified, and allocated to the correct size only when needed. If instead they are specified, their size is checked in the initialize method.

francesco-romano · 2017-08-17T12:41:59Z

toolbox/src/DiscreteFilter.cpp

-        y0 = new Vector(inputSignalWidth, 0.0);
+    if (y0 == nullptr) {
+        unsigned outputSignalWidth = blockInfo->getInputPortWidth(OUTPUT_IDX_SIGNAL);
+        y0                         = new Vector(outputSignalWidth, 0.0);


can you avoid to align the =? I don't like this kind of indentation

francesco-romano · 2017-08-17T12:43:10Z

A part from the inline comment the .cpp is ok.
As soon as I have time I will test the .mdl

francesco-romano · 2017-08-23T11:26:13Z

@diegoferigo
To "rebase" on top of devel, rebase the C++ files.
Regarding the mdl, save the new block in a temporary model, get the devel version of the library and then re-add the block

SetReferences subList parameter was erroneously converted to a boolean, while it is actually the index of the selected item in the Simulink mask dropdown. Being the index >=1 the boolean conversion resulted always in a true value. Fix robotology#57

Note: actually the std::cerr are warnings for the user, this is why they are not returned as an error. We have to find a smarter way to return the warning to the user.

This class contains and validates the configuration passed to every wbt::WBBlock object. ValidConfiguration is a Dependent property and it is set to 1 only if the basic checks currently implemented in other properties don't fail. From this state, it is possible to call getSimulinkParameters() in order to obtain the struct which is passed to the S-Functions and then parsed in C++. This class also contains utilities functions for obtainng a simple string representation of 1D vectors and 1D chars, required to serialize the data in the Config Block's mask. The deserialization is performed using eval().

Initial version of the library, which will become independent in the future. MxAnyType handles the C / C++ mxArray pointer that represents any generic data type in Matlab. It wraps most of the C APIs, and it has optional support of strict type checking.

wbt::Log is the new class that provides log support to the WBToolbox. It now supports both Errors and Warnings. For being more flexible, wbt::Log is a singleton. This approach simplifies dramatically the handling of warnings, and can be called from every method without the need of passing it though arguments. Since singletons cannot be deleted, wbt::Log state is reset in the early stage of the processing. [Squash] toolbox.cpp

Its functionality are implemented by the new generic WBBlock, which is a refactored version of WBIBlock that access robot resources in a lazy way. This approach allows avoiding the previous separation between WBIBlock and WBIModelBlock.

Also renamed Migration.md

WBToolboxLibrary.mdl: R2012A WBToolboxLibrary.slx: R2014B WBToolboxLibrary_repository: R2016B

Ref: robotology/idyntree#288

These messages are displayed in the console from which Matlab was launched

Kept as private variables only what is strictly required

WBToolboxLibrary.mdl: R2012A WBToolboxLibrary.slx: R2014B WBToolboxLibrary_repository.mdl: R2016B

Now it works as expected when the initial state vector is set but is not enabled

diegoferigo · 2017-12-12T09:29:43Z

For simplifying the test of the WB-Toolbox 3.0, I cherry picked these commits to #56.

Closing this PR.

diegoferigo self-assigned this Aug 10, 2017

diegoferigo added the type::enhancement label Aug 10, 2017

francesco-romano mentioned this pull request Aug 10, 2017

WB-Toolbox 3.0 #46

Closed

5 tasks

diegoferigo force-pushed the DiscreteFilterBlock branch 2 times, most recently from c27a8dd to 57a432d Compare August 10, 2017 09:58

traversaro reviewed Aug 10, 2017

View reviewed changes

diegoferigo force-pushed the DiscreteFilterBlock branch from d0cd476 to d138501 Compare August 10, 2017 16:24

diegoferigo changed the base branch from master to devel August 11, 2017 07:11

diegoferigo changed the title ~~[WIP] DiscreteFilter block~~ DiscreteFilter block Aug 11, 2017

diegoferigo requested a review from francesco-romano August 11, 2017 07:26

traversaro reviewed Aug 11, 2017

View reviewed changes

francesco-romano reviewed Aug 11, 2017

View reviewed changes

francesco-romano suggested changes Aug 11, 2017

View reviewed changes

diegoferigo force-pushed the DiscreteFilterBlock branch from 7e1d745 to e7f3da7 Compare August 11, 2017 12:55

francesco-romano suggested changes Aug 11, 2017

View reviewed changes

francesco-romano reviewed Aug 11, 2017

View reviewed changes

francesco-romano reviewed Aug 17, 2017

View reviewed changes

francesco-romano mentioned this pull request Aug 23, 2017

Add blocks for Filters #49

Closed

francesco-romano and others added 7 commits September 25, 2017 12:10

Migrated old DH conversion code from embedded to iDynTree

288d6e9

Temporary fix: included iostream

6716aaa

Note: actually the std::cerr are warnings for the user, this is why they are not returned as an error. We have to find a smarter way to return the warning to the user.

Removed WBIModelBlock

b4beb26

Its functionality are implemented by the new generic WBBlock, which is a refactored version of WBIBlock that access robot resources in a lazy way. This approach allows avoiding the previous separation between WBIBlock and WBIModelBlock.

diegoferigo added 2 commits December 1, 2017 07:57

Explicitly set the velocity representation in iDynTree

163ddeb

Fixed the generalized torques conversion of InverseDynamics block

98aafcc

diegoferigo force-pushed the DiscreteFilterBlock branch from 2dc2d8e to 22244c4 Compare December 6, 2017 13:14

diegoferigo changed the base branch from devel to WB3.0 December 6, 2017 13:14

diegoferigo mentioned this pull request Dec 6, 2017

WB-Toolbox 3.0 Release Notes #65

Closed

diegoferigo added 15 commits December 6, 2017 15:46

Updated README.md

5480fae

Added Migration How-To from 2.0 to 3.0

47f3b5a

Also renamed Migration.md

Included the support of a new DiscreteFilter Block

edf4a58

First working version of DiscreteFilter block with 1D I/O signal

73c1c4e

DiscreteFilter: Multidimesional signal support

79c0c57

Updated library

a49d7b9

WBToolboxLibrary.mdl: R2012A WBToolboxLibrary.slx: R2014B WBToolboxLibrary_repository: R2016B

Avoid problems on streams due to system's locale

ee56684

Ref: robotology/idyntree#288

Included support to console debug messages

5737e5e

These messages are displayed in the console from which Matlab was launched

Minor indentation change

0238299

DiscreteFilter: Included parameters for initializing input / output

077d986

Kept as private variables only what is strictly required

Updated library

b8ba0fe

WBToolboxLibrary.mdl: R2012A WBToolboxLibrary.slx: R2014B WBToolboxLibrary_repository.mdl: R2016B

DiscreteFilter: improved behavior of optional parameters

4791efe

Now it works as expected when the initial state vector is set but is not enabled

Reduced memory allocations

189df02

DiscreteFilter: uploaded test model

fafaaf1

Rebased DiscreteFilter block on WB3.0

580e430

diegoferigo force-pushed the DiscreteFilterBlock branch from 22244c4 to 580e430 Compare December 6, 2017 15:59

diegoferigo closed this Dec 12, 2017

diegoferigo added complexity::medium pr::resolution::rejected component::blocks component:library-mex component:simulink labels Feb 27, 2018

diegoferigo deleted the DiscreteFilterBlock branch May 11, 2018 13:31

diegoferigo mentioned this pull request Jan 3, 2019

Synchronisation problems between Gazebo and estimation threads #26

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

DiscreteFilter block #47

DiscreteFilter block #47

diegoferigo commented Aug 10, 2017 •

edited

Loading

traversaro commented Aug 10, 2017

traversaro Aug 10, 2017

diegoferigo Aug 10, 2017

traversaro Aug 10, 2017

diegoferigo Aug 10, 2017 •

edited

Loading

traversaro Aug 11, 2017 •

edited

Loading

diegoferigo Aug 11, 2017

francesco-romano Aug 11, 2017

diegoferigo Aug 11, 2017

diegoferigo Aug 11, 2017

diegoferigo Aug 11, 2017 •

edited

Loading

francesco-romano Aug 11, 2017

francesco-romano left a comment

francesco-romano Aug 11, 2017

diegoferigo Aug 11, 2017

francesco-romano Aug 11, 2017

diegoferigo Aug 11, 2017

francesco-romano Aug 11, 2017

diegoferigo Aug 11, 2017

francesco-romano Aug 11, 2017

diegoferigo Aug 11, 2017

francesco-romano Aug 11, 2017

diegoferigo Aug 11, 2017

diegoferigo commented Aug 11, 2017

francesco-romano Aug 11, 2017

francesco-romano Aug 11, 2017

francesco-romano Aug 11, 2017

diegoferigo Aug 11, 2017

francesco-romano Aug 17, 2017

francesco-romano commented Aug 17, 2017

francesco-romano commented Aug 23, 2017

diegoferigo commented Dec 12, 2017

DiscreteFilter block #47

DiscreteFilter block #47

Conversation

diegoferigo commented Aug 10, 2017 • edited Loading

Future TODOs:

traversaro commented Aug 10, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

diegoferigo Aug 10, 2017 • edited Loading

Choose a reason for hiding this comment

traversaro Aug 11, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

diegoferigo Aug 11, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

francesco-romano left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

diegoferigo commented Aug 11, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

francesco-romano commented Aug 17, 2017

francesco-romano commented Aug 23, 2017

diegoferigo commented Dec 12, 2017

diegoferigo commented Aug 10, 2017 •

edited

Loading

diegoferigo Aug 10, 2017 •

edited

Loading

traversaro Aug 11, 2017 •

edited

Loading

diegoferigo Aug 11, 2017 •

edited

Loading