GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
return std::string( |
||
51 |
"Undocumented unary operator\n" |
||
52 |
" - input ") + |
||
53 |
nameTypeIn() + |
||
54 |
std::string( |
||
55 |
"\n" |
||
56 |
" - output ") + |
||
57 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
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 |
✓✓ | 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 |
assert((nr >= 0) && (R + nr <= m.size())); |
72 |
✓✗✓✗ |
2 |
res.segment(r, nr) = m.segment(R, nr); |
73 |
2 |
r += nr; |
|
74 |
} |
||
75 |
✗✓ | 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 |
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 |
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 |
2 |
std::string doc; |
|
96 |
|||
97 |
boost::function<void(const int &, const int &)> setBound = |
||
98 |
✓✗✓✗ |
2 |
boost::bind(&VectorSelecter::setBounds, this, _1, _2); |
99 |
✓✗✓✗ ✓✗✓✗ |
2 |
doc = docCommandVoid2("Set the bound of the selection [m,M[.", "int (min)", |
100 |
1 |
"int (max)"); |
|
101 |
✓✗✓✗ ✓✗✓✗ |
1 |
ADD_COMMAND("selec", makeCommandVoid2(ent, setBound, doc)); |
102 |
boost::function<void(const int &, const int &)> addBound = |
||
103 |
✓✗✓✗ |
1 |
boost::bind(&VectorSelecter::addBounds, this, _1, _2); |
104 |
✓✗✓✗ ✓✗✓✗ |
2 |
doc = docCommandVoid2("Add a segment to be selected [m,M[.", "int (min)", |
105 |
1 |
"int (max)"); |
|
106 |
✓✗✓✗ ✓✗✓✗ |
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 |
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 |
2 |
std::string doc; |
|
124 |
|||
125 |
boost::function<void(const int &)> callback = |
||
126 |
✓✗✓✗ |
1 |
boost::bind(&VectorComponent::setIndex, this, _1); |
127 |
✓✗✓✗ ✓✗ |
2 |
doc = command::docCommandVoid1("Set the index of the component.", |
128 |
1 |
"int (index)"); |
|
129 |
✓✗✓✗ ✓✗✓✗ |
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 |
✓✗ | 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 |
✓✗✓✗ |
1 |
assert((imin <= imax) && (imax <= m.rows())); |
144 |
✓✗✓✗ |
1 |
assert((jmin <= jmax) && (jmax <= m.cols())); |
145 |
1 |
res.resize(imax - imin, jmax - jmin); |
|
146 |
✓✓ | 3 |
for (int i = imin; i < imax; ++i) |
147 |
✓✓ | 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 |
2 |
std::string doc; |
|
167 |
|||
168 |
boost::function<void(const int &, const int &)> setBoundsRow = |
||
169 |
✓✗✓✗ |
2 |
boost::bind(&MatrixSelector::setBoundsRow, this, _1, _2); |
170 |
boost::function<void(const int &, const int &)> setBoundsCol = |
||
171 |
✓✗✓✗ |
1 |
boost::bind(&MatrixSelector::setBoundsCol, this, _1, _2); |
172 |
|||
173 |
✓✗✓✗ ✓✗✓✗ |
1 |
doc = docCommandVoid2("Set the bound on rows.", "int (min)", "int (max)"); |
174 |
✓✗✓✗ ✓✗✓✗ |
1 |
ADD_COMMAND("selecRows", makeCommandVoid2(ent, setBoundsRow, doc)); |
175 |
|||
176 |
✓✗✓✗ ✓✗✓✗ |
2 |
doc = docCommandVoid2("Set the bound on cols [m,M[.", "int (min)", |
177 |
1 |
"int (max)"); |
|
178 |
✓✗✓✗ ✓✗✓✗ |
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 |
✓✗✓✗ |
9 |
Mres.translation() = vect.head<3>(); |
324 |
✓✗✓✗ ✓✗ |
9 |
Mres.linear().row(0) = vect.segment(3, 3); |
325 |
✓✗✓✗ ✓✗ |
9 |
Mres.linear().row(1) = vect.segment(6, 3); |
326 |
✓✗✓✗ ✓✗ |
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 |
✓✗✓✗ |
9 |
res.head<3>() = M.translation(); |
335 |
✓✗✓✗ ✓✗ |
9 |
res.segment(3, 3) = M.linear().row(0); |
336 |
✓✗✓✗ ✓✗ |
9 |
res.segment(6, 3) = M.linear().row(1); |
337 |
✓✗✓✗ ✓✗ |
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 |
✓✗✓✗ |
9 |
Mres.translation() = vect.head<3>(); |
345 |
✓✗✓✗ ✓✗✓✗ |
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 |
✓✗ | 9 |
res.resize(7); |
353 |
✓✗✓✗ ✓✗ |
9 |
res.head<3>() = M.translation(); |
354 |
✓✗✓✗ |
9 |
Eigen::Map<VectorQuaternion> q(res.tail<4>().data()); |
355 |
✓✗✓✗ |
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 |
✓✗✓✗ ✓✗✓✗ |
9 |
res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) * |
454 |
✓✗✓✗ ✓✗✓✗ |
18 |
Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) * |
455 |
✓✗✓✗ |
9 |
Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX())) |
456 |
✓✗ | 9 |
.toRotationMatrix(); |
457 |
9 |
} |
|
458 |
}; |
||
459 |
|||
460 |
struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> { |
||
461 |
9 |
inline void operator()(const MatrixRotation &r, VectorRollPitchYaw &res) { |
|
462 |
✓✗✓✗ |
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 |
✓✗✓✗ ✓✗✓✗ |
9 |
res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) * |
470 |
✓✗✓✗ ✓✗✓✗ |
18 |
Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) * |
471 |
✓✗✓✗ |
9 |
Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX())) |
472 |
✓✗✓✗ |
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 |
✓✗✓✗ ✓✗ |
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.") |
Generated by: GCOVR (Version 4.2) |