1 |
|
|
/* |
2 |
|
|
* Copyright 2014-2017, Andrea Del Prete, Rohan Budhiraja LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#include <dynamic-graph/factory.h> |
7 |
|
|
|
8 |
|
|
#include <Eigen/Dense> |
9 |
|
|
#include <sot/core/debug.hh> |
10 |
|
|
#include <sot/torque_control/commands-helper.hh> |
11 |
|
|
#include <sot/torque_control/motor-model.hh> |
12 |
|
|
#include <sot/torque_control/numerical-difference.hh> |
13 |
|
|
|
14 |
|
|
namespace dynamicgraph { |
15 |
|
|
namespace sot { |
16 |
|
|
namespace torque_control { |
17 |
|
|
|
18 |
|
|
#define ALL_INPUT_SIGNALS m_xSIN |
19 |
|
|
|
20 |
|
|
#define ALL_OUTPUT_SIGNALS m_x_filteredSOUT << m_dxSOUT << m_ddxSOUT |
21 |
|
|
|
22 |
|
|
namespace dynamicgraph = ::dynamicgraph; |
23 |
|
|
using namespace dynamicgraph; |
24 |
|
|
using namespace dynamicgraph::command; |
25 |
|
|
using namespace Eigen; |
26 |
|
|
|
27 |
|
|
/// Define EntityClassName here rather than in the header file |
28 |
|
|
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. |
29 |
|
|
typedef NumericalDifference EntityClassName; |
30 |
|
|
|
31 |
|
|
/* --- DG FACTORY ------------------------------------------------------- */ |
32 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NumericalDifference, "NumericalDifference"); |
33 |
|
|
|
34 |
|
|
/* --- CONSTRUCTION ----------------------------------------------------- */ |
35 |
|
|
/* --- CONSTRUCTION ----------------------------------------------------- */ |
36 |
|
|
/* --- CONSTRUCTION ----------------------------------------------------- */ |
37 |
|
|
NumericalDifference::NumericalDifference(const std::string& name) |
38 |
|
|
: Entity(name), |
39 |
|
|
CONSTRUCT_SIGNAL_IN(x, dynamicgraph::Vector), |
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) { |
44 |
|
|
Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS); |
45 |
|
|
|
46 |
|
|
/* Commands. */ |
47 |
|
|
addCommand( |
48 |
|
|
"getTimestep", |
49 |
|
|
makeDirectGetter(*this, &m_dt, |
50 |
|
|
docDirectGetter("Control timestep [s ]", "double"))); |
51 |
|
|
addCommand( |
52 |
|
|
"getDelay", |
53 |
|
|
makeDirectGetter( |
54 |
|
|
*this, &m_delay, |
55 |
|
|
docDirectGetter("Delay in the estimation of signal x", "double"))); |
56 |
|
|
addCommand("getSize", |
57 |
|
|
makeDirectGetter(*this, &x_size, |
58 |
|
|
docDirectGetter("Size of the x signal", "int"))); |
59 |
|
|
addCommand("init", |
60 |
|
|
makeCommandVoid4(*this, &NumericalDifference::init, |
61 |
|
|
docCommandVoid4("Initialize the estimator.", |
62 |
|
|
"Control timestep [s].", |
63 |
|
|
"Size of the input signal x", |
64 |
|
|
"Estimation delay for signal x", |
65 |
|
|
"Polynomial order"))); |
66 |
|
|
} |
67 |
|
|
|
68 |
|
|
/* --- COMMANDS ---------------------------------------------------------- */ |
69 |
|
|
/* --- COMMANDS ---------------------------------------------------------- */ |
70 |
|
|
/* --- COMMANDS ---------------------------------------------------------- */ |
71 |
|
|
void NumericalDifference::init(const double& timestep, const int& xSize, |
72 |
|
|
const double& delay, const int& polyOrder) { |
73 |
|
|
assert(timestep > 0.0 && "Timestep should be > 0"); |
74 |
|
|
assert(delay >= 1.5 * timestep && |
75 |
|
|
"Estimation delay should be >= 1.5*timestep"); |
76 |
|
|
m_dt = timestep; |
77 |
|
|
m_delay = delay; |
78 |
|
|
x_size = xSize; |
79 |
|
|
int winSizeEnc = (int)(2 * delay / m_dt); |
80 |
|
|
assert(winSizeEnc >= 3 && "Estimation-window's length should be >= 3"); |
81 |
|
|
|
82 |
|
|
if (polyOrder == 1) |
83 |
|
|
m_filter = new LinEstimator(winSizeEnc, x_size, m_dt); |
84 |
|
|
else if (polyOrder == 2) |
85 |
|
|
m_filter = new QuadEstimator(winSizeEnc, x_size, m_dt); |
86 |
|
|
else |
87 |
|
|
SEND_MSG("Only polynomial orders 1 and 2 allowed. Reinitialize the filter", |
88 |
|
|
MSG_TYPE_INFO); |
89 |
|
|
m_ddx_filter_std.resize(x_size); |
90 |
|
|
m_dx_filter_std.resize(x_size); |
91 |
|
|
m_x_filter_std.resize(x_size); |
92 |
|
|
m_x_std.resize(x_size); |
93 |
|
|
} |
94 |
|
|
|
95 |
|
|
/* --- SIGNALS ---------------------------------------------------------- */ |
96 |
|
|
/* --- SIGNALS ---------------------------------------------------------- */ |
97 |
|
|
/* --- SIGNALS ---------------------------------------------------------- */ |
98 |
|
|
|
99 |
|
|
/** Signal Filtering and Differentiation. */ |
100 |
|
|
DEFINE_SIGNAL_INNER_FUNCTION(x_dx_ddx, dynamicgraph::Vector) { |
101 |
|
|
sotDEBUG(15) << "Compute x_dx_ddx inner signal " << iter << std::endl; |
102 |
|
|
|
103 |
|
|
// read encoders and copy in std vector |
104 |
|
|
const dynamicgraph::Vector& base_x = m_xSIN(iter); |
105 |
|
|
COPY_VECTOR_TO_ARRAY(base_x, m_x_std); |
106 |
|
|
|
107 |
|
|
// Signal Filters |
108 |
|
|
m_filter->estimate(m_x_filter_std, m_x_std); |
109 |
|
|
m_filter->getEstimateDerivative(m_dx_filter_std, 1); |
110 |
|
|
m_filter->getEstimateDerivative(m_ddx_filter_std, 2); |
111 |
|
|
|
112 |
|
|
// copy data in signal vector |
113 |
|
|
if (s.size() != 3 * x_size) s.resize(3 * x_size); |
114 |
|
|
for (int i = 0; i < x_size; i++) s(i) = m_x_filter_std[i]; |
115 |
|
|
for (int i = 0; i < x_size; i++) s(i + x_size) = m_dx_filter_std[i]; |
116 |
|
|
for (int i = 0; i < x_size; i++) s(i + 2 * x_size) = m_ddx_filter_std[i]; |
117 |
|
|
|
118 |
|
|
return s; |
119 |
|
|
} |
120 |
|
|
|
121 |
|
|
/// ************************************************************************* |
122 |
|
|
/// /// The following signals depend only on other inner signals, so they just |
123 |
|
|
/// need to copy the interested part of the inner signal they depend on. |
124 |
|
|
/// ************************************************************************* |
125 |
|
|
/// /// |
126 |
|
|
|
127 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(x_filtered, dynamicgraph::Vector) { |
128 |
|
|
sotDEBUG(15) << "Compute x_filtered output signal " << iter << std::endl; |
129 |
|
|
|
130 |
|
|
const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter); |
131 |
|
|
if (s.size() != x_size) s.resize(x_size); |
132 |
|
|
s = x_dx_ddx.head(x_size); |
133 |
|
|
return s; |
134 |
|
|
} |
135 |
|
|
|
136 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector) { |
137 |
|
|
sotDEBUG(15) << "Compute dx output signal " << iter << std::endl; |
138 |
|
|
|
139 |
|
|
const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter); |
140 |
|
|
if (s.size() != x_size) s.resize(x_size); |
141 |
|
|
s = x_dx_ddx.segment(x_size, x_size); |
142 |
|
|
return s; |
143 |
|
|
} |
144 |
|
|
|
145 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector) { |
146 |
|
|
sotDEBUG(15) << "Compute ddx output signal " << iter << std::endl; |
147 |
|
|
|
148 |
|
|
const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter); |
149 |
|
|
if (s.size() != x_size) s.resize(x_size); |
150 |
|
|
s = x_dx_ddx.segment(2 * x_size, x_size); |
151 |
|
|
return s; |
152 |
|
|
} |
153 |
|
|
|
154 |
|
|
void NumericalDifference::display(std::ostream& os) const { |
155 |
|
|
os << "NumericalDifference " << getName() << ":\n"; |
156 |
|
|
try { |
157 |
|
|
getProfiler().report_all(3, os); |
158 |
|
|
} catch (ExceptionSignal e) { |
159 |
|
|
} |
160 |
|
|
} |
161 |
|
|
|
162 |
|
|
} // namespace torque_control |
163 |
|
|
} // namespace sot |
164 |
|
|
} // namespace dynamicgraph |