8 #include <dynamic-graph/factory.h>
9 #include <sot/core/debug.hh>
18 #define ALL_INPUT_SIGNALS m_xSIN
20 #define ALL_OUTPUT_SIGNALS m_x_filteredSOUT << m_dxSOUT << m_ddxSOUT
25 using namespace Eigen;
40 CONSTRUCT_SIGNAL_OUT(x_filtered,
dynamicgraph::Vector, m_x_dx_ddxSINNER),
41 CONSTRUCT_SIGNAL_OUT(dx,
dynamicgraph::Vector, m_x_dx_ddxSINNER),
42 CONSTRUCT_SIGNAL_OUT(ddx,
dynamicgraph::Vector, m_x_dx_ddxSINNER),
43 CONSTRUCT_SIGNAL_INNER(x_dx_ddx,
dynamicgraph::Vector, m_xSIN) {
47 addCommand(
"getTimestep", makeDirectGetter(*
this, &
m_dt, docDirectGetter(
"Control timestep [s ]",
"double")));
48 addCommand(
"getDelay",
49 makeDirectGetter(*
this, &
m_delay, docDirectGetter(
"Delay in the estimation of signal x",
"double")));
50 addCommand(
"getSize", makeDirectGetter(*
this, &
x_size, docDirectGetter(
"Size of the x signal",
"int")));
52 docCommandVoid4(
"Initialize the estimator.",
"Control timestep [s].",
53 "Size of the input signal x",
"Estimation delay for signal x",
54 "Polynomial order")));
61 assert(timestep > 0.0 &&
"Timestep should be > 0");
62 assert(delay >= 1.5 * timestep &&
"Estimation delay should be >= 1.5*timestep");
66 int winSizeEnc = (int)(2 * delay /
m_dt);
67 assert(winSizeEnc >= 3 &&
"Estimation-window's length should be >= 3");
71 else if (polyOrder == 2)
74 SEND_MSG(
"Only polynomial orders 1 and 2 allowed. Reinitialize the filter", MSG_TYPE_INFO);
87 sotDEBUG(15) <<
"Compute x_dx_ddx inner signal " << iter << std::endl;
90 const dynamicgraph::Vector& base_x = m_xSIN(iter);
94 m_filter->estimate(m_x_filter_std, m_x_std);
95 m_filter->getEstimateDerivative(m_dx_filter_std, 1);
96 m_filter->getEstimateDerivative(m_ddx_filter_std, 2);
99 if (s.size() != 3 * x_size) s.resize(3 * x_size);
100 for (
int i = 0; i < x_size; i++) s(i) = m_x_filter_std[i];
101 for (
int i = 0; i < x_size; i++) s(i + x_size) = m_dx_filter_std[i];
102 for (
int i = 0; i < x_size; i++) s(i + 2 * x_size) = m_ddx_filter_std[i];
113 sotDEBUG(15) <<
"Compute x_filtered output signal " << iter << std::endl;
115 const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter);
116 if (s.size() != x_size) s.resize(x_size);
117 s = x_dx_ddx.head(x_size);
122 sotDEBUG(15) <<
"Compute dx output signal " << iter << std::endl;
124 const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter);
125 if (s.size() != x_size) s.resize(x_size);
126 s = x_dx_ddx.segment(x_size, x_size);
131 sotDEBUG(15) <<
"Compute ddx output signal " << iter << std::endl;
133 const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter);
134 if (s.size() != x_size) s.resize(x_size);
135 s = x_dx_ddx.segment(2 * x_size, x_size);
140 os <<
"NumericalDifference " << getName() <<
":\n";
142 getProfiler().report_all(3, os);
143 }
catch (ExceptionSignal e) {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW NumericalDifference(const std::string &name)
PolyEstimator * m_filter
x signal
std::vector< double > m_ddx_filter_std
virtual void display(std::ostream &os) const
void init(const double ×tep, const int &sigSize, const double &delay, const int &polyOrder)
int x_size
delay introduced by the estimation
std::vector< double > m_x_filter_std
1st derivative
double m_delay
sampling timestep of the input signal
std::vector< double > m_dx_filter_std
2nd derivative
std::vector< double > m_x_std
filtered output
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
AdmittanceController EntityClassName
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
#define ALL_OUTPUT_SIGNALS
#define ALL_INPUT_SIGNALS
#define COPY_VECTOR_TO_ARRAY(src, dest)