Directory: | ./ |
---|---|
File: | src/matrix/operator.hh |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 126 | 415 | 30.4% |
Branches: | 141 | 984 | 14.3% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /* | ||
2 | * Copyright 2010, | ||
3 | * François Bleibel, | ||
4 | * Olivier Stasse, | ||
5 | * Nicolas Mansard | ||
6 | * Joseph Mirabel | ||
7 | * | ||
8 | * CNRS/AIST | ||
9 | * | ||
10 | */ | ||
11 | |||
12 | #include <dynamic-graph/all-commands.h> | ||
13 | #include <dynamic-graph/factory.h> | ||
14 | #include <dynamic-graph/linear-algebra.h> | ||
15 | |||
16 | #include <boost/function.hpp> | ||
17 | #include <boost/numeric/conversion/cast.hpp> | ||
18 | #include <deque> | ||
19 | #include <sot/core/binary-op.hh> | ||
20 | #include <sot/core/debug.hh> | ||
21 | #include <sot/core/factory.hh> | ||
22 | #include <sot/core/matrix-geometry.hh> | ||
23 | #include <sot/core/unary-op.hh> | ||
24 | #include <sot/core/variadic-op.hh> | ||
25 | |||
26 | #include "../tools/type-name-helper.hh" | ||
27 | |||
28 | namespace dg = ::dynamicgraph; | ||
29 | |||
30 | /* ---------------------------------------------------------------------------*/ | ||
31 | /* ------- GENERIC HELPERS -------------------------------------------------- */ | ||
32 | /* ---------------------------------------------------------------------------*/ | ||
33 | |||
34 | #define ADD_COMMAND(name, def) commandMap.insert(std::make_pair(name, def)) | ||
35 | |||
36 | namespace dynamicgraph { | ||
37 | namespace sot { | ||
38 | template <typename TypeIn, typename TypeOut> | ||
39 | struct UnaryOpHeader { | ||
40 | typedef TypeIn Tin; | ||
41 | typedef TypeOut Tout; | ||
42 | 22 | static inline std::string nameTypeIn(void) { | |
43 | 22 | return TypeNameHelper<Tin>::typeName(); | |
44 | } | ||
45 | 11 | static inline std::string nameTypeOut(void) { | |
46 | 22 | return TypeNameHelper<Tout>::typeName(); | |
47 | } | ||
48 | ✗ | inline void addSpecificCommands(Entity &, Entity::CommandMap_t &) {} | |
49 | 2 | inline std::string getDocString() const { | |
50 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
8 | return std::string( |
51 | "Undocumented unary operator\n" | ||
52 | " - input ") + | ||
53 | nameTypeIn() + | ||
54 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
8 | std::string( |
55 | "\n" | ||
56 | " - output ") + | ||
57 |
7/14✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
|
20 | nameTypeOut() + std::string("\n"); |
58 | } | ||
59 | }; | ||
60 | |||
61 | /* ---------------------------------------------------------------------- */ | ||
62 | /* --- ALGEBRA SELECTORS ------------------------------------------------ */ | ||
63 | /* ---------------------------------------------------------------------- */ | ||
64 | struct VectorSelecter : public UnaryOpHeader<dg::Vector, dg::Vector> { | ||
65 | 1 | inline void operator()(const Tin &m, Vector &res) const { | |
66 | 1 | res.resize(size); | |
67 | 1 | Vector::Index r = 0; | |
68 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
3 | for (std::size_t i = 0; i < idxs.size(); ++i) { |
69 | 2 | const Vector::Index &R = idxs[i].first; | |
70 | 2 | const Vector::Index &nr = idxs[i].second; | |
71 |
2/4✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2 | assert((nr >= 0) && (R + nr <= m.size())); |
72 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
2 | res.segment(r, nr) = m.segment(R, nr); |
73 | 2 | r += nr; | |
74 | } | ||
75 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | assert(r == size); |
76 | 1 | } | |
77 | |||
78 | typedef std::pair<Vector::Index, Vector::Index> segment_t; | ||
79 | typedef std::vector<segment_t> segments_t; | ||
80 | segments_t idxs; | ||
81 | Vector::Index size; | ||
82 | |||
83 | 1 | inline void setBounds(const int &m, const int &M) { | |
84 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | idxs = segments_t(1, segment_t(m, M - m)); |
85 | 1 | size = M - m; | |
86 | 1 | } | |
87 | 1 | inline void addBounds(const int &m, const int &M) { | |
88 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | idxs.push_back(segment_t(m, M - m)); |
89 | 1 | size += M - m; | |
90 | 1 | } | |
91 | |||
92 | 1 | inline void addSpecificCommands(Entity &ent, | |
93 | Entity::CommandMap_t &commandMap) { | ||
94 | using namespace dynamicgraph::command; | ||
95 | 1 | std::string doc; | |
96 | |||
97 | boost::function<void(const int &, const int &)> setBound = | ||
98 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | boost::bind(&VectorSelecter::setBounds, this, _1, _2); |
99 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | doc = docCommandVoid2("Set the bound of the selection [m,M[.", "int (min)", |
100 | 1 | "int (max)"); | |
101 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | ADD_COMMAND("selec", makeCommandVoid2(ent, setBound, doc)); |
102 | boost::function<void(const int &, const int &)> addBound = | ||
103 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | boost::bind(&VectorSelecter::addBounds, this, _1, _2); |
104 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | doc = docCommandVoid2("Add a segment to be selected [m,M[.", "int (min)", |
105 | 1 | "int (max)"); | |
106 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | ADD_COMMAND("addSelec", makeCommandVoid2(ent, addBound, doc)); |
107 | 1 | } | |
108 | 2 | VectorSelecter() : size(0) {} | |
109 | }; | ||
110 | |||
111 | /* ---------------------------------------------------------------------- */ | ||
112 | struct VectorComponent : public UnaryOpHeader<dg::Vector, double> { | ||
113 | 1 | inline void operator()(const Tin &m, double &res) const { | |
114 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | assert(index < m.size()); |
115 | 1 | res = m(index); | |
116 | 1 | } | |
117 | |||
118 | int index; | ||
119 | 1 | inline void setIndex(const int &m) { index = m; } | |
120 | |||
121 | 1 | inline void addSpecificCommands(Entity &ent, | |
122 | Entity::CommandMap_t &commandMap) { | ||
123 | 1 | std::string doc; | |
124 | |||
125 | boost::function<void(const int &)> callback = | ||
126 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | boost::bind(&VectorComponent::setIndex, this, _1); |
127 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
2 | doc = command::docCommandVoid1("Set the index of the component.", |
128 | 1 | "int (index)"); | |
129 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | ADD_COMMAND("setIndex", command::makeCommandVoid1(ent, callback, doc)); |
130 | 1 | } | |
131 | 2 | inline std::string getDocString() const { | |
132 | std::string docString( | ||
133 | "Select a component of a vector\n" | ||
134 | " - input vector\n" | ||
135 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | " - output double"); |
136 | 2 | return docString; | |
137 | } | ||
138 | }; | ||
139 | |||
140 | /* ---------------------------------------------------------------------- */ | ||
141 | struct MatrixSelector : public UnaryOpHeader<dg::Matrix, dg::Matrix> { | ||
142 | 1 | inline void operator()(const Matrix &m, Matrix &res) const { | |
143 |
2/4✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | assert((imin <= imax) && (imax <= m.rows())); |
144 |
2/4✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | assert((jmin <= jmax) && (jmax <= m.cols())); |
145 | 1 | res.resize(imax - imin, jmax - jmin); | |
146 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
|
3 | for (int i = imin; i < imax; ++i) |
147 |
2/2✓ Branch 2 taken 4 times.
✓ Branch 3 taken 2 times.
|
6 | for (int j = jmin; j < jmax; ++j) res(i - imin, j - jmin) = m(i, j); |
148 | 1 | } | |
149 | |||
150 | public: | ||
151 | int imin, imax; | ||
152 | int jmin, jmax; | ||
153 | |||
154 | 1 | inline void setBoundsRow(const int &m, const int &M) { | |
155 | 1 | imin = m; | |
156 | 1 | imax = M; | |
157 | 1 | } | |
158 | 1 | inline void setBoundsCol(const int &m, const int &M) { | |
159 | 1 | jmin = m; | |
160 | 1 | jmax = M; | |
161 | 1 | } | |
162 | |||
163 | 1 | inline void addSpecificCommands(Entity &ent, | |
164 | Entity::CommandMap_t &commandMap) { | ||
165 | using namespace dynamicgraph::command; | ||
166 | 1 | std::string doc; | |
167 | |||
168 | boost::function<void(const int &, const int &)> setBoundsRow = | ||
169 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | boost::bind(&MatrixSelector::setBoundsRow, this, _1, _2); |
170 | boost::function<void(const int &, const int &)> setBoundsCol = | ||
171 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | boost::bind(&MatrixSelector::setBoundsCol, this, _1, _2); |
172 | |||
173 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
1 | doc = docCommandVoid2("Set the bound on rows.", "int (min)", "int (max)"); |
174 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | ADD_COMMAND("selecRows", makeCommandVoid2(ent, setBoundsRow, doc)); |
175 | |||
176 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | doc = docCommandVoid2("Set the bound on cols [m,M[.", "int (min)", |
177 | 1 | "int (max)"); | |
178 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | ADD_COMMAND("selecCols", makeCommandVoid2(ent, setBoundsCol, doc)); |
179 | 1 | } | |
180 | }; | ||
181 | |||
182 | /* ---------------------------------------------------------------------- */ | ||
183 | struct MatrixColumnSelector : public UnaryOpHeader<dg::Matrix, dg::Vector> { | ||
184 | public: | ||
185 | ✗ | inline void operator()(const Tin &m, Tout &res) const { | |
186 | ✗ | assert((imin <= imax) && (imax <= m.rows())); | |
187 | ✗ | assert(jcol < m.cols()); | |
188 | |||
189 | ✗ | res.resize(imax - imin); | |
190 | ✗ | for (int i = imin; i < imax; ++i) res(i - imin) = m(i, jcol); | |
191 | } | ||
192 | |||
193 | int imin, imax; | ||
194 | int jcol; | ||
195 | ✗ | inline void selectCol(const int &m) { jcol = m; } | |
196 | ✗ | inline void setBoundsRow(const int &m, const int &M) { | |
197 | ✗ | imin = m; | |
198 | ✗ | imax = M; | |
199 | } | ||
200 | |||
201 | ✗ | inline void addSpecificCommands(Entity &ent, | |
202 | Entity::CommandMap_t &commandMap) { | ||
203 | using namespace dynamicgraph::command; | ||
204 | ✗ | std::string doc; | |
205 | |||
206 | boost::function<void(const int &, const int &)> setBoundsRow = | ||
207 | ✗ | boost::bind(&MatrixColumnSelector::setBoundsRow, this, _1, _2); | |
208 | boost::function<void(const int &)> selectCol = | ||
209 | ✗ | boost::bind(&MatrixColumnSelector::selectCol, this, _1); | |
210 | |||
211 | ✗ | doc = docCommandVoid2("Set the bound on rows.", "int (min)", "int (max)"); | |
212 | ✗ | ADD_COMMAND("selecRows", makeCommandVoid2(ent, setBoundsRow, doc)); | |
213 | |||
214 | ✗ | doc = docCommandVoid1("Select the col to copy.", "int (col index)"); | |
215 | ✗ | ADD_COMMAND("selecCols", makeCommandVoid1(ent, selectCol, doc)); | |
216 | } | ||
217 | }; | ||
218 | |||
219 | /* ---------------------------------------------------------------------- */ | ||
220 | struct MatrixTranspose : public UnaryOpHeader<dg::Matrix, dg::Matrix> { | ||
221 | ✗ | inline void operator()(const Tin &m, Tout &res) const { res = m.transpose(); } | |
222 | }; | ||
223 | |||
224 | /* ---------------------------------------------------------------------- */ | ||
225 | struct Diagonalizer : public UnaryOpHeader<Vector, Matrix> { | ||
226 | ✗ | inline void operator()(const dg::Vector &r, dg::Matrix &res) { | |
227 | ✗ | res = r.asDiagonal(); | |
228 | } | ||
229 | |||
230 | public: | ||
231 | ✗ | Diagonalizer(void) : nbr(0), nbc(0) {} | |
232 | unsigned int nbr, nbc; | ||
233 | ✗ | inline void resize(const int &r, const int &c) { | |
234 | ✗ | nbr = r; | |
235 | ✗ | nbc = c; | |
236 | } | ||
237 | ✗ | inline void addSpecificCommands(Entity &ent, | |
238 | Entity::CommandMap_t &commandMap) { | ||
239 | using namespace dynamicgraph::command; | ||
240 | ✗ | std::string doc; | |
241 | |||
242 | boost::function<void(const int &, const int &)> resize = | ||
243 | ✗ | boost::bind(&Diagonalizer::resize, this, _1, _2); | |
244 | |||
245 | ✗ | doc = docCommandVoid2("Set output size.", "int (row)", "int (col)"); | |
246 | ✗ | ADD_COMMAND("resize", makeCommandVoid2(ent, resize, doc)); | |
247 | } | ||
248 | }; | ||
249 | |||
250 | /* ---------------------------------------------------------------------- */ | ||
251 | /* --- INVERSION -------------------------------------------------------- */ | ||
252 | /* ---------------------------------------------------------------------- */ | ||
253 | |||
254 | template <typename matrixgen> | ||
255 | struct Inverser : public UnaryOpHeader<matrixgen, matrixgen> { | ||
256 | typedef typename UnaryOpHeader<matrixgen, matrixgen>::Tin Tin; | ||
257 | typedef typename UnaryOpHeader<matrixgen, matrixgen>::Tout Tout; | ||
258 | ✗ | inline void operator()(const Tin &m, Tout &res) const { res = m.inverse(); } | |
259 | }; | ||
260 | |||
261 | struct Normalize : public UnaryOpHeader<dg::Vector, double> { | ||
262 | ✗ | inline void operator()(const dg::Vector &m, double &res) const { | |
263 | ✗ | res = m.norm(); | |
264 | } | ||
265 | |||
266 | ✗ | inline std::string getDocString() const { | |
267 | std::string docString( | ||
268 | "Computes the norm of a vector\n" | ||
269 | " - input vector\n" | ||
270 | ✗ | " - output double"); | |
271 | ✗ | return docString; | |
272 | } | ||
273 | }; | ||
274 | |||
275 | struct InverserRotation : public UnaryOpHeader<MatrixRotation, MatrixRotation> { | ||
276 | ✗ | inline void operator()(const Tin &m, Tout &res) const { res = m.transpose(); } | |
277 | }; | ||
278 | |||
279 | struct InverserQuaternion | ||
280 | : public UnaryOpHeader<VectorQuaternion, VectorQuaternion> { | ||
281 | ✗ | inline void operator()(const Tin &m, Tout &res) const { res = m.conjugate(); } | |
282 | }; | ||
283 | |||
284 | /* ----------------------------------------------------------------------- */ | ||
285 | /* --- SE3/SO3 conversions ----------------------------------------------- */ | ||
286 | /* ----------------------------------------------------------------------- */ | ||
287 | |||
288 | struct MatrixHomoToPoseUTheta | ||
289 | : public UnaryOpHeader<MatrixHomogeneous, dg::Vector> { | ||
290 | ✗ | inline void operator()(const MatrixHomogeneous &M, dg::Vector &res) { | |
291 | ✗ | res.resize(6); | |
292 | ✗ | VectorUTheta r(M.linear()); | |
293 | ✗ | res.head<3>() = M.translation(); | |
294 | ✗ | res.tail<3>() = r.angle() * r.axis(); | |
295 | } | ||
296 | }; | ||
297 | |||
298 | struct SkewSymToVector : public UnaryOpHeader<Matrix, Vector> { | ||
299 | ✗ | inline void operator()(const Matrix &M, Vector &res) { | |
300 | ✗ | res.resize(3); | |
301 | ✗ | res(0) = M(7); | |
302 | ✗ | res(1) = M(2); | |
303 | ✗ | res(2) = M(3); | |
304 | } | ||
305 | }; | ||
306 | |||
307 | struct PoseUThetaToMatrixHomo | ||
308 | : public UnaryOpHeader<Vector, MatrixHomogeneous> { | ||
309 | ✗ | inline void operator()(const dg::Vector &v, MatrixHomogeneous &res) { | |
310 | ✗ | assert(v.size() >= 6); | |
311 | ✗ | res.translation() = v.head<3>(); | |
312 | ✗ | double theta = v.tail<3>().norm(); | |
313 | ✗ | if (theta > 0) | |
314 | ✗ | res.linear() = Eigen::AngleAxisd(theta, v.tail<3>() / theta).matrix(); | |
315 | else | ||
316 | ✗ | res.linear().setIdentity(); | |
317 | } | ||
318 | }; | ||
319 | |||
320 | struct SE3VectorToMatrixHomo | ||
321 | : public UnaryOpHeader<dg::Vector, MatrixHomogeneous> { | ||
322 | 9 | void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) { | |
323 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
9 | Mres.translation() = vect.head<3>(); |
324 |
3/6✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
|
9 | Mres.linear().row(0) = vect.segment(3, 3); |
325 |
3/6✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
|
9 | Mres.linear().row(1) = vect.segment(6, 3); |
326 |
3/6✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
|
9 | Mres.linear().row(2) = vect.segment(9, 3); |
327 | 9 | } | |
328 | }; | ||
329 | |||
330 | struct MatrixHomoToSE3Vector | ||
331 | : public UnaryOpHeader<MatrixHomogeneous, dg::Vector> { | ||
332 | 9 | void operator()(const MatrixHomogeneous &M, dg::Vector &res) { | |
333 | 9 | res.resize(12); | |
334 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
9 | res.head<3>() = M.translation(); |
335 |
3/6✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
|
9 | res.segment(3, 3) = M.linear().row(0); |
336 |
3/6✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
|
9 | res.segment(6, 3) = M.linear().row(1); |
337 |
3/6✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
|
9 | res.segment(9, 3) = M.linear().row(2); |
338 | 9 | } | |
339 | }; | ||
340 | |||
341 | struct PoseQuaternionToMatrixHomo | ||
342 | : public UnaryOpHeader<Vector, MatrixHomogeneous> { | ||
343 | 9 | void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) { | |
344 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
9 | Mres.translation() = vect.head<3>(); |
345 |
4/8✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
|
9 | Mres.linear() = VectorQuaternion(vect.tail<4>()).toRotationMatrix(); |
346 | 9 | } | |
347 | }; | ||
348 | |||
349 | struct MatrixHomoToPoseQuaternion | ||
350 | : public UnaryOpHeader<MatrixHomogeneous, Vector> { | ||
351 | 9 | inline void operator()(const MatrixHomogeneous &M, Vector &res) { | |
352 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | res.resize(7); |
353 |
3/6✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
|
9 | res.head<3>() = M.translation(); |
354 |
2/4✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
9 | Eigen::Map<VectorQuaternion> q(res.tail<4>().data()); |
355 |
2/4✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
|
9 | q = M.linear(); |
356 | 9 | } | |
357 | }; | ||
358 | |||
359 | struct MatrixHomoToPoseRollPitchYaw | ||
360 | : public UnaryOpHeader<MatrixHomogeneous, Vector> { | ||
361 | ✗ | inline void operator()(const MatrixHomogeneous &M, dg::Vector &res) { | |
362 | ✗ | VectorRollPitchYaw r = (M.linear().eulerAngles(2, 1, 0)).reverse(); | |
363 | ✗ | dg::Vector t(3); | |
364 | ✗ | t = M.translation(); | |
365 | ✗ | res.resize(6); | |
366 | ✗ | for (unsigned int i = 0; i < 3; ++i) res(i) = t(i); | |
367 | ✗ | for (unsigned int i = 0; i < 3; ++i) res(i + 3) = r(i); | |
368 | } | ||
369 | }; | ||
370 | |||
371 | struct PoseRollPitchYawToMatrixHomo | ||
372 | : public UnaryOpHeader<Vector, MatrixHomogeneous> { | ||
373 | ✗ | inline void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) { | |
374 | ✗ | VectorRollPitchYaw r; | |
375 | ✗ | for (unsigned int i = 0; i < 3; ++i) r(i) = vect(i + 3); | |
376 | ✗ | MatrixRotation R = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) * | |
377 | ✗ | Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) * | |
378 | ✗ | Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX())) | |
379 | ✗ | .toRotationMatrix(); | |
380 | |||
381 | ✗ | dg::Vector t(3); | |
382 | ✗ | for (unsigned int i = 0; i < 3; ++i) t(i) = vect(i); | |
383 | |||
384 | // buildFrom(R,t); | ||
385 | ✗ | Mres = Eigen::Translation3d(t) * R; | |
386 | } | ||
387 | }; | ||
388 | |||
389 | struct PoseRollPitchYawToPoseUTheta : public UnaryOpHeader<Vector, Vector> { | ||
390 | ✗ | inline void operator()(const dg::Vector &vect, dg::Vector &vectres) { | |
391 | ✗ | VectorRollPitchYaw r; | |
392 | ✗ | for (unsigned int i = 0; i < 3; ++i) r(i) = vect(i + 3); | |
393 | ✗ | MatrixRotation R = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) * | |
394 | ✗ | Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) * | |
395 | ✗ | Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX())) | |
396 | ✗ | .toRotationMatrix(); | |
397 | |||
398 | ✗ | VectorUTheta rrot(R); | |
399 | |||
400 | ✗ | vectres.resize(6); | |
401 | ✗ | for (unsigned int i = 0; i < 3; ++i) { | |
402 | ✗ | vectres(i) = vect(i); | |
403 | ✗ | vectres(i + 3) = rrot.angle() * rrot.axis()(i); | |
404 | } | ||
405 | } | ||
406 | }; | ||
407 | |||
408 | struct HomoToMatrix : public UnaryOpHeader<MatrixHomogeneous, Matrix> { | ||
409 | ✗ | inline void operator()(const MatrixHomogeneous &M, dg::Matrix &res) { | |
410 | ✗ | res = M.matrix(); | |
411 | } | ||
412 | }; | ||
413 | |||
414 | struct MatrixToHomo : public UnaryOpHeader<Matrix, MatrixHomogeneous> { | ||
415 | ✗ | inline void operator()(const Eigen::Matrix<double, 4, 4> &M, | |
416 | MatrixHomogeneous &res) { | ||
417 | ✗ | res = M; | |
418 | } | ||
419 | }; | ||
420 | |||
421 | struct HomoToTwist : public UnaryOpHeader<MatrixHomogeneous, MatrixTwist> { | ||
422 | ✗ | inline void operator()(const MatrixHomogeneous &M, MatrixTwist &res) { | |
423 | ✗ | Eigen::Vector3d _t = M.translation(); | |
424 | ✗ | MatrixRotation R(M.linear()); | |
425 | ✗ | Eigen::Matrix3d Tx; | |
426 | ✗ | Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0; | |
427 | |||
428 | ✗ | Eigen::Matrix3d sk; | |
429 | ✗ | sk = Tx * R; | |
430 | ✗ | res.block<3, 3>(0, 0) = R; | |
431 | ✗ | res.block<3, 3>(0, 3) = sk; | |
432 | ✗ | res.block<3, 3>(3, 0) = Eigen::Matrix3d::Zero(); | |
433 | ✗ | res.block<3, 3>(3, 3) = R; | |
434 | } | ||
435 | }; | ||
436 | |||
437 | struct HomoToRotation | ||
438 | : public UnaryOpHeader<MatrixHomogeneous, MatrixRotation> { | ||
439 | ✗ | inline void operator()(const MatrixHomogeneous &M, MatrixRotation &res) { | |
440 | ✗ | res = M.linear(); | |
441 | } | ||
442 | }; | ||
443 | |||
444 | struct MatrixHomoToPose : public UnaryOpHeader<MatrixHomogeneous, Vector> { | ||
445 | ✗ | inline void operator()(const MatrixHomogeneous &M, Vector &res) { | |
446 | ✗ | res.resize(3); | |
447 | ✗ | res = M.translation(); | |
448 | } | ||
449 | }; | ||
450 | |||
451 | struct RPYToMatrix : public UnaryOpHeader<VectorRollPitchYaw, MatrixRotation> { | ||
452 | 9 | inline void operator()(const VectorRollPitchYaw &r, MatrixRotation &res) { | |
453 |
4/8✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
|
9 | res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) * |
454 |
4/8✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
|
18 | Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) * |
455 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
9 | Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX())) |
456 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | .toRotationMatrix(); |
457 | 9 | } | |
458 | }; | ||
459 | |||
460 | struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> { | ||
461 | 9 | inline void operator()(const MatrixRotation &r, VectorRollPitchYaw &res) { | |
462 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
9 | res = (r.eulerAngles(2, 1, 0)).reverse(); |
463 | 9 | } | |
464 | }; | ||
465 | |||
466 | struct RPYToQuaternion | ||
467 | : public UnaryOpHeader<VectorRollPitchYaw, VectorQuaternion> { | ||
468 | 9 | inline void operator()(const VectorRollPitchYaw &r, VectorQuaternion &res) { | |
469 |
4/8✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
|
9 | res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) * |
470 |
4/8✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
|
18 | Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) * |
471 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
9 | Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX())) |
472 |
2/4✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
|
9 | .toRotationMatrix(); |
473 | 9 | } | |
474 | }; | ||
475 | |||
476 | struct QuaternionToRPY | ||
477 | : public UnaryOpHeader<VectorQuaternion, VectorRollPitchYaw> { | ||
478 | 9 | inline void operator()(const VectorQuaternion &r, VectorRollPitchYaw &res) { | |
479 |
3/6✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
|
9 | res = (r.toRotationMatrix().eulerAngles(2, 1, 0)).reverse(); |
480 | 9 | } | |
481 | }; | ||
482 | |||
483 | struct QuaternionToMatrix | ||
484 | : public UnaryOpHeader<VectorQuaternion, MatrixRotation> { | ||
485 | 9 | inline void operator()(const VectorQuaternion &r, MatrixRotation &res) { | |
486 | 9 | res = r.toRotationMatrix(); | |
487 | 9 | } | |
488 | }; | ||
489 | |||
490 | struct MatrixToQuaternion | ||
491 | : public UnaryOpHeader<MatrixRotation, VectorQuaternion> { | ||
492 | 9 | inline void operator()(const MatrixRotation &r, VectorQuaternion &res) { | |
493 | 9 | res = r; | |
494 | 9 | } | |
495 | }; | ||
496 | |||
497 | struct MatrixToUTheta : public UnaryOpHeader<MatrixRotation, VectorUTheta> { | ||
498 | ✗ | inline void operator()(const MatrixRotation &r, VectorUTheta &res) { | |
499 | ✗ | res = r; | |
500 | } | ||
501 | }; | ||
502 | |||
503 | struct UThetaToQuaternion | ||
504 | : public UnaryOpHeader<VectorUTheta, VectorQuaternion> { | ||
505 | ✗ | inline void operator()(const VectorUTheta &r, VectorQuaternion &res) { | |
506 | ✗ | res = r; | |
507 | } | ||
508 | }; | ||
509 | |||
510 | template <typename TypeIn1, typename TypeIn2, typename TypeOut> | ||
511 | struct BinaryOpHeader { | ||
512 | typedef TypeIn1 Tin1; | ||
513 | typedef TypeIn2 Tin2; | ||
514 | typedef TypeOut Tout; | ||
515 | ✗ | inline static std::string nameTypeIn1(void) { | |
516 | ✗ | return TypeNameHelper<Tin1>::typeName(); | |
517 | } | ||
518 | ✗ | inline static std::string nameTypeIn2(void) { | |
519 | ✗ | return TypeNameHelper<Tin2>::typeName(); | |
520 | } | ||
521 | ✗ | inline static std::string nameTypeOut(void) { | |
522 | ✗ | return TypeNameHelper<Tout>::typeName(); | |
523 | } | ||
524 | ✗ | inline void addSpecificCommands(Entity &, Entity::CommandMap_t &) {} | |
525 | ✗ | inline std::string getDocString() const { | |
526 | ✗ | return std::string( | |
527 | "Undocumented binary operator\n" | ||
528 | " - input ") + | ||
529 | nameTypeIn1() + | ||
530 | ✗ | std::string( | |
531 | "\n" | ||
532 | " - ") + | ||
533 | nameTypeIn2() + | ||
534 | ✗ | std::string( | |
535 | "\n" | ||
536 | " - output ") + | ||
537 | ✗ | nameTypeOut() + std::string("\n"); | |
538 | } | ||
539 | }; | ||
540 | |||
541 | } /* namespace sot */ | ||
542 | } /* namespace dynamicgraph */ | ||
543 | |||
544 | /* ---------------------------------------------------------------------------*/ | ||
545 | /* ---------------------------------------------------------------------------*/ | ||
546 | /* ---------------------------------------------------------------------------*/ | ||
547 | |||
548 | namespace dynamicgraph { | ||
549 | namespace sot { | ||
550 | |||
551 | /* --- MULTIPLICATION --------------------------------------------------- */ | ||
552 | |||
553 | template <typename F, typename E> | ||
554 | struct Multiplier_FxE__E : public BinaryOpHeader<F, E, E> { | ||
555 | ✗ | inline void operator()(const F &f, const E &e, E &res) const { res = f * e; } | |
556 | }; | ||
557 | |||
558 | template <> | ||
559 | inline void | ||
560 | ✗ | Multiplier_FxE__E<dynamicgraph::sot::MatrixHomogeneous, dynamicgraph::Vector>:: | |
561 | operator()(const dynamicgraph::sot::MatrixHomogeneous &f, | ||
562 | const dynamicgraph::Vector &e, dynamicgraph::Vector &res) const { | ||
563 | ✗ | res = f.matrix() * e; | |
564 | } | ||
565 | |||
566 | template <> | ||
567 | ✗ | inline void Multiplier_FxE__E<double, dynamicgraph::Vector>::operator()( | |
568 | const double &x, const dynamicgraph::Vector &v, | ||
569 | dynamicgraph::Vector &res) const { | ||
570 | ✗ | res = v; | |
571 | ✗ | res *= x; | |
572 | } | ||
573 | |||
574 | typedef Multiplier_FxE__E<double, dynamicgraph::Vector> | ||
575 | Multiplier_double_vector; | ||
576 | typedef Multiplier_FxE__E<dynamicgraph::Matrix, dynamicgraph::Vector> | ||
577 | Multiplier_matrix_vector; | ||
578 | typedef Multiplier_FxE__E<MatrixHomogeneous, dynamicgraph::Vector> | ||
579 | Multiplier_matrixHomo_vector; | ||
580 | typedef Multiplier_FxE__E<MatrixTwist, dynamicgraph::Vector> | ||
581 | Multiplier_matrixTwist_vector; | ||
582 | |||
583 | /* --- SUBSTRACTION ----------------------------------------------------- */ | ||
584 | template <typename T> | ||
585 | struct Substraction : public BinaryOpHeader<T, T, T> { | ||
586 | ✗ | inline void operator()(const T &v1, const T &v2, T &r) const { | |
587 | ✗ | r = v1; | |
588 | ✗ | r -= v2; | |
589 | } | ||
590 | }; | ||
591 | |||
592 | /* --- STACK ------------------------------------------------------------ */ | ||
593 | struct VectorStack | ||
594 | : public BinaryOpHeader<dynamicgraph::Vector, dynamicgraph::Vector, | ||
595 | dynamicgraph::Vector> { | ||
596 | public: | ||
597 | int v1min, v1max; | ||
598 | int v2min, v2max; | ||
599 | ✗ | inline void operator()(const dynamicgraph::Vector &v1, | |
600 | const dynamicgraph::Vector &v2, | ||
601 | dynamicgraph::Vector &res) const { | ||
602 | ✗ | assert((v1max >= v1min) && (v1.size() >= v1max)); | |
603 | ✗ | assert((v2max >= v2min) && (v2.size() >= v2max)); | |
604 | |||
605 | ✗ | const int v1size = v1max - v1min, v2size = v2max - v2min; | |
606 | ✗ | res.resize(v1size + v2size); | |
607 | ✗ | for (int i = 0; i < v1size; ++i) { | |
608 | ✗ | res(i) = v1(i + v1min); | |
609 | } | ||
610 | ✗ | for (int i = 0; i < v2size; ++i) { | |
611 | ✗ | res(v1size + i) = v2(i + v2min); | |
612 | } | ||
613 | } | ||
614 | |||
615 | ✗ | inline void selec1(const int &m, const int M) { | |
616 | ✗ | v1min = m; | |
617 | ✗ | v1max = M; | |
618 | } | ||
619 | ✗ | inline void selec2(const int &m, const int M) { | |
620 | ✗ | v2min = m; | |
621 | ✗ | v2max = M; | |
622 | } | ||
623 | |||
624 | ✗ | inline void addSpecificCommands(Entity &ent, | |
625 | Entity::CommandMap_t &commandMap) { | ||
626 | using namespace dynamicgraph::command; | ||
627 | ✗ | std::string doc; | |
628 | |||
629 | boost::function<void(const int &, const int &)> selec1 = | ||
630 | ✗ | boost::bind(&VectorStack::selec1, this, _1, _2); | |
631 | boost::function<void(const int &, const int &)> selec2 = | ||
632 | ✗ | boost::bind(&VectorStack::selec2, this, _1, _2); | |
633 | |||
634 | ✗ | ADD_COMMAND( | |
635 | "selec1", | ||
636 | makeCommandVoid2(ent, selec1, | ||
637 | docCommandVoid2("set the min and max of selection.", | ||
638 | "int (imin)", "int (imax)"))); | ||
639 | ✗ | ADD_COMMAND( | |
640 | "selec2", | ||
641 | makeCommandVoid2(ent, selec2, | ||
642 | docCommandVoid2("set the min and max of selection.", | ||
643 | "int (imin)", "int (imax)"))); | ||
644 | } | ||
645 | }; | ||
646 | |||
647 | /* ---------------------------------------------------------------------- */ | ||
648 | |||
649 | struct Composer | ||
650 | : public BinaryOpHeader<dynamicgraph::Matrix, dynamicgraph::Vector, | ||
651 | MatrixHomogeneous> { | ||
652 | ✗ | inline void operator()(const dynamicgraph::Matrix &R, | |
653 | const dynamicgraph::Vector &t, | ||
654 | MatrixHomogeneous &H) const { | ||
655 | ✗ | H.linear() = R; | |
656 | ✗ | H.translation() = t; | |
657 | } | ||
658 | }; | ||
659 | |||
660 | /* --- CONVOLUTION PRODUCT ---------------------------------------------- */ | ||
661 | struct ConvolutionTemporal | ||
662 | : public BinaryOpHeader<dynamicgraph::Vector, dynamicgraph::Matrix, | ||
663 | dynamicgraph::Vector> { | ||
664 | typedef std::deque<dynamicgraph::Vector> MemoryType; | ||
665 | MemoryType memory; | ||
666 | |||
667 | ✗ | inline void convolution(const MemoryType &f1, const dynamicgraph::Matrix &f2, | |
668 | dynamicgraph::Vector &res) { | ||
669 | ✗ | const Vector::Index nconv = (Vector::Index)f1.size(), nsig = f2.rows(); | |
670 | sotDEBUG(15) << "Size: " << nconv << "x" << nsig << std::endl; | ||
671 | ✗ | if (nconv > f2.cols()) return; // TODO: error, this should not happen | |
672 | |||
673 | ✗ | res.resize(nsig); | |
674 | ✗ | res.fill(0); | |
675 | ✗ | unsigned int j = 0; | |
676 | ✗ | for (MemoryType::const_iterator iter = f1.begin(); iter != f1.end(); | |
677 | iter++) { | ||
678 | ✗ | const dynamicgraph::Vector &s_tau = *iter; | |
679 | sotDEBUG(45) << "Sig" << j << ": " << s_tau; | ||
680 | ✗ | if (s_tau.size() != nsig) return; // TODO: error throw; | |
681 | ✗ | for (int i = 0; i < nsig; ++i) { | |
682 | ✗ | res(i) += f2(i, j) * s_tau(i); | |
683 | } | ||
684 | ✗ | j++; | |
685 | } | ||
686 | } | ||
687 | ✗ | inline void operator()(const dynamicgraph::Vector &v1, | |
688 | const dynamicgraph::Matrix &m2, | ||
689 | dynamicgraph::Vector &res) { | ||
690 | ✗ | memory.push_front(v1); | |
691 | ✗ | while ((Vector::Index)memory.size() > m2.cols()) memory.pop_back(); | |
692 | ✗ | convolution(memory, m2, res); | |
693 | } | ||
694 | }; | ||
695 | |||
696 | /* --- BOOLEAN REDUCTION ------------------------------------------------ */ | ||
697 | |||
698 | template <typename T> | ||
699 | struct Comparison : public BinaryOpHeader<T, T, bool> { | ||
700 | ✗ | inline void operator()(const T &a, const T &b, bool &res) const { | |
701 | ✗ | res = (a < b); | |
702 | } | ||
703 | ✗ | inline std::string getDocString() const { | |
704 | typedef BinaryOpHeader<T, T, bool> Base; | ||
705 | ✗ | return std::string( | |
706 | "Comparison of inputs:\n" | ||
707 | " - input ") + | ||
708 | Base::nameTypeIn1() + | ||
709 | ✗ | std::string( | |
710 | "\n" | ||
711 | " - ") + | ||
712 | Base::nameTypeIn2() + | ||
713 | ✗ | std::string( | |
714 | "\n" | ||
715 | " - output ") + | ||
716 | Base::nameTypeOut() + | ||
717 | ✗ | std::string( | |
718 | "\n" | ||
719 | ✗ | " sout = ( sin1 < sin2 )\n"); | |
720 | } | ||
721 | }; | ||
722 | |||
723 | template <typename T1, typename T2 = T1> | ||
724 | struct MatrixComparison : public BinaryOpHeader<T1, T2, bool> { | ||
725 | // TODO T1 or T2 could be a scalar type. | ||
726 | ✗ | inline void operator()(const T1 &a, const T2 &b, bool &res) const { | |
727 | ✗ | if (equal && any) | |
728 | ✗ | res = (a.array() <= b.array()).any(); | |
729 | ✗ | else if (equal && !any) | |
730 | ✗ | res = (a.array() <= b.array()).all(); | |
731 | ✗ | else if (!equal && any) | |
732 | ✗ | res = (a.array() < b.array()).any(); | |
733 | ✗ | else if (!equal && !any) | |
734 | ✗ | res = (a.array() < b.array()).all(); | |
735 | } | ||
736 | ✗ | inline std::string getDocString() const { | |
737 | typedef BinaryOpHeader<T1, T2, bool> Base; | ||
738 | ✗ | return std::string( | |
739 | "Comparison of inputs:\n" | ||
740 | " - input ") + | ||
741 | Base::nameTypeIn1() + | ||
742 | ✗ | std::string( | |
743 | "\n" | ||
744 | " - ") + | ||
745 | Base::nameTypeIn2() + | ||
746 | ✗ | std::string( | |
747 | "\n" | ||
748 | " - output ") + | ||
749 | Base::nameTypeOut() + | ||
750 | ✗ | std::string( | |
751 | "\n" | ||
752 | " sout = ( sin1 < sin2 ).op()\n") + | ||
753 | ✗ | std::string( | |
754 | "\n" | ||
755 | " where op is either any (default) or all. The " | ||
756 | ✗ | "comparison can be made <=.\n"); | |
757 | } | ||
758 | ✗ | MatrixComparison() : any(true), equal(false) {} | |
759 | ✗ | inline void addSpecificCommands(Entity &ent, | |
760 | Entity::CommandMap_t &commandMap) { | ||
761 | using namespace dynamicgraph::command; | ||
762 | ✗ | ADD_COMMAND( | |
763 | "setTrueIfAny", | ||
764 | makeDirectSetter(ent, &any, docDirectSetter("trueIfAny", "bool"))); | ||
765 | ✗ | ADD_COMMAND( | |
766 | "getTrueIfAny", | ||
767 | makeDirectGetter(ent, &any, docDirectGetter("trueIfAny", "bool"))); | ||
768 | ✗ | ADD_COMMAND("setEqual", makeDirectSetter(ent, &equal, | |
769 | docDirectSetter("equal", "bool"))); | ||
770 | ✗ | ADD_COMMAND("getEqual", makeDirectGetter(ent, &equal, | |
771 | docDirectGetter("equal", "bool"))); | ||
772 | } | ||
773 | bool any, equal; | ||
774 | }; | ||
775 | |||
776 | } /* namespace sot */ | ||
777 | } /* namespace dynamicgraph */ | ||
778 | |||
779 | namespace dynamicgraph { | ||
780 | namespace sot { | ||
781 | |||
782 | template <typename T> | ||
783 | struct WeightedAdder : public BinaryOpHeader<T, T, T> { | ||
784 | public: | ||
785 | double gain1, gain2; | ||
786 | ✗ | inline void operator()(const T &v1, const T &v2, T &res) const { | |
787 | ✗ | res = v1; | |
788 | ✗ | res *= gain1; | |
789 | ✗ | res += gain2 * v2; | |
790 | } | ||
791 | |||
792 | ✗ | inline void addSpecificCommands(Entity &ent, | |
793 | Entity::CommandMap_t &commandMap) { | ||
794 | using namespace dynamicgraph::command; | ||
795 | ✗ | std::string doc; | |
796 | |||
797 | ✗ | ADD_COMMAND( | |
798 | "setGain1", | ||
799 | makeDirectSetter(ent, &gain1, docDirectSetter("gain1", "double"))); | ||
800 | ✗ | ADD_COMMAND( | |
801 | "setGain2", | ||
802 | makeDirectSetter(ent, &gain2, docDirectSetter("gain2", "double"))); | ||
803 | ✗ | ADD_COMMAND( | |
804 | "getGain1", | ||
805 | makeDirectGetter(ent, &gain1, docDirectGetter("gain1", "double"))); | ||
806 | ✗ | ADD_COMMAND( | |
807 | "getGain2", | ||
808 | makeDirectGetter(ent, &gain2, docDirectGetter("gain2", "double"))); | ||
809 | } | ||
810 | |||
811 | ✗ | inline std::string getDocString() const { | |
812 | ✗ | return std::string("Weighted Combination of inputs : \n - gain{1|2} gain."); | |
813 | } | ||
814 | }; | ||
815 | |||
816 | } // namespace sot | ||
817 | } // namespace dynamicgraph | ||
818 | |||
819 | namespace dynamicgraph { | ||
820 | namespace sot { | ||
821 | template <typename Tin, typename Tout, typename Time> | ||
822 | ✗ | std::string VariadicAbstract<Tin, Tout, Time>::getTypeInName(void) { | |
823 | ✗ | return TypeNameHelper<Tin>::typeName(); | |
824 | } | ||
825 | template <typename Tin, typename Tout, typename Time> | ||
826 | ✗ | std::string VariadicAbstract<Tin, Tout, Time>::getTypeOutName(void) { | |
827 | ✗ | return TypeNameHelper<Tout>::typeName(); | |
828 | } | ||
829 | |||
830 | template <typename TypeIn, typename TypeOut> | ||
831 | struct VariadicOpHeader { | ||
832 | typedef TypeIn Tin; | ||
833 | typedef TypeOut Tout; | ||
834 | ✗ | inline static std::string nameTypeIn(void) { | |
835 | ✗ | return TypeNameHelper<Tin>::typeName(); | |
836 | } | ||
837 | ✗ | inline static std::string nameTypeOut(void) { | |
838 | ✗ | return TypeNameHelper<Tout>::typeName(); | |
839 | } | ||
840 | template <typename Op> | ||
841 | ✗ | inline void initialize(VariadicOp<Op> *, Entity::CommandMap_t &) {} | |
842 | ✗ | inline void updateSignalNumber(const int &) {} | |
843 | ✗ | inline std::string getDocString() const { | |
844 | return std::string( | ||
845 | "Undocumented variadic operator\n" | ||
846 | " - input " + | ||
847 | nameTypeIn() + | ||
848 | "\n" | ||
849 | " - output " + | ||
850 | ✗ | nameTypeOut() + "\n"); | |
851 | } | ||
852 | }; | ||
853 | |||
854 | /* --- VectorMix ------------------------------------------------------------ */ | ||
855 | struct VectorMix : public VariadicOpHeader<Vector, Vector> { | ||
856 | public: | ||
857 | typedef VariadicOp<VectorMix> Base; | ||
858 | struct segment_t { | ||
859 | Vector::Index index, size, input; | ||
860 | std::size_t sigIdx; | ||
861 | ✗ | segment_t(Vector::Index i, Vector::Index s, std::size_t sig) | |
862 | ✗ | : index(i), size(s), sigIdx(sig) {} | |
863 | }; | ||
864 | typedef std::vector<segment_t> segments_t; | ||
865 | Base *entity; | ||
866 | segments_t idxs; | ||
867 | ✗ | inline void operator()(const std::vector<const Vector *> &vs, | |
868 | Vector &res) const { | ||
869 | ✗ | res = *vs[0]; | |
870 | ✗ | for (std::size_t i = 0; i < idxs.size(); ++i) { | |
871 | ✗ | const segment_t &s = idxs[i]; | |
872 | ✗ | if (s.sigIdx >= vs.size()) | |
873 | ✗ | throw std::invalid_argument("Index out of range in VectorMix"); | |
874 | ✗ | res.segment(s.index, s.size) = *vs[s.sigIdx]; | |
875 | } | ||
876 | } | ||
877 | |||
878 | ✗ | inline void addSelec(const int &sigIdx, const int &i, const int &s) { | |
879 | ✗ | idxs.push_back(segment_t(i, s, sigIdx)); | |
880 | } | ||
881 | |||
882 | ✗ | inline void initialize(Base *ent, Entity::CommandMap_t &commandMap) { | |
883 | using namespace dynamicgraph::command; | ||
884 | ✗ | entity = ent; | |
885 | |||
886 | ✗ | ent->addSignal("default"); | |
887 | |||
888 | boost::function<void(const int &, const int &, const int &)> selec = | ||
889 | ✗ | boost::bind(&VectorMix::addSelec, this, _1, _2, _3); | |
890 | |||
891 | ✗ | commandMap.insert(std::make_pair( | |
892 | ✗ | "addSelec", makeCommandVoid3<Base, int, int, int>( | |
893 | *ent, selec, | ||
894 | ✗ | docCommandVoid3("add selection from a vector.", | |
895 | "int (signal index >= 1)", | ||
896 | "int (index)", "int (size)")))); | ||
897 | } | ||
898 | }; | ||
899 | |||
900 | /* --- ADDITION --------------------------------------------------------- */ | ||
901 | template <typename T> | ||
902 | struct AdderVariadic : public VariadicOpHeader<T, T> { | ||
903 | typedef VariadicOp<AdderVariadic> Base; | ||
904 | |||
905 | Base *entity; | ||
906 | Vector coeffs; | ||
907 | |||
908 | ✗ | AdderVariadic() : coeffs() {} | |
909 | ✗ | inline void operator()(const std::vector<const T *> &vs, T &res) const { | |
910 | ✗ | assert(vs.size() == (std::size_t)coeffs.size()); | |
911 | ✗ | if (vs.size() == 0) return; | |
912 | ✗ | res = coeffs[0] * (*vs[0]); | |
913 | ✗ | for (std::size_t i = 1; i < vs.size(); ++i) res += coeffs[i] * (*vs[i]); | |
914 | } | ||
915 | |||
916 | ✗ | inline void setCoeffs(const Vector &c) { | |
917 | ✗ | if (entity->getSignalNumber() != c.size()) | |
918 | ✗ | throw std::invalid_argument("Invalid coefficient size."); | |
919 | ✗ | coeffs = c; | |
920 | } | ||
921 | ✗ | inline void updateSignalNumber(const int &n) { coeffs = Vector::Ones(n); } | |
922 | |||
923 | ✗ | inline void initialize(Base *ent, Entity::CommandMap_t &) { | |
924 | ✗ | entity = ent; | |
925 | ✗ | entity->setSignalNumber(2); | |
926 | } | ||
927 | |||
928 | ✗ | inline std::string getDocString() const { | |
929 | return "Linear combination of inputs\n" | ||
930 | " - input " + | ||
931 | VariadicOpHeader<T, T>::nameTypeIn() + | ||
932 | "\n" | ||
933 | " - output " + | ||
934 | VariadicOpHeader<T, T>::nameTypeOut() + | ||
935 | "\n" | ||
936 | " sout = sum ([coeffs[i] * sin[i] for i in range(n) ])\n" | ||
937 | ✗ | " Coefficients are set by commands, default value is 1.\n"; | |
938 | } | ||
939 | }; | ||
940 | |||
941 | /* --- MULTIPLICATION --------------------------------------------------- */ | ||
942 | template <typename T> | ||
943 | struct Multiplier : public VariadicOpHeader<T, T> { | ||
944 | typedef VariadicOp<Multiplier> Base; | ||
945 | |||
946 | ✗ | inline void operator()(const std::vector<const T *> &vs, T &res) const { | |
947 | ✗ | if (vs.size() == 0) | |
948 | ✗ | setIdentity(res); | |
949 | else { | ||
950 | ✗ | res = *vs[0]; | |
951 | ✗ | for (std::size_t i = 1; i < vs.size(); ++i) res *= *vs[i]; | |
952 | } | ||
953 | } | ||
954 | |||
955 | ✗ | inline void setIdentity(T &res) const { res.setIdentity(); } | |
956 | |||
957 | ✗ | inline void initialize(Base *ent, Entity::CommandMap_t &) { | |
958 | ✗ | ent->setSignalNumber(2); | |
959 | } | ||
960 | }; | ||
961 | template <> | ||
962 | ✗ | inline void Multiplier<double>::setIdentity(double &res) const { | |
963 | ✗ | res = 1; | |
964 | } | ||
965 | template <> | ||
966 | ✗ | inline void Multiplier<MatrixHomogeneous>::operator()( | |
967 | const std::vector<const MatrixHomogeneous *> &vs, | ||
968 | MatrixHomogeneous &res) const { | ||
969 | ✗ | if (vs.size() == 0) | |
970 | ✗ | setIdentity(res); | |
971 | else { | ||
972 | ✗ | res = *vs[0]; | |
973 | ✗ | for (std::size_t i = 1; i < vs.size(); ++i) res = res * *vs[i]; | |
974 | } | ||
975 | } | ||
976 | template <> | ||
977 | ✗ | inline void Multiplier<Vector>::operator()( | |
978 | const std::vector<const Vector *> &vs, Vector &res) const { | ||
979 | ✗ | if (vs.size() == 0) | |
980 | ✗ | res.resize(0); | |
981 | else { | ||
982 | ✗ | res = *vs[0]; | |
983 | ✗ | for (std::size_t i = 1; i < vs.size(); ++i) res.array() *= vs[i]->array(); | |
984 | } | ||
985 | } | ||
986 | |||
987 | /* --- BOOLEAN --------------------------------------------------------- */ | ||
988 | template <int operation> | ||
989 | struct BoolOp : public VariadicOpHeader<bool, bool> { | ||
990 | typedef VariadicOp<BoolOp> Base; | ||
991 | |||
992 | ✗ | inline void operator()(const std::vector<const bool *> &vs, bool &res) const { | |
993 | // TODO computation could be optimized with lazy evaluation of the | ||
994 | // signals. When the output result is know, the remaining signals are | ||
995 | // not computed. | ||
996 | ✗ | if (vs.size() == 0) return; | |
997 | ✗ | res = *vs[0]; | |
998 | ✗ | for (std::size_t i = 1; i < vs.size(); ++i) switch (operation) { | |
999 | ✗ | case 0: | |
1000 | ✗ | if (!res) return; | |
1001 | ✗ | res = *vs[i]; | |
1002 | ✗ | break; | |
1003 | ✗ | case 1: | |
1004 | ✗ | if (res) return; | |
1005 | ✗ | res = *vs[i]; | |
1006 | ✗ | break; | |
1007 | } | ||
1008 | } | ||
1009 | }; | ||
1010 | |||
1011 | } // namespace sot | ||
1012 | } // namespace dynamicgraph | ||
1013 | |||
1014 | /* --- TODO ------------------------------------------------------------------*/ | ||
1015 | // The following commented lines are sot-v1 entities that are still waiting | ||
1016 | // for conversion. Help yourself! | ||
1017 | |||
1018 | // /* -------------------------------------------------------------------------- | ||
1019 | // */ | ||
1020 | |||
1021 | // struct WeightedDirection | ||
1022 | // { | ||
1023 | // public: | ||
1024 | // void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector& | ||
1025 | // v2,dynamicgraph::Vector& res ) const | ||
1026 | // { | ||
1027 | // const double norm1 = v1.norm(); | ||
1028 | // const double norm2 = v2.norm(); | ||
1029 | // res=v2; res*=norm1; | ||
1030 | // res*= (1/norm2); | ||
1031 | // } | ||
1032 | // }; | ||
1033 | // typedef BinaryOp< Vector,Vector,Vector,WeightedDirection > weightdir; | ||
1034 | // SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E(weightdir,vector,weight_dir,"WeightDir") | ||
1035 | |||
1036 | // /* -------------------------------------------------------------------------- | ||
1037 | // */ | ||
1038 | |||
1039 | // struct Nullificator | ||
1040 | // { | ||
1041 | // public: | ||
1042 | // void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector& | ||
1043 | // v2,dynamicgraph::Vector& res ) const | ||
1044 | // { | ||
1045 | // const unsigned int s = std::max( v1.size(),v2.size() ); | ||
1046 | // res.resize(s); | ||
1047 | // for( unsigned int i=0;i<s;++i ) | ||
1048 | // { | ||
1049 | // if( v1(i)>v2(i) ) res(i)=v1(i)-v2(i); | ||
1050 | // else if( v1(i)<-v2(i) ) res(i)=v1(i)+v2(i); | ||
1051 | // else res(i)=0; | ||
1052 | // } | ||
1053 | // } | ||
1054 | // }; | ||
1055 | // typedef BinaryOp< Vector,Vector,Vector,Nullificator > vectNil; | ||
1056 | // SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E(vectNil,vector,vectnil_,"Nullificator") | ||
1057 | |||
1058 | // /* -------------------------------------------------------------------------- | ||
1059 | // */ | ||
1060 | |||
1061 | // struct VirtualSpring | ||
1062 | // { | ||
1063 | // public: | ||
1064 | // double spring; | ||
1065 | |||
1066 | // void operator()( const dynamicgraph::Vector& pos,const | ||
1067 | // dynamicgraph::Vector& ref,dynamicgraph::Vector& res ) const | ||
1068 | // { | ||
1069 | // double norm = ref.norm(); | ||
1070 | // double dist = ref.scalarProduct(pos) / (norm*norm); | ||
1071 | |||
1072 | // res.resize( ref.size() ); | ||
1073 | // res = ref; res *= dist; res -= pos; | ||
1074 | // res *= spring; | ||
1075 | // } | ||
1076 | // }; | ||
1077 | // typedef BinaryOp< Vector,Vector,Vector,VirtualSpring > virtspring; | ||
1078 | // SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E_CMD | ||
1079 | // (virtspring,vector,virtspring_, | ||
1080 | // "VirtualSpring" | ||
1081 | // ,else if( cmdLine=="spring" ){ CMDARGS_INOUT(op.spring); } | ||
1082 | // ,"VirtualSpring<pos,ref> compute the virtual force of a spring attache " | ||
1083 | // "to the reference line <ref>. The eq is: k.(<ref|pos>/<ref|ref>.ref-pos)" | ||
1084 | // "Params:\n - spring: get/set the spring factor.") | ||
1085 |