From c27a8dda90ecf14b1bca75f625a92d624352b7bc Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 10 Aug 2017 09:16:25 +0000 Subject: [PATCH] First working version of DiscreteFilter block with 1D I/O signal --- toolbox/include/DiscreteFilter.h | 44 ++++++ toolbox/src/DiscreteFilter.cpp | 222 +++++++++++++++++++++++++++++++ 2 files changed, 266 insertions(+) create mode 100644 toolbox/include/DiscreteFilter.h create mode 100644 toolbox/src/DiscreteFilter.cpp diff --git a/toolbox/include/DiscreteFilter.h b/toolbox/include/DiscreteFilter.h new file mode 100644 index 000000000..be47c7944 --- /dev/null +++ b/toolbox/include/DiscreteFilter.h @@ -0,0 +1,44 @@ +#include "Block.h" +#include + +#ifndef WBT_FILTER_H +#define WBT_FILTER_H + +namespace wbt { + class DiscreteFilter; +} // namespace wbt + +namespace iCub { + namespace ctrl { + class IFilter; + } +} // namespace iCub + +namespace yarp { + namespace sig { + class Vector; + } +} // namespace yarp + +class wbt::DiscreteFilter : public wbt::Block { +private: + iCub::ctrl::IFilter* filter; + yarp::sig::Vector* num; + yarp::sig::Vector* den; + + static void stringToYarpVector(const std::string s, yarp::sig::Vector* v); + +public: + static 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 terminate(BlockInformation* blockInfo, wbt::Error* error); + virtual bool output(BlockInformation* blockInfo, wbt::Error* error); +}; + +#endif // WBT_FILTER_H diff --git a/toolbox/src/DiscreteFilter.cpp b/toolbox/src/DiscreteFilter.cpp new file mode 100644 index 000000000..ce5496dcc --- /dev/null +++ b/toolbox/src/DiscreteFilter.cpp @@ -0,0 +1,222 @@ +#include "DiscreteFilter.h" +#include "BlockInformation.h" +#include "Error.h" +#include "ForwardKinematics.h" +#include "Signal.h" +#include +#include +#include +#include +#include +#include + +// 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 +// Inputs +#define INPUT_IDX_SIGNAL 0 +// Outputs +#define OUTPUT_IDX_SIGNAL 0 +// Other defines +#define SIGNAL_DYNAMIC_SIZE -1 + +using namespace wbt; + +std::string DiscreteFilter::ClassName = "DiscreteFilter"; + +DiscreteFilter::DiscreteFilter() : filter(nullptr), num(nullptr), den(nullptr) +{ + num = new yarp::sig::Vector(); + den = new yarp::sig::Vector(); +} + +unsigned DiscreteFilter::numberOfParameters() +{ + return 6; +} + +bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo, wbt::Error* error) +{ + // Memory allocation / Saving data not allowed here + + // Specify I/O + // =========== + + // INPUTS + // ------ + + // Number of input ports + int numberOfInputPorts = 1; + if (!blockInfo->setNumberOfInputPorts(numberOfInputPorts)) { + if (error) { + error->message = ClassName + " Failed to set input port number."; + } + return false; + } + + // Input port sizes + blockInfo->setInputPortVectorSize(INPUT_IDX_SIGNAL, SIGNAL_DYNAMIC_SIZE); + blockInfo->setInputPortType(INPUT_IDX_SIGNAL, PortDataTypeDouble); + + // OUTPUTS + // ------- + + // Number of output ports + int numberOfOutputPorts = 1; + if (!blockInfo->setNumberOfOuputPorts(numberOfOutputPorts)) { + if (error) { + error->message = ClassName + " Failed to set output port number."; + } + return false; + } + + // Output port sizes + blockInfo->setOutputPortVectorSize(OUTPUT_IDX_SIGNAL, SIGNAL_DYNAMIC_SIZE); + blockInfo->setOutputPortType(OUTPUT_IDX_SIGNAL, PortDataTypeDouble); + + return true; +} + +bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) +{ + // Variables for the filter parameters + std::string filter_type; + std::string num_coeff_str; + std::string den_coeff_str; + wbt::Data firstOrderLowPassFilter_fc; + wbt::Data firstOrderLowPassFilter_ts; + wbt::Data 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); + + // 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))) { + if (error) { + error->message = ClassName + " Failed to parse string parameters."; + } + return false; + } + + // Convert the strings from the Matlab syntax ("[1.0 2 3.33]") to yarp::sig::Vector + stringToYarpVector(num_coeff_str, num); + stringToYarpVector(den_coeff_str, den); + + // Create the filter object + // Default + if (filter_type == "Default") { + if (num->length() == 0 || den->length() == 0) { + if (error) { + error->message = ClassName + " (Default) Wrong coefficients size"; + } + return 1; + } + filter = new iCub::ctrl::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"; + } + return false; + } + filter = new iCub::ctrl::FirstOrderLowPassFilter(firstOrderLowPassFilter_fc.floatData(), + firstOrderLowPassFilter_ts.floatData()); + } + // MedianFilter + else if (filter_type == "MedianFilter") { + if (medianFilter_order.int32Data() == 0) { + if (error) { + error->message = ClassName + + " (MedianFilter) You need to specify the " + "filter order."; + } + return false; + } + filter = new iCub::ctrl::MedianFilter(medianFilter_order.int32Data()); + } + else { + if (error) error->message = ClassName + " Filter type not recognized."; + return false; + } + + return true; +} + +bool DiscreteFilter::terminate(BlockInformation* blockInfo, wbt::Error* error) +{ + if (filter) { + delete filter; + filter = nullptr; + } + + if (num) { + delete num; + num = nullptr; + } + + if (den) { + delete den; + den = nullptr; + } + + return true; +} + +bool DiscreteFilter::output(BlockInformation* blockInfo, wbt::Error* error) +{ + if (filter == nullptr) return false; + + // Get the input signal + Signal inputSignal = blockInfo->getInputPortSignal(INPUT_IDX_SIGNAL); + + unsigned index_element = 0; + yarp::sig::Vector inputSignalVector(1, inputSignal.get(index_element).doubleData()); + + // Filter the input signal + const yarp::sig::Vector& outputVector = filter->filt(inputSignalVector); + + // Forward the filtered signal to the output port + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_SIGNAL); + output.setBuffer(outputVector.data(), outputVector.length()); + + return true; +} + +void DiscreteFilter::stringToYarpVector(const std::string str, yarp::sig::Vector* v) +{ + assert(v != nullptr); + + std::string s = str; + + // Lambda expression used to remove "[]," carachters + // TODO: what about removing everything but digits and "."? + auto lambda_remove_chars = [](const char& c) { + if ((c == '[') || (c == ']') || c == ',') + return true; + else + return false; + }; + + // Apply the lambda expression the input parameters + 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); + float f; + + while (sstrm >> f) + v->push_back(f); +}