17 #define LOGFILE "/tmp/fd_log.dat" 21 std::ofstream LogFile; \ 22 LogFile.open(LOGFILE, std::ofstream::app); \ 23 LogFile << x << std::endl; \ 28 #include <dynamic-graph/factory.h> 33 #include <Eigen/Dense> 38 #define ALL_INPUT_SIGNALS m_xSIN 40 #define ALL_OUTPUT_SIGNALS m_x_filteredSOUT << m_dxSOUT << m_ddxSOUT 45 using namespace Eigen;
53 "FilterDifferentiator");
78 "Control timestep [s].",
79 "Size of the input signal x",
80 "Numerator of the filter",
81 "Denominator of the filter")));
86 "Denominator of the filter")));
93 const Eigen::VectorXd &filter_numerator,
94 const Eigen::VectorXd &filter_denominator) {
98 new CausalFilter(timestep, xSize, filter_numerator, filter_denominator);
100 LOG(
"Filtering started with " 101 <<
"Numerator " << filter_numerator << std::endl
102 <<
"Denominator" << filter_denominator << std::endl);
107 const Eigen::VectorXd &filter_numerator,
108 const Eigen::VectorXd &filter_denominator) {
109 LOG(
"Filter switched with " 110 <<
"Numerator " << filter_numerator << std::endl
111 <<
"Denominator" << filter_denominator << std::endl
112 <<
"at time" << m_xSIN.getTime());
121 sotDEBUG(15) <<
"Compute x_dx inner signal " << iter << std::endl;
137 sotDEBUG(15) <<
"Compute x_filtered output signal " << iter << std::endl;
146 sotDEBUG(15) <<
"Compute dx output signal " << iter << std::endl;
155 sotDEBUG(15) <<
"Compute ddx output signal " << iter << std::endl;
164 os <<
"FilterDifferentiator " <<
getName() <<
":\n";
std::string docDirectGetter(const std::string &name, const std::string &type)
DirectGetter< E, T > * makeDirectGetter(E &entity, T *ptr, const std::string &docString)
CausalFilter * m_filter
polynomial-fitting filters
void signalRegistration(const SignalArray< int > &signals)
#define CONSTRUCT_SIGNAL_IN(name, type)
DEFINE_SIGNAL_OUT_FUNCTION(x_filtered, dynamicgraph::Vector)
void get_x_dx_ddx(const Eigen::VectorXd &base_x, Eigen::VectorXd &x_output_dx_ddx)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture")
std::string docCommandVoid4(const std::string &doc, const std::string &type1, const std::string &type2, const std::string &type3, const std::string &type4)
FilterDifferentiator EntityClassName
void switch_filter(const Eigen::VectorXd &filter_numerator, const Eigen::VectorXd &filter_denominator)
#define ALL_INPUT_SIGNALS
DEFINE_SIGNAL_INNER_FUNCTION(x_dx_ddx, dynamicgraph::Vector)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW FilterDifferentiator(const std::string &name)
#define CONSTRUCT_SIGNAL_OUT(name, type, dep)
#define CONSTRUCT_SIGNAL_INNER(name, type, dep)
Stopwatch & getProfiler()
CommandVoid4< E, T1, T2, T3, T4 > * makeCommandVoid4(E &entity, typename CommandVoid4< E, T1, T2, T3, T4 >::function_t function, const std::string &docString)
virtual void display(std::ostream &os) const
#define ALL_OUTPUT_SIGNALS
int m_x_size
sampling timestep of the input signal
void init(const double ×tep, const int &xSize, const Eigen::VectorXd &filter_numerator, const Eigen::VectorXd &filter_denominator)
const std::string & getName() const
std::string docCommandVoid2(const std::string &doc, const std::string &type1, const std::string &type2)
void switch_filter(const Eigen::VectorXd &filter_numerator, const Eigen::VectorXd &filter_denominator)
void addCommand(const std::string &name, command::Command *command)
void report_all(int precision=2, std::ostream &output=std::cout)
CommandVoid2< E, T1, T2 > * makeCommandVoid2(E &entity, boost::function< void(const T1 &, const T2 &)> function, const std::string &docString)