GCC Code Coverage Report

Directory: ./
File: src/matrix/operator.hh
Date: 2025-05-13 12:28:21
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