diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index b4c538673..fa353fa7a 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -94,6 +94,7 @@ if (WBT_USES_ICUB) # # include_directories(SYSTEM ${iKin_INCLUDE_DIRS}) # endif() + configure_block(BLOCK_NAME "Discrete Filter" GROUP "Utilities" LIST_PREFIX WBT diff --git a/toolbox/include/DiscreteFilter.h b/toolbox/include/DiscreteFilter.h index 098ca3966..65915f3a1 100644 --- a/toolbox/include/DiscreteFilter.h +++ b/toolbox/include/DiscreteFilter.h @@ -1,6 +1,7 @@ #include "Block.h" #include #include +#include #ifndef WBT_FILTER_H #define WBT_FILTER_H @@ -24,25 +25,41 @@ namespace yarp { class wbt::DiscreteFilter : public wbt::Block { private: unsigned inputSignalWidth; - iCub::ctrl::IFilter* filter; - yarp::sig::Vector* y0; - yarp::sig::Vector* u0; - yarp::sig::Vector* inputSignalVector; + std::unique_ptr filter; + std::unique_ptr y0; + std::unique_ptr u0; + std::unique_ptr inputSignalVector; static void stringToYarpVector(const std::string s, yarp::sig::Vector& v); + // Parameters + static const unsigned PARAM_IDX_FLT_TYPE; + static const unsigned PARAM_IDX_NUMCOEFF; + static const unsigned PARAM_IDX_DENCOEFF; + static const unsigned PARAM_IDX_1LOWP_FC; + static const unsigned PARAM_IDX_1LOWP_TS; + static const unsigned PARAM_IDX_MD_ORDER; + static const unsigned PARAM_IDX_INIT_Y0; + static const unsigned PARAM_IDX_INIT_U0; + // Inputs + static const unsigned INPUT_IDX_SIGNAL; + // Outputs + static const unsigned OUTPUT_IDX_SIGNAL; + // Other defines + static const int SIGNAL_DYNAMIC_SIZE; + public: - static std::string ClassName; + static const std::string ClassName; DiscreteFilter(); - ~DiscreteFilter() = default; - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation* blockInfo, wbt::Error* error); - virtual bool initialize(BlockInformation* blockInfo, wbt::Error* error); - virtual bool initializeInitialConditions(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation* blockInfo, wbt::Error* error); - virtual bool output(BlockInformation* blockInfo, wbt::Error* error); + ~DiscreteFilter() override = default; + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool initializeInitialConditions(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif // WBT_FILTER_H diff --git a/toolbox/matlab/library/WBToolboxLibrary_repository.mdl b/toolbox/matlab/library/WBToolboxLibrary_repository.mdl index 0ae80c40b..1083107e5 100644 --- a/toolbox/matlab/library/WBToolboxLibrary_repository.mdl +++ b/toolbox/matlab/library/WBToolboxLibrary_repository.mdl @@ -61,7 +61,7 @@ Library { LoadSaveID "192" Extents [1557.0, 726.0] ZoomFactor [2.13] - Offset [0.0, 0.0] + Offset [-46.444835680751169, 21.953051643192453] } PropName "EditorsInfo" } @@ -97,9 +97,9 @@ Library { ModifiedByFormat "%" LastModifiedBy "dferigo" ModifiedDateFormat "%" - LastModifiedDate "Thu Nov 30 14:54:35 2017" - RTWModifiedTimeStamp 433954470 - ModelVersionFormat "1.%" + LastModifiedDate "Wed Dec 06 10:03:50 2017" + RTWModifiedTimeStamp 434455429 + ModelVersionFormat "1.%" ConfigurationManager "none" SampleTimeColors off SampleTimeAnnotations off @@ -1080,7 +1080,7 @@ Library { ShowPageBoundaries off ZoomFactor "343" ReportName "simulink-default.rpt" - SIDHighWatermark "1789" + SIDHighWatermark "1790" Block { BlockType SubSystem Name "Utilities" @@ -1573,6 +1573,217 @@ Library { } } } + Block { + BlockType S-Function + Name "DiscreteFilter" + SID "1790" + Ports [1, 1] + Position [125, 260, 185, 290] + ZOrder 96 + BackgroundColor "yellow" + FunctionName "WBToolbox" + Parameters "'DiscreteFilter',filterType,numCoeffs,denCoeffs,Fc,Ts,orderMedianFilter,y0,u0" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 47 + $ClassName "Simulink.Mask" + Type "Discrete Filter" + Description "This block wraps the Filter, FirstOrderLowPassFilter, and MedianFilter from iCub::ctrl." + Initialization "filterType = get_param(gcb, 'filterType');\nnumCoeffs = mat2str(numCoeffs);\ndenCoeffs = ma" + "t2str(denCoeffs);\n\ninitStatus = get_param(gcb, 'initStatus');\n\nif (strcmp(initStatus,'on'))\n y0 = mat2str" + "(y0);\n u0 = mat2str(u0);\nelse\n y0 = 'none';\n u0 = 'none';\nend\n\nclear initStatus" + Display "disp('Filter')" + Array { + Type "Simulink.MaskParameter" + Dimension 9 + Object { + $ObjectID 48 + Type "popup" + Array { + Type "Cell" + Dimension 3 + Cell "Generic" + Cell "FirstOrderLowPassFilter" + Cell "MedianFilter" + PropName "TypeOptions" + } + Name "filterType" + Prompt "Type of the filter" + Value "Generic" + Callback "% From: https://it.mathworks.com/help/simulink/slref/simulink.mask-class.html\nfilterType = get_param(gcb" + ", 'filterType');\ninitStatus = get_param(gcb, 'initStatus');\np = Simulink.Mask.get(gcbh);\nhowToCoeffs = p.getDialo" + "gControl('howToCoeffs');\n\n%set_param(gcb, 'initStatus','off');\nif (strcmp(initStatus,'on'))\n vis_init = 'on';" + "\nelse\n vis_init = 'off';\nend\n\nif(strcmp(filterType, 'Generic'))\n set_param(gcb, 'MaskVisibilities',{'on'" + ",'on','on','off','off','off','on',vis_init,vis_init});\n howToCoeffs.Visible = 'on';\nelseif(strcmp(filterType, '" + "FirstOrderLowPassFilter'))\n set_param(gcb, 'MaskVisibilities',{'on','off','off','on','on','off','on',vis_init,'o" + "ff'});\n howToCoeffs.Visible = 'off';\nelseif(strcmp(filterType, 'MedianFilter'))\n set_param(gcb, 'MaskVisibi" + "lities',{'on','off','off','off','off','on','on',vis_init,'off'});\n howToCoeffs.Visible = 'off';\nend\n\nclear fi" + "lterType initStatus p howToCoeffs vis_init;" + } + Object { + $ObjectID 49 + Type "edit" + Name "numCoeffs" + Prompt "Numerator Coefficients*" + Value "[0]" + } + Object { + $ObjectID 50 + Type "edit" + Name "denCoeffs" + Prompt "Denominator Coefficients*" + Value "[0]" + } + Object { + $ObjectID 51 + Type "edit" + Name "Fc" + Prompt "Cut Frequency (Hz)" + Value "0" + Visible "off" + } + Object { + $ObjectID 52 + Type "edit" + Name "Ts" + Prompt "Sampling time (s)" + Value "0" + Visible "off" + } + Object { + $ObjectID 53 + Type "edit" + Name "orderMedianFilter" + Prompt "Order" + Value "0" + Visible "off" + } + Object { + $ObjectID 54 + Type "checkbox" + Name "initStatus" + Prompt "Define initial conditions" + Value "off" + Callback "initStatus = get_param(gcb, 'initStatus');\nvisibilities = get_param(gcb, 'MaskVisibilities');\nfilterT" + "ype = get_param(gcb, 'filterType');\n\nif (strcmp(initStatus,'off'))\n visibilities{8} = 'off';\n visibiliti" + "es{9} = 'off';\nelseif (strcmp(initStatus,'on'))\n visibilities{8} = 'on';\n if (strcmp(filterType,'Generic'))" + "\n visibilities{9} = 'on';\n end\nend\n\nset_param(gcb, 'MaskVisibilities', visibilities);\n\nclear initSt" + "atus visibilities filterType;" + } + Object { + $ObjectID 55 + Type "edit" + Name "y0" + Prompt "Output y0" + Value "[0]" + Visible "off" + } + Object { + $ObjectID 56 + Type "edit" + Name "u0" + Prompt "Input u0" + Value "[0]" + Visible "off" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 57 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 58 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 59 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Control" + Dimension 8 + Object { + $ObjectID 60 + $ClassName "Simulink.dialog.parameter.Popup" + Name "filterType" + } + Object { + $ObjectID 61 + $ClassName "Simulink.dialog.parameter.Edit" + Name "numCoeffs" + } + Object { + $ObjectID 62 + $ClassName "Simulink.dialog.parameter.Edit" + Name "denCoeffs" + } + Object { + $ObjectID 63 + $ClassName "Simulink.dialog.Text" + Prompt "* The coefficients are ordered in increasing power of z^-1" + Name "howToCoeffs" + } + Object { + $ObjectID 64 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "Fc" + } + Object { + $ObjectID 65 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "Ts" + } + Object { + $ObjectID 66 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "orderMedianFilter" + } + Object { + $ObjectID 67 + $ClassName "Simulink.dialog.Group" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 3 + Object { + $ObjectID 68 + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "initStatus" + } + Object { + $ObjectID 69 + $ClassName "Simulink.dialog.parameter.Edit" + Name "y0" + } + Object { + $ObjectID 70 + $ClassName "Simulink.dialog.parameter.Edit" + Name "u0" + } + PropName "DialogControls" + } + Name "Container3" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + } Block { BlockType S-Function Name "DoFs converter" @@ -1587,7 +1798,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 47 + $ObjectID 71 $ClassName "Simulink.Mask" Type "DoFs converter" Description "This block converts from a DoFs serialization to a YARP serialization or viceversa depending on" @@ -1604,7 +1815,7 @@ Library { RunInitForIconRedraw "off" Object { $PropName "Parameters" - $ObjectID 48 + $ObjectID 72 $ClassName "Simulink.MaskParameter" Type "popup" Array { @@ -1622,11 +1833,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 49 + $ObjectID 73 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 50 + $ObjectID 74 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -1634,22 +1845,22 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 51 + $ObjectID 75 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 52 + $ObjectID 76 $ClassName "Simulink.dialog.parameter.Popup" Name "yarp2WBIOption" } Object { - $ObjectID 53 + $ObjectID 77 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 54 + $ObjectID 78 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -1683,7 +1894,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 55 + $ObjectID 79 $ClassName "Simulink.Mask" Type "Minimum Jerk Trajectory Generator" Description "This block wraps the minJerkTrajGen class from iCub::ctrl::minJerkTrajGen.\n\nThe Minimum Jerk " @@ -1706,7 +1917,7 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 56 + $ObjectID 80 Type "checkbox" Name "externalSettlingTime" Prompt "External Settling Time" @@ -1717,42 +1928,42 @@ Library { "SettlingTime" } Object { - $ObjectID 57 + $ObjectID 81 Type "edit" Name "settlingTime" Prompt "Settling Time" Value "3" } Object { - $ObjectID 58 + $ObjectID 82 Type "edit" Name "sampleTime" Prompt "Sample Time" Value "0.01" } Object { - $ObjectID 59 + $ObjectID 83 Type "checkbox" Name "resetOnSettlingTime" Prompt "Reset on Settling Time Changes" Value "off" } Object { - $ObjectID 60 + $ObjectID 84 Type "checkbox" Name "firstDerivatives" Prompt "Output First Derivative" Value "on" } Object { - $ObjectID 61 + $ObjectID 85 Type "checkbox" Name "secondDerivatives" Prompt "Output Second Derivative" Value "on" } Object { - $ObjectID 62 + $ObjectID 86 Type "checkbox" Name "explicitInitialValue" Prompt "Explicit Initial Value" @@ -1765,11 +1976,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 63 + $ObjectID 87 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 64 + $ObjectID 88 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -1777,38 +1988,38 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 65 + $ObjectID 89 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 66 + $ObjectID 90 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 67 + $ObjectID 91 Prompt "Trajectory Parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 4 Object { - $ObjectID 68 + $ObjectID 92 $ClassName "Simulink.dialog.parameter.CheckBox" Name "externalSettlingTime" } Object { - $ObjectID 69 + $ObjectID 93 $ClassName "Simulink.dialog.parameter.Edit" Name "settlingTime" } Object { - $ObjectID 70 + $ObjectID 94 $ClassName "Simulink.dialog.parameter.Edit" Name "sampleTime" } Object { - $ObjectID 71 + $ObjectID 95 $ClassName "Simulink.dialog.parameter.CheckBox" Name "resetOnSettlingTime" } @@ -1817,21 +2028,21 @@ Library { Name "Tab1" } Object { - $ObjectID 72 + $ObjectID 96 Prompt "Input/Output" Array { Type "Simulink.dialog.parameter.CheckBox" Dimension 3 Object { - $ObjectID 73 + $ObjectID 97 Name "firstDerivatives" } Object { - $ObjectID 74 + $ObjectID 98 Name "secondDerivatives" } Object { - $ObjectID 75 + $ObjectID 99 Name "explicitInitialValue" } PropName "DialogControls" @@ -1865,7 +2076,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 76 + $ObjectID 100 $ClassName "Simulink.Mask" Type "Real Time Synchronizer" Description "This block slows down the simulation trying to match the period specified \nas parameter (in se" @@ -1873,7 +2084,7 @@ Library { Display "disp('Real Time Synchronizer')" Object { $PropName "Parameters" - $ObjectID 77 + $ObjectID 101 $ClassName "Simulink.MaskParameter" Type "edit" Name "period" @@ -1898,7 +2109,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 78 + $ObjectID 102 $ClassName "Simulink.Mask" Type "Simulator Synchronizer" Description "This block synchronizes with the external simulation on a simulator \n(only Gazebo is supported" @@ -1908,21 +2119,21 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 79 + $ObjectID 103 Type "edit" Name "period" Prompt "Controller Period (in seconds)" Value "0.01" } Object { - $ObjectID 80 + $ObjectID 104 Type "edit" Name "serverPortName" Prompt "Server Port Name" Value "'/clock/rpc'" } Object { - $ObjectID 81 + $ObjectID 105 Type "edit" Name "clientPortName" Prompt "Client Port Name" @@ -1944,11 +2155,11 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 82 + $ObjectID 106 $ClassName "Simulink.Mask" Object { $PropName "Parameters" - $ObjectID 83 + $ObjectID 107 $ClassName "Simulink.MaskParameter" Type "edit" Name "tol" @@ -2165,7 +2376,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 84 + $ObjectID 108 $ClassName "Simulink.Mask" Type "YARP Clock" Description "This block outputs the current YARP Time.\nIn a nutshell, this block outputs the equivalent of " @@ -2189,7 +2400,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 85 + $ObjectID 109 $ClassName "Simulink.Mask" Type "YARP Read" Description "This block behaves as the command 'yarp read'. \nBy default it has the option 'Autoconnect' act" @@ -2207,35 +2418,35 @@ Library { Type "Simulink.MaskParameter" Dimension 6 Object { - $ObjectID 86 + $ObjectID 110 Type "edit" Name "portName" Prompt "Source Port Name" Value "'/portname'" } Object { - $ObjectID 87 + $ObjectID 111 Type "edit" Name "signalSize" Prompt "Port Size" Value "1" } Object { - $ObjectID 88 + $ObjectID 112 Type "checkbox" Name "blocking" Prompt "Blocking Read" Value "off" } Object { - $ObjectID 89 + $ObjectID 113 Type "checkbox" Name "timestamp" Prompt "Read Timestamp" Value "on" } Object { - $ObjectID 90 + $ObjectID 114 Type "checkbox" Name "autoconnect" Prompt "Autoconnect" @@ -2247,7 +2458,7 @@ Library { "t_string" } Object { - $ObjectID 91 + $ObjectID 115 Type "checkbox" Name "errorOnConnection" Prompt "Error on missing connection" @@ -2272,7 +2483,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 92 + $ObjectID 116 $ClassName "Simulink.Mask" Type "YARP Write" Description "This block behaves as the command 'yarp write'. \n\nBy default this block opens a port names as" @@ -2285,14 +2496,14 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 93 + $ObjectID 117 Type "edit" Name "portName" Prompt "Opened Port Name" Value "'/portname'" } Object { - $ObjectID 94 + $ObjectID 118 Type "checkbox" Name "autoconnect" Prompt "Autoconnect" @@ -2303,7 +2514,7 @@ Library { "';'off'});\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear autoconnect_val prompt_string" } Object { - $ObjectID 95 + $ObjectID 119 Type "checkbox" Name "errorOnConnection" Prompt "Error on missing connection" @@ -2326,7 +2537,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 96 + $ObjectID 120 $ClassName "Simulink.Mask" Type "Errors" Description "Computes two kinds of errors. The first is just the difference between x\nand y while the secon" @@ -2470,7 +2681,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 97 + $ObjectID 121 $ClassName "Simulink.Mask" Type "Holder" Description "This block holds the first input value during the simulation." @@ -2707,7 +2918,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 98 + $ObjectID 122 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyActuators.png'),'center')" } @@ -2739,7 +2950,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 99 + $ObjectID 123 $ClassName "Simulink.Mask" Type "Set Low Level PIDs" Description "This block sets the low level PID values for the robot actuators.\n\nThe PIDs can be stored in " @@ -2761,7 +2972,7 @@ Library { Type "Simulink.MaskParameter" Dimension 2 Object { - $ObjectID 100 + $ObjectID 124 Type "edit" Name "WBTPIDConfigObjectName" Prompt "WBTPIDConfig Object Name" @@ -2769,7 +2980,7 @@ Library { Tunable "off" } Object { - $ObjectID 101 + $ObjectID 125 Type "popup" Array { Type "Cell" @@ -2795,11 +3006,11 @@ Library { Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 102 + $ObjectID 126 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 103 + $ObjectID 127 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -2807,19 +3018,19 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 104 + $ObjectID 128 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.parameter.Control" Dimension 2 Object { - $ObjectID 105 + $ObjectID 129 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "WBTPIDConfigObjectName" } Object { - $ObjectID 106 + $ObjectID 130 $ClassName "Simulink.dialog.parameter.Popup" Name "pidType" } @@ -2828,10 +3039,10 @@ Library { Name "ParameterGroupVar" } Object { - $ObjectID 107 + $ObjectID 131 Object { $PropName "DialogControls" - $ObjectID 108 + $ObjectID 132 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" @@ -2888,7 +3099,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 109 + $ObjectID 133 $ClassName "Simulink.Mask" Type "Set References" Description "This block sets the references for the robot actuators.\nThe type of control mode is specified " @@ -2905,7 +3116,7 @@ Library { Type "Simulink.MaskParameter" Dimension 2 Object { - $ObjectID 110 + $ObjectID 134 Type "popup" Array { Type "Cell" @@ -2937,7 +3148,7 @@ Library { " maskStr" } Object { - $ObjectID 111 + $ObjectID 135 Type "edit" Name "refSpeed" Prompt "Reference Speed" @@ -2949,11 +3160,11 @@ Library { Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 112 + $ObjectID 136 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 113 + $ObjectID 137 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -2961,18 +3172,18 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 114 + $ObjectID 138 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.parameter.Control" Dimension 2 Object { - $ObjectID 115 + $ObjectID 139 $ClassName "Simulink.dialog.parameter.Popup" Name "controlType" } Object { - $ObjectID 116 + $ObjectID 140 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "refSpeed" @@ -2982,10 +3193,10 @@ Library { Name "ParameterGroupVar" } Object { - $ObjectID 117 + $ObjectID 141 Object { $PropName "DialogControls" - $ObjectID 118 + $ObjectID 142 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" @@ -3060,7 +3271,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 119 + $ObjectID 143 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyModel.png'),'center')" } @@ -3092,7 +3303,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 120 + $ObjectID 144 $ClassName "Simulink.Mask" Display "image(imread('Dynamics.png'))" } @@ -3123,7 +3334,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 121 + $ObjectID 145 $ClassName "Simulink.Mask" Type "Centroidal Momentum" Description "This block computed the 6 element centroidal momentum, as defined in:\n\"Centroidal dynamics of a huma" @@ -3139,24 +3350,24 @@ Library { RunInitForIconRedraw "on" Object { $PropName "DialogControls" - $ObjectID 122 + $ObjectID 146 $ClassName "Simulink.dialog.Group" Prompt "%" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 123 + $ObjectID 147 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } Object { - $ObjectID 124 + $ObjectID 148 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 125 + $ObjectID 149 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3292,7 +3503,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 126 + $ObjectID 150 $ClassName "Simulink.Mask" Type "Get Generalized Bias Forces" Description "This block retrieves the generalied bias forces of the robot.\n\nAssuming DoFs is the number of degree" @@ -3307,24 +3518,24 @@ Library { RunInitForIconRedraw "on" Object { $PropName "DialogControls" - $ObjectID 127 + $ObjectID 151 $ClassName "Simulink.dialog.Group" Prompt "%" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 128 + $ObjectID 152 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } Object { - $ObjectID 129 + $ObjectID 153 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 130 + $ObjectID 154 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3505,7 +3716,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 131 + $ObjectID 155 $ClassName "Simulink.Mask" Type "Gravity bias" Description "This block compute the generalized bias forces due to the gravity\n\nAssuming DoFs is the number of de" @@ -3518,24 +3729,24 @@ Library { RunInitForIconRedraw "off" Object { $PropName "DialogControls" - $ObjectID 132 + $ObjectID 156 $ClassName "Simulink.dialog.Group" Prompt "%" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 133 + $ObjectID 157 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } Object { - $ObjectID 134 + $ObjectID 158 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 135 + $ObjectID 159 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3707,7 +3918,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 136 + $ObjectID 160 $ClassName "Simulink.Mask" Type "Inverse Dynamics" Description "This block compute the inverse dynamics of the robot.\n\nAssuming DoFs is the number of degrees of fre" @@ -3727,24 +3938,24 @@ Library { RunInitForIconRedraw "on" Object { $PropName "DialogControls" - $ObjectID 137 + $ObjectID 161 $ClassName "Simulink.dialog.Group" Prompt "%" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 138 + $ObjectID 162 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } Object { - $ObjectID 139 + $ObjectID 163 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 140 + $ObjectID 164 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3772,7 +3983,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 141 + $ObjectID 165 $ClassName "Simulink.Mask" Type "Mass Matrix" Description "This block retrieves the robot mass matrix.\n\nAssuming DoFs is the number of degrees of freedom of th" @@ -3785,24 +3996,24 @@ Library { RunInitForIconRedraw "on" Object { $PropName "DialogControls" - $ObjectID 142 + $ObjectID 166 $ClassName "Simulink.dialog.Group" Prompt "%" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 143 + $ObjectID 167 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } Object { - $ObjectID 144 + $ObjectID 168 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 145 + $ObjectID 169 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3911,7 +4122,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 146 + $ObjectID 170 $ClassName "Simulink.Mask" Display "image(imread('jacobian.png'))" } @@ -3942,7 +4153,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 147 + $ObjectID 171 $ClassName "Simulink.Mask" Type "DotJ Nu" Description "This block computes the product between the time derivative of the Jacobian of the specified frame and" @@ -3963,7 +4174,7 @@ Library { RunInitForIconRedraw "on" Object { $PropName "Parameters" - $ObjectID 148 + $ObjectID 172 $ClassName "Simulink.MaskParameter" Type "edit" Name "frameName" @@ -3974,11 +4185,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 149 + $ObjectID 173 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 150 + $ObjectID 174 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -3986,22 +4197,22 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 151 + $ObjectID 175 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 152 + $ObjectID 176 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } Object { - $ObjectID 153 + $ObjectID 177 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 154 + $ObjectID 178 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -4139,7 +4350,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 155 + $ObjectID 179 $ClassName "Simulink.Mask" Type "Jacobian" Description "This block retrieves the Jacobian of the specified frame.\n\nAssuming DoFs is the number of degrees of" @@ -4156,7 +4367,7 @@ Library { RunInitForIconRedraw "on" Object { $PropName "Parameters" - $ObjectID 156 + $ObjectID 180 $ClassName "Simulink.MaskParameter" Type "edit" Name "frameName" @@ -4167,11 +4378,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 157 + $ObjectID 181 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 158 + $ObjectID 182 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4179,22 +4390,22 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 159 + $ObjectID 183 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 160 + $ObjectID 184 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } Object { - $ObjectID 161 + $ObjectID 185 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 162 + $ObjectID 186 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -4305,7 +4516,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 163 + $ObjectID 187 $ClassName "Simulink.Mask" Display "image(imread('forwardKinematics.png'))" } @@ -4336,7 +4547,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 164 + $ObjectID 188 $ClassName "Simulink.Mask" Type "Forward Kinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -4353,7 +4564,7 @@ Library { "', 2, 'Joint configuration')\n\nclear escapedFrameName;" Object { $PropName "Parameters" - $ObjectID 165 + $ObjectID 189 $ClassName "Simulink.MaskParameter" Type "edit" Name "frameName" @@ -4364,11 +4575,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 166 + $ObjectID 190 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 167 + $ObjectID 191 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4376,22 +4587,22 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 168 + $ObjectID 192 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 169 + $ObjectID 193 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } Object { - $ObjectID 170 + $ObjectID 194 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 171 + $ObjectID 195 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -4504,7 +4715,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 172 + $ObjectID 196 $ClassName "Simulink.Mask" Type "Forward Kinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -4528,21 +4739,21 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 173 + $ObjectID 197 Type "edit" Name "baseFrame" Prompt "Base Frame" Value "'root_link'" } Object { - $ObjectID 174 + $ObjectID 198 Type "edit" Name "endEffFrame" Prompt "End Effector frame" Value "'l_sole'" } Object { - $ObjectID 175 + $ObjectID 199 Type "popup" Array { Type "Cell" @@ -4556,7 +4767,7 @@ Library { Value "Full Constraint (Position and Orientation)" } Object { - $ObjectID 176 + $ObjectID 200 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -4564,7 +4775,7 @@ Library { Tunable "off" } Object { - $ObjectID 177 + $ObjectID 201 Type "edit" Name "localName" Prompt "Model Name" @@ -4572,7 +4783,7 @@ Library { Tunable "off" } Object { - $ObjectID 178 + $ObjectID 202 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -4580,7 +4791,7 @@ Library { Tunable "off" } Object { - $ObjectID 179 + $ObjectID 203 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -4593,11 +4804,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 180 + $ObjectID 204 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 181 + $ObjectID 205 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4605,34 +4816,34 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 182 + $ObjectID 206 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 183 + $ObjectID 207 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 184 + $ObjectID 208 Prompt "Block parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 3 Object { - $ObjectID 185 + $ObjectID 209 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "baseFrame" } Object { - $ObjectID 186 + $ObjectID 210 $ClassName "Simulink.dialog.parameter.Edit" Name "endEffFrame" } Object { - $ObjectID 187 + $ObjectID 211 $ClassName "Simulink.dialog.parameter.Popup" Name "optOption" } @@ -4641,25 +4852,25 @@ Library { Name "Container8" } Object { - $ObjectID 188 + $ObjectID 212 Prompt "WBI parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 189 + $ObjectID 213 Name "robotName" } Object { - $ObjectID 190 + $ObjectID 214 Name "localName" } Object { - $ObjectID 191 + $ObjectID 215 Name "wbiFile" } Object { - $ObjectID 192 + $ObjectID 216 Name "wbiList" } PropName "DialogControls" @@ -4800,7 +5011,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 193 + $ObjectID 217 $ClassName "Simulink.Mask" Type "Forward Kinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -4826,14 +5037,14 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 194 + $ObjectID 218 Type "edit" Name "solverName" Prompt "Solver Name" Value "'/cartesianSolver'" } Object { - $ObjectID 195 + $ObjectID 219 Type "edit" Name "dofs" Prompt "#Dofs" @@ -4841,7 +5052,7 @@ Library { Tunable "off" } Object { - $ObjectID 196 + $ObjectID 220 Type "popup" Array { Type "Cell" @@ -4860,11 +5071,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 197 + $ObjectID 221 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 198 + $ObjectID 222 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4872,33 +5083,33 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 199 + $ObjectID 223 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 200 + $ObjectID 224 $ClassName "Simulink.dialog.TabContainer" Object { $PropName "DialogControls" - $ObjectID 201 + $ObjectID 225 $ClassName "Simulink.dialog.Tab" Prompt "Block parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 3 Object { - $ObjectID 202 + $ObjectID 226 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "solverName" } Object { - $ObjectID 203 + $ObjectID 227 $ClassName "Simulink.dialog.parameter.Edit" Name "dofs" } Object { - $ObjectID 204 + $ObjectID 228 $ClassName "Simulink.dialog.parameter.Popup" Name "optOption" } @@ -5007,7 +5218,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 205 + $ObjectID 229 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyStates.png'),'center');" } @@ -5039,7 +5250,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 206 + $ObjectID 230 $ClassName "Simulink.Mask" Type "Get Limits" Description "This block provides the joint limits gathering data from either the Robot's Control Board or UR" @@ -5063,7 +5274,7 @@ Library { Type "Simulink.MaskParameter" Dimension 2 Object { - $ObjectID 207 + $ObjectID 231 Type "popup" Array { Type "Cell" @@ -5083,7 +5294,7 @@ Library { "imit blockParameters limitsTypeBlockParam" } Object { - $ObjectID 208 + $ObjectID 232 Type "popup" Array { Type "Cell" @@ -5108,11 +5319,11 @@ Library { Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 209 + $ObjectID 233 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 210 + $ObjectID 234 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5120,17 +5331,17 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 211 + $ObjectID 235 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.parameter.Popup" Dimension 2 Object { - $ObjectID 212 + $ObjectID 236 Name "limitsSource" } Object { - $ObjectID 213 + $ObjectID 237 Name "limitsType" } PropName "DialogControls" @@ -5138,10 +5349,10 @@ Library { Name "ParameterGroupVar" } Object { - $ObjectID 214 + $ObjectID 238 Object { $PropName "DialogControls" - $ObjectID 215 + $ObjectID 239 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" @@ -5230,7 +5441,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 216 + $ObjectID 240 $ClassName "Simulink.Mask" Type "Get Measurement" Description "This block gathers the measurement of the specified type.\n\nAssuming DoFs is the number of deg" @@ -5243,7 +5454,7 @@ Library { RunInitForIconRedraw "off" Object { $PropName "Parameters" - $ObjectID 217 + $ObjectID 241 $ClassName "Simulink.MaskParameter" Type "popup" Array { @@ -5270,11 +5481,11 @@ Library { Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 218 + $ObjectID 242 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 219 + $ObjectID 243 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5282,21 +5493,21 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 220 + $ObjectID 244 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 221 + $ObjectID 245 $ClassName "Simulink.dialog.parameter.Popup" Name "measuredType" } Name "ParameterGroupVar" } Object { - $ObjectID 222 + $ObjectID 246 Object { $PropName "DialogControls" - $ObjectID 223 + $ObjectID 247 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" diff --git a/toolbox/matlab/library/exported/WBToolboxLibrary.slx b/toolbox/matlab/library/exported/WBToolboxLibrary.slx index d2f615da6..da72a0ac4 100644 Binary files a/toolbox/matlab/library/exported/WBToolboxLibrary.slx and b/toolbox/matlab/library/exported/WBToolboxLibrary.slx differ diff --git a/toolbox/src/DiscreteFilter.cpp b/toolbox/src/DiscreteFilter.cpp index be74d7c70..674501a50 100644 --- a/toolbox/src/DiscreteFilter.cpp +++ b/toolbox/src/DiscreteFilter.cpp @@ -1,7 +1,6 @@ #include "DiscreteFilter.h" #include "BlockInformation.h" -#include "Error.h" -#include "ForwardKinematics.h" +#include "Log.h" #include "Signal.h" #include #include @@ -10,40 +9,38 @@ #include #include +using namespace wbt; +using namespace iCub::ctrl; +using namespace yarp::sig; + +const std::string DiscreteFilter::ClassName = "DiscreteFilter"; + // Parameters -#define PARAM_IDX_FLT_TYPE 1 // ::Filter type -#define PARAM_IDX_NUMCOEFF 2 // ::Filter numerator coefficients -#define PARAM_IDX_DENCOEFF 3 // ::Filter denominator coefficients -#define PARAM_IDX_1LOWP_FC 4 // ::FirstOrderLowPassFilter cut frequency -#define PARAM_IDX_1LOWP_TS 5 // ::FirstOrderLowPassFilter sampling time -#define PARAM_IDX_MD_ORDER 6 // ::MedianFilter order -#define PARAM_IDX_INIT_Y0 7 // Output initialization -#define PARAM_IDX_INIT_U0 8 // ::Filter input initialization +const unsigned DiscreteFilter::PARAM_IDX_FLT_TYPE = 1; // ::Filter type +const unsigned DiscreteFilter::PARAM_IDX_NUMCOEFF = 2; // ::Filter numerator coefficients +const unsigned DiscreteFilter::PARAM_IDX_DENCOEFF = 3; // ::Filter denominator coefficients +const unsigned DiscreteFilter::PARAM_IDX_1LOWP_FC = 4; // ::FirstOrderLowPassFilter cut frequency +const unsigned DiscreteFilter::PARAM_IDX_1LOWP_TS = 5; // ::FirstOrderLowPassFilter sampling time +const unsigned DiscreteFilter::PARAM_IDX_MD_ORDER = 6; // ::MedianFilter order +const unsigned DiscreteFilter::PARAM_IDX_INIT_Y0 = 7; // Output initialization +const unsigned DiscreteFilter::PARAM_IDX_INIT_U0 = 8; // ::Filter input initialization // Inputs -#define INPUT_IDX_SIGNAL 0 +const unsigned DiscreteFilter::INPUT_IDX_SIGNAL = 0; // Outputs -#define OUTPUT_IDX_SIGNAL 0 +const unsigned DiscreteFilter::OUTPUT_IDX_SIGNAL = 0; // Other defines -#define SIGNAL_DYNAMIC_SIZE -1 - -using namespace wbt; -using namespace iCub::ctrl; -using namespace yarp::sig; - -std::string DiscreteFilter::ClassName = "DiscreteFilter"; +const int DiscreteFilter::SIGNAL_DYNAMIC_SIZE = -1; DiscreteFilter::DiscreteFilter() - : filter(nullptr), y0(nullptr), u0(nullptr), inputSignalVector(nullptr) -{ -} +{} unsigned DiscreteFilter::numberOfParameters() { return 8; } -bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo, wbt::Error* error) +bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo) { // Memory allocation / Saving data not allowed here @@ -56,9 +53,7 @@ bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo, wbt::Err // Number of input ports int numberOfInputPorts = 1; if (!blockInfo->setNumberOfInputPorts(numberOfInputPorts)) { - if (error) { - error->message = ClassName + " Failed to set input port number."; - } + Log::getSingleton().error("Failed to set input port number."); return false; } @@ -71,10 +66,8 @@ bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo, wbt::Err // Number of output ports int numberOfOutputPorts = 1; - if (!blockInfo->setNumberOfOuputPorts(numberOfOutputPorts)) { - if (error) { - error->message = ClassName + " Failed to set output port number."; - } + if (!blockInfo->setNumberOfOutputPorts(numberOfOutputPorts)) { + Log::getSingleton().error("Failed to set output port number."); return false; } @@ -85,7 +78,7 @@ bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo, wbt::Err return true; } -bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) +bool DiscreteFilter::initialize(const BlockInformation* blockInfo) { // Handle the parameters // ===================== @@ -96,24 +89,29 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) std::string den_coeff_str; std::string y0_str; std::string u0_str; - wbt::Data firstOrderLowPassFilter_fc; - wbt::Data firstOrderLowPassFilter_ts; - wbt::Data medianFilter_order; + double firstOrderLowPassFilter_fc; + double firstOrderLowPassFilter_ts; + double medianFilter_order; // Get the scalar parameters - firstOrderLowPassFilter_fc = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_FC); - firstOrderLowPassFilter_ts = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_TS); - medianFilter_order = blockInfo->getScalarParameterAtIndex(PARAM_IDX_MD_ORDER); + bool ok = true; + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_FC, + firstOrderLowPassFilter_fc); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_TS, + firstOrderLowPassFilter_ts); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_MD_ORDER, + medianFilter_order); // Get the string parameter - if (!(blockInfo->getStringParameterAtIndex(PARAM_IDX_FLT_TYPE, filter_type) - && blockInfo->getStringParameterAtIndex(PARAM_IDX_NUMCOEFF, num_coeff_str) - && blockInfo->getStringParameterAtIndex(PARAM_IDX_DENCOEFF, den_coeff_str) - && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_Y0, y0_str) - && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_U0, u0_str))) { - if (error) { - error->message = ClassName + " Failed to parse string parameters."; - } + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_FLT_TYPE, filter_type); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_NUMCOEFF, num_coeff_str); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_DENCOEFF, den_coeff_str); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_Y0, y0_str); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_U0, u0_str); + + + if (!ok) { + Log::getSingleton().error("Failed to parse parameters."); return false; } @@ -127,13 +125,13 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) unsigned y0Width, u0Width; if (y0_str != "none") { - y0 = new Vector(0); + y0 = std::unique_ptr(new Vector(0)); stringToYarpVector(y0_str, *y0); - y0Width = y0->length(); + y0Width = y0->length(); } if (u0_str != "none") { - u0 = new Vector(0); + u0 = std::unique_ptr(new Vector(0)); stringToYarpVector(u0_str, *u0); u0Width = u0->length(); } @@ -145,43 +143,35 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) // ------- if (filter_type == "Generic") { if (num.length() == 0 || den.length() == 0) { - if (error) { - error->message = ClassName + " (Generic) Wrong coefficients size"; - } - return 1; + Log::getSingleton().error("(Generic) Wrong coefficients size."); + return false; } - filter = new Filter(num, den); + filter = std::unique_ptr(new Filter(num, den)); } // FirstOrderLowPassFilter // ----------------------- else if (filter_type == "FirstOrderLowPassFilter") { - if (firstOrderLowPassFilter_fc.floatData() == 0 - || firstOrderLowPassFilter_ts.floatData() == 0) { - if (error) { - error->message = ClassName - + " (FirstOrderLowPassFilter) You need to " - "specify Fc and Ts"; - } + if (firstOrderLowPassFilter_fc == 0 || firstOrderLowPassFilter_ts == 0) { + Log::getSingleton().error("(FirstOrderLowPassFilter) You need to " + "specify Fc and Ts."); return false; } - filter = new FirstOrderLowPassFilter(firstOrderLowPassFilter_fc.floatData(), - firstOrderLowPassFilter_ts.floatData()); + filter = std::unique_ptr( + new FirstOrderLowPassFilter(firstOrderLowPassFilter_fc, + firstOrderLowPassFilter_ts)); } // MedianFilter // ------------ else if (filter_type == "MedianFilter") { - if (medianFilter_order.int32Data() == 0) { - if (error) { - error->message = ClassName - + " (MedianFilter) You need to specify the " - "filter order."; - } + if (static_cast(medianFilter_order) == 0) { + Log::getSingleton().error("(MedianFilter) You need to specify the " + "filter order."); return false; } - filter = new MedianFilter(medianFilter_order.int32Data()); + filter = std::unique_ptr(new MedianFilter(static_cast(medianFilter_order))); } else { - if (error) error->message = ClassName + " Filter type not recognized."; + Log::getSingleton().error("Filter type not recognized."); return false; } @@ -195,54 +185,27 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) unsigned outputSignalWidth = blockInfo->getInputPortWidth(OUTPUT_IDX_SIGNAL); if ((y0 != nullptr) && (y0Width != outputSignalWidth)) { - if (error) { - error->message = ClassName + " y0 and output signal sizes don't match"; - } + Log::getSingleton().error("y0 and output signal sizes don't match."); return false; } if ((u0 != nullptr) && (filter_type == "Generic") && (u0Width != inputSignalWidth)) { - if (error) { - error->message = ClassName + " (Generic) u0 and input signal sizes don't match"; - } + Log::getSingleton().error("(Generic) u0 and input signal sizes don't match."); return false; } // Allocate the input signal - inputSignalVector = new Vector(inputSignalWidth, 0.0); + inputSignalVector = std::unique_ptr(new Vector(inputSignalWidth, 0.0)); return true; } -bool DiscreteFilter::terminate(BlockInformation* blockInfo, wbt::Error* error) +bool DiscreteFilter::terminate(const BlockInformation* blockInfo) { - // Deallocate all the memory - // ------------------------- - - if (filter) { - delete filter; - filter = nullptr; - } - - if (inputSignalVector) { - delete inputSignalVector; - inputSignalVector = nullptr; - } - - if (y0) { - delete y0; - y0 = nullptr; - } - - if (u0) { - delete u0; - u0 = nullptr; - } - return true; } -bool DiscreteFilter::initializeInitialConditions(BlockInformation* blockInfo, wbt::Error* error) +bool DiscreteFilter::initializeInitialConditions(const BlockInformation* blockInfo) { // Reminder: this function is called when, during runtime, a block is disabled // and enabled again. The method ::initialize instead is called just one time. @@ -251,17 +214,17 @@ bool DiscreteFilter::initializeInitialConditions(BlockInformation* blockInfo, wb // sized vector if (y0 == nullptr) { unsigned outputSignalWidth = blockInfo->getInputPortWidth(OUTPUT_IDX_SIGNAL); - y0 = new Vector(outputSignalWidth, 0.0); + y0 = std::unique_ptr(new Vector(outputSignalWidth, 0.0)); } if (u0 == nullptr) { - u0 = new Vector(inputSignalWidth, 0.0); + u0 = std::unique_ptr(new Vector(inputSignalWidth, 0.0)); } // Initialize the filter. This is required because if the signal is not 1D, // the default filter constructor initialize a wrongly sized y0 // Moreover, the Filter class has a different constructor that handles the // zero-gain case - Filter* filter_c = dynamic_cast(filter); + Filter* filter_c = dynamic_cast(filter.get()); if (filter_c != nullptr) { filter_c->init(*y0, *u0); } @@ -272,7 +235,7 @@ bool DiscreteFilter::initializeInitialConditions(BlockInformation* blockInfo, wb return true; } -bool DiscreteFilter::output(BlockInformation* blockInfo, wbt::Error* error) +bool DiscreteFilter::output(const BlockInformation* blockInfo) { if (filter == nullptr) return false; @@ -282,7 +245,7 @@ bool DiscreteFilter::output(BlockInformation* blockInfo, wbt::Error* error) // Copy the Signal to the data structure that the filter wants for (unsigned i = 0; i < inputSignalWidth; ++i) { - (*inputSignalVector)[i] = inputSignal.get(i).doubleData(); + (*inputSignalVector)[i] = inputSignal.get(i); } // Filter the current component of the input signal