GCC Code Coverage Report

Directory: ./
File: src/sot/sot.cpp
Date: 2024-12-13 12:22:33
Exec Total Coverage
Lines: 0 307 0.0%
Branches: 0 694 0.0%
Line Branch Exec Source
1 /*
2 * Copyright 2010,
3 * François Bleibel,
4 * Olivier Stasse,
5 *
6 * CNRS/AIST
7 *
8 */
9
10 /* --------------------------------------------------------------------- */
11 /* --- INCLUDE --------------------------------------------------------- */
12 /* --------------------------------------------------------------------- */
13
14 //#define VP_DEBUG
15 //#define VP_DEBUG_MODE 45
16 #include <sot/core/debug.hh>
17
18 /* SOT */
19 #ifdef VP_DEBUG
20 class sotSOT__INIT {
21 public:
22 sotSOT__INIT(void) { dynamicgraph::sot::DebugTrace::openFile(); }
23 };
24 sotSOT__INIT sotSOT_initiator;
25 #endif //#ifdef VP_DEBUG
26
27 #include <dynamic-graph/command-direct-getter.h>
28 #include <dynamic-graph/command-direct-setter.h>
29
30 #include <sot/core/factory.hh>
31 #include <sot/core/feature-posture.hh>
32 #include <sot/core/matrix-geometry.hh>
33 #include <sot/core/matrix-svd.hh>
34 #include <sot/core/memory-task-sot.hh>
35 #include <sot/core/pool.hh>
36 #include <sot/core/sot.hh>
37 #include <sot/core/task.hh>
38
39 using namespace std;
40 using namespace dynamicgraph::sot;
41 using namespace dynamicgraph;
42
43 #include "../src/sot/sot-command.h"
44
45 /* --------------------------------------------------------------------- */
46 /* --- CLASS ----------------------------------------------------------- */
47 /* --------------------------------------------------------------------- */
48
49 ✗ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sot, "SOT");
50
51 const double Sot::INVERSION_THRESHOLD_DEFAULT = 1e-4;
52 const Eigen::IOFormat python(Eigen::FullPrecision, 0,
53 ", ", // coeff sep
54 ",\n", // row sep
55 "[", "]", // row prefix and suffix
56 "[", "]" // mat prefix and suffix
57 );
58
59 /* --------------------------------------------------------------------- */
60 /* --- CONSTRUCTION ---------------------------------------------------- */
61 /* --------------------------------------------------------------------- */
62 ✗ Sot::Sot(const std::string &name)
63 : Entity(name),
64 ✗ stack(),
65 ✗ nbJoints(0),
66 ✗ enablePostureTaskAcceleration(false),
67 ✗ maxControlIncrementSquaredNorm(std::numeric_limits<double>::max()),
68 ✗ q0SIN(NULL, "sotSOT(" + name + ")::input(double)::q0"),
69 ✗ proj0SIN(NULL, "sotSOT(" + name + ")::input(double)::proj0"),
70 ✗ inversionThresholdSIN(NULL,
71 ✗ "sotSOT(" + name + ")::input(double)::damping"),
72 ✗ controlSOUT(boost::bind(&Sot::computeControlLaw, this, _1, _2),
73 ✗ inversionThresholdSIN << q0SIN << proj0SIN,
74 ✗ "sotSOT(" + name + ")::output(vector)::control") {
75 ✗ inversionThresholdSIN = INVERSION_THRESHOLD_DEFAULT;
76
77 ✗ signalRegistration(inversionThresholdSIN << controlSOUT << q0SIN << proj0SIN);
78
79 // Commands
80 //
81 ✗ std::string docstring;
82
83 docstring =
84 " \n"
85 " setNumberDofs.\n"
86 " \n"
87 " Input:\n"
88 " - a positive integer : number of degrees of freedom of "
89 "the robot.\n"
90 ✗ " \n";
91 ✗ addCommand("setSize", new dynamicgraph::command::Setter<Sot, unsigned int>(
92 ✗ *this, &Sot::defineNbDof, docstring));
93
94 docstring =
95 " \n"
96 " getNumberDofs.\n"
97 " \n"
98 " Output:\n"
99 " - a positive integer : number of degrees of freedom of "
100 "the robot.\n"
101 ✗ " \n";
102 ✗ addCommand("getSize",
103 new dynamicgraph::command::Getter<Sot, const unsigned int &>(
104 ✗ *this, &Sot::getNbDof, docstring));
105
106 ✗ addCommand("enablePostureTaskAcceleration",
107 ✗ dynamicgraph::command::makeDirectSetter(
108 *this, &enablePostureTaskAcceleration,
109 ✗ dynamicgraph::command::docDirectSetter(
110 "option to bypass SVD computation for the posture task at "
111 "the last"
112 "level",
113 "boolean")));
114
115 ✗ addCommand("isPostureTaskAccelerationEnabled",
116 ✗ dynamicgraph::command::makeDirectGetter(
117 *this, &enablePostureTaskAcceleration,
118 ✗ dynamicgraph::command::docDirectGetter(
119 "option to bypass SVD computation for the posture task at "
120 "the last"
121 "level",
122 "boolean")));
123
124 docstring =
125 " \n"
126 " Maximum allowed squared norm of control increment.\n"
127 " A task whose control increment is above this value is\n"
128 " discarded. It defaults to the maximum double value.\n"
129 " \n"
130 " WARNING: This is a security feature and is **not** a good\n"
131 " way of adding a proper constraint on the control\n"
132 " generated by SoT.\n"
133 ✗ " \n";
134
135 ✗ addCommand("setMaxControlIncrementSquaredNorm",
136 ✗ dynamicgraph::command::makeDirectSetter(
137 *this, &maxControlIncrementSquaredNorm,
138 ✗ docstring + " Input:\n"
139 " - a strictly positive double\n"
140 " \n"));
141
142 ✗ addCommand("getMaxControlIncrementSquaredNorm",
143 ✗ dynamicgraph::command::makeDirectGetter(
144 *this, &maxControlIncrementSquaredNorm,
145 ✗ docstring + " Output:\n"
146 " - a double\n"
147 " \n"));
148
149 docstring =
150 " \n"
151 " push a task into the stack.\n"
152 " \n"
153 " Input:\n"
154 " - a string : Name of the task.\n"
155 ✗ " \n";
156 ✗ addCommand("push", new command::classSot::Push(*this, docstring));
157
158 docstring =
159 " \n"
160 " remove a task into the stack.\n"
161 " \n"
162 " Input:\n"
163 " - a string : Name of the task.\n"
164 ✗ " \n";
165 ✗ addCommand("remove", new command::classSot::Remove(*this, docstring));
166
167 docstring =
168 " \n"
169 " up a task into the stack.\n"
170 " \n"
171 " Input:\n"
172 " - a string : Name of the task.\n"
173 ✗ " \n";
174 ✗ addCommand("up", new command::classSot::Up(*this, docstring));
175
176 docstring =
177 " \n"
178 " down a task into the stack.\n"
179 " \n"
180 " Input:\n"
181 " - a string : Name of the task.\n"
182 ✗ " \n";
183 ✗ addCommand("down", new command::classSot::Down(*this, docstring));
184
185 // Display
186 docstring =
187 " \n"
188 " display the list of tasks pushed inside the stack.\n"
189 ✗ " \n";
190 ✗ addCommand("display", new command::classSot::Display(*this, docstring));
191
192 // Clear
193 docstring =
194 " \n"
195 " clear the list of tasks pushed inside the stack.\n"
196 ✗ " \n";
197 ✗ addCommand("clear", new command::classSot::Clear(*this, docstring));
198
199 // List
200 docstring =
201 " \n"
202 " returns the list of tasks pushed inside the stack.\n"
203 ✗ " \n";
204 ✗ addCommand("list", new command::classSot::List(*this, docstring));
205 }
206
207 /* --------------------------------------------------------------------- */
208 /* --- STACK MANIPULATION --- */
209 /* --------------------------------------------------------------------- */
210 ✗ void Sot::push(TaskAbstract &task) {
211 ✗ if (nbJoints == 0)
212 ✗ throw std::logic_error("Set joint size of " + getClassName() + " \"" +
213 ✗ getName() + "\" first");
214 ✗ stack.push_back(&task);
215 ✗ controlSOUT.addDependency(task.taskSOUT);
216 ✗ controlSOUT.addDependency(task.jacobianSOUT);
217 ✗ controlSOUT.setReady();
218 }
219 ✗ TaskAbstract &Sot::pop(void) {
220 ✗ TaskAbstract *res = stack.back();
221 ✗ stack.pop_back();
222 ✗ controlSOUT.removeDependency(res->taskSOUT);
223 ✗ controlSOUT.removeDependency(res->jacobianSOUT);
224 ✗ controlSOUT.setReady();
225 ✗ return *res;
226 }
227 ✗ bool Sot::exist(const TaskAbstract &key) {
228 ✗ StackType::iterator it;
229 ✗ for (it = stack.begin(); stack.end() != it; ++it) {
230 ✗ if (*it == &key) {
231 ✗ return true;
232 }
233 }
234 ✗ return false;
235 }
236 ✗ void Sot::remove(const TaskAbstract &key) {
237 ✗ bool find = false;
238 ✗ StackType::iterator it;
239 ✗ for (it = stack.begin(); stack.end() != it; ++it) {
240 ✗ if (*it == &key) {
241 ✗ find = true;
242 ✗ break;
243 }
244 }
245 ✗ if (!find) {
246 ✗ return;
247 }
248
249 ✗ stack.erase(it);
250 ✗ removeDependency(key);
251 }
252
253 ✗ void Sot::removeDependency(const TaskAbstract &key) {
254 ✗ controlSOUT.removeDependency(key.taskSOUT);
255 ✗ controlSOUT.removeDependency(key.jacobianSOUT);
256 ✗ controlSOUT.setReady();
257 }
258
259 ✗ void Sot::up(const TaskAbstract &key) {
260 ✗ bool find = false;
261 ✗ StackType::iterator it;
262 ✗ for (it = stack.begin(); stack.end() != it; ++it) {
263 ✗ if (*it == &key) {
264 ✗ find = true;
265 ✗ break;
266 }
267 }
268 ✗ if (stack.begin() == it) {
269 ✗ return;
270 }
271 ✗ if (!find) {
272 ✗ return;
273 }
274
275 ✗ StackType::iterator pos = it;
276 ✗ pos--;
277 ✗ TaskAbstract *task = *it;
278 ✗ stack.erase(it);
279 ✗ stack.insert(pos, task);
280 ✗ controlSOUT.setReady();
281 }
282 ✗ void Sot::down(const TaskAbstract &key) {
283 ✗ bool find = false;
284 ✗ StackType::iterator it;
285 ✗ for (it = stack.begin(); stack.end() != it; ++it) {
286 ✗ if (*it == &key) {
287 ✗ find = true;
288 ✗ break;
289 }
290 }
291 ✗ if (stack.end() == it) {
292 ✗ return;
293 }
294 ✗ if (!find) {
295 ✗ return;
296 }
297
298 ✗ StackType::iterator pos = it;
299 ✗ pos++;
300 ✗ TaskAbstract *task = *it;
301 ✗ stack.erase(it);
302 ✗ if (stack.end() == pos) {
303 ✗ stack.push_back(task);
304 } else {
305 ✗ pos++;
306 ✗ stack.insert(pos, task);
307 }
308 ✗ controlSOUT.setReady();
309 }
310
311 ✗ void Sot::clear(void) {
312 ✗ for (StackType::iterator it = stack.begin(); stack.end() != it; ++it) {
313 ✗ removeDependency(**it);
314 }
315 ✗ stack.clear();
316 ✗ controlSOUT.setReady();
317 }
318
319 ✗ void Sot::defineNbDof(const unsigned int &nbDof) {
320 ✗ nbJoints = nbDof;
321 ✗ controlSOUT.setReady();
322 }
323
324 /* --------------------------------------------------------------------- */
325 /* --------------------------------------------------------------------- */
326 /* --------------------------------------------------------------------- */
327
328 ✗ const Matrix &computeJacobianActivated(TaskAbstract *Ta, Task *T, Matrix &Jmem,
329 const int &iterTime) {
330 ✗ if (T != NULL) {
331 ✗ const Flags &controlSelec = T->controlSelectionSIN(iterTime);
332 sotDEBUG(25) << "Control selection = " << controlSelec << endl;
333 ✗ if (controlSelec) {
334 ✗ if (!controlSelec) {
335 ✗ Jmem = Ta->jacobianSOUT.accessCopy();
336 ✗ for (int i = 0; i < Jmem.cols(); ++i)
337 ✗ if (!controlSelec(i)) Jmem.col(i).setZero();
338 ✗ return Jmem;
339 } else
340 ✗ return Ta->jacobianSOUT.accessCopy();
341 } else {
342 sotDEBUG(15) << "Task not activated." << endl;
343 ✗ const Matrix &Jac = Ta->jacobianSOUT.accessCopy();
344 ✗ Jmem = Matrix::Zero(Jac.rows(), Jac.cols());
345 ✗ return Jmem;
346 }
347 } else /* No selection specification: nothing to do. */
348 ✗ return Ta->jacobianSOUT.accessCopy();
349 }
350
351 typedef MemoryTaskSOT::Kernel_t Kernel_t;
352 typedef MemoryTaskSOT::KernelConst_t KernelConst_t;
353
354 template <typename MapType, typename MatrixType>
355 ✗ inline void makeMap(MapType &map, MatrixType &m) {
356 // There is not memory allocation here.
357 // See https://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
358 ✗ new (&map) KernelConst_t(m.data(), m.rows(), m.cols());
359 }
360
361 ✗ bool updateControl(MemoryTaskSOT *mem, const Matrix::Index rankJ,
362 bool has_kernel, const KernelConst_t &kernel,
363 Vector &control, const double &threshold) {
364 ✗ const SVD_t &svd(mem->svd);
365 ✗ Vector &tmpTask(mem->tmpTask);
366 ✗ Vector &tmpVar(mem->tmpVar);
367 ✗ Vector &tmpControl(mem->tmpControl);
368 ✗ const Vector &err(mem->err);
369
370 // tmpTask <- S^-1 * U^T * err
371 ✗ tmpTask.head(rankJ).noalias() = svd.matrixU().leftCols(rankJ).adjoint() * err;
372 ✗ tmpTask.head(rankJ).array() *=
373 ✗ svd.singularValues().head(rankJ).array().inverse();
374
375 // control <- kernel * (V * S^-1 * U^T * err)
376 ✗ if (has_kernel) {
377 ✗ tmpVar.head(kernel.cols()).noalias() =
378 ✗ svd.matrixV().leftCols(rankJ) * tmpTask.head(rankJ);
379 ✗ tmpControl.noalias() = kernel * tmpVar.head(kernel.cols());
380 } else
381 ✗ tmpControl.noalias() = svd.matrixV().leftCols(rankJ) * tmpTask.head(rankJ);
382 ✗ if (tmpControl.squaredNorm() > threshold) return false;
383 ✗ control += tmpControl;
384 ✗ return true;
385 }
386
387 ✗ bool isFullPostureTask(Task *task, const Matrix::Index &nDof,
388 const int &iterTime) {
389 ✗ if (task == NULL || task->getFeatureList().size() != 1 ||
390 ✗ !task->controlSelectionSIN(iterTime))
391 ✗ return false;
392 FeaturePosture *posture =
393 ✗ dynamic_cast<FeaturePosture *>(task->getFeatureList().front());
394
395 ✗ assert(posture->dimensionSOUT(iterTime) <= nDof - 6);
396 ✗ return posture != NULL && posture->dimensionSOUT(iterTime) == nDof - 6;
397 }
398
399 ✗ MemoryTaskSOT *getMemory(TaskAbstract &t, const Matrix::Index &tDim,
400 const Matrix::Index &nDof) {
401 ✗ MemoryTaskSOT *mem = dynamic_cast<MemoryTaskSOT *>(t.memoryInternal);
402 ✗ if (NULL == mem) {
403 ✗ if (NULL != t.memoryInternal) delete t.memoryInternal;
404 ✗ mem = new MemoryTaskSOT(tDim, nDof);
405 ✗ t.memoryInternal = mem;
406 }
407 ✗ return mem;
408 }
409
410 /* --------------------------------------------------------------------- */
411 /* --- CONTROL --------------------------------------------------------- */
412 /* --------------------------------------------------------------------- */
413
414 //#define WITH_CHRONO
415
416 #ifdef WITH_CHRONO
417 #ifndef WIN32
418 #include <sys/time.h>
419 #else /*WIN32*/
420 #include <sot/core/utils-windows.hh>
421 #endif /*WIN32*/
422 #endif /*WITH_CHRONO*/
423
424 #ifdef WITH_CHRONO
425 #define TIME_STREAM DYNAMIC_GRAPH_ENTITY_DEBUG(*this)
426 #define sotINIT_CHRONO1 \
427 struct timespec t0, t1; \
428 double dt
429 #define sotSTART_CHRONO1 clock_gettime(CLOCK_THREAD_CPUTIME_ID, &t0)
430 #define sotCHRONO1 \
431 clock_gettime(CLOCK_THREAD_CPUTIME_ID, &t1); \
432 dt = ((double)(t1.tv_sec - t0.tv_sec) * 1e6 + \
433 (double)(t1.tv_nsec - t0.tv_nsec) / 1e3); \
434 TIME_STREAM << "dT " << (long int)dt << std::endl
435 #define sotINITPARTCOUNTERS struct timespec tpart0
436 #define sotSTARTPARTCOUNTERS clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tpart0)
437 #define sotCOUNTER(nbc1, nbc2) \
438 clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tpart##nbc2); \
439 dt##nbc2 = ((double)(tpart##nbc2.tv_sec - tpart##nbc1.tv_sec) * 1e6 + \
440 (double)(tpart##nbc2.tv_nsec - tpart##nbc1.tv_nsec) / 1e3)
441 #define sotINITCOUNTER(nbc1) \
442 struct timespec tpart##nbc1; \
443 double dt##nbc1 = 0;
444 #define sotPRINTCOUNTER(nbc1) \
445 TIME_STREAM << "dt" << iterTask << '_' << nbc1 << ' ' << (long int)dt##nbc1 \
446 << ' '
447 #else // #ifdef WITH_CHRONO
448 #define sotINIT_CHRONO1
449 #define sotSTART_CHRONO1
450 #define sotCHRONO1
451 #define sotINITPARTCOUNTERS
452 #define sotSTARTPARTCOUNTERS
453 #define sotCOUNTER(nbc1, nbc2)
454 #define sotINITCOUNTER(nbc1)
455 #define sotPRINTCOUNTER(nbc1)
456 #endif // #ifdef WITH_CHRONO
457
458 ✗ void Sot::taskVectorToMlVector(const VectorMultiBound &taskVector,
459 Vector &res) {
460 ✗ res.resize(taskVector.size());
461 ✗ unsigned int i = 0;
462
463 ✗ for (VectorMultiBound::const_iterator iter = taskVector.begin();
464 ✗ iter != taskVector.end(); ++iter, ++i) {
465 ✗ res(i) = iter->getSingleBound();
466 }
467 }
468
469 ✗ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
470 const int &iterTime) {
471 sotDEBUGIN(15);
472
473 sotINIT_CHRONO1;
474 sotINITPARTCOUNTERS;
475 sotINITCOUNTER(1);
476 sotINITCOUNTER(2);
477 sotINITCOUNTER(3);
478 sotINITCOUNTER(4);
479 sotINITCOUNTER(5);
480 sotINITCOUNTER(6);
481
482 sotSTART_CHRONO1;
483
484 ✗ const double &th = inversionThresholdSIN(iterTime);
485
486 ✗ bool controlIsZero = true;
487 ✗ if (q0SIN.isPlugged()) {
488 ✗ control = q0SIN(iterTime);
489 ✗ controlIsZero = false;
490 ✗ if (control.size() != nbJoints) {
491 ✗ std::ostringstream oss;
492 ✗ oss << "SOT(" << getName() << "): q0SIN value length is "
493 ✗ << control.size() << "while the expected lenth is " << nbJoints;
494 ✗ throw std::length_error(oss.str());
495 }
496 } else {
497 ✗ if (stack.size() == 0) {
498 ✗ std::ostringstream oss;
499 ✗ oss << "SOT(" << getName()
500 ✗ << ") contains no task and q0SIN is not plugged.";
501 ✗ throw std::logic_error(oss.str());
502 }
503 ✗ control = Vector::Zero(nbJoints);
504 sotDEBUG(25) << "No initial velocity." << endl;
505 }
506
507 ✗ sotDEBUGF(5, " --- Time %d -------------------", iterTime);
508 ✗ unsigned int iterTask = 0;
509 ✗ KernelConst_t kernel(NULL, 0, 0);
510 ✗ bool has_kernel = false;
511 // Get initial projector if any.
512 ✗ if (proj0SIN.isPlugged()) {
513 ✗ const Matrix &K = proj0SIN.access(iterTime);
514 ✗ if (K.rows() == nbJoints) {
515 ✗ makeMap(kernel, K);
516 ✗ has_kernel = true;
517 } else {
518 ✗ DYNAMIC_GRAPH_ENTITY_ERROR_STREAM(*this)
519 ✗ << "Projector of " << getName() << " has " << K.rows()
520 ✗ << " rows while " << nbJoints << " expected.\n";
521 }
522 }
523 ✗ for (StackType::iterator iter = stack.begin(); iter != stack.end(); ++iter) {
524 sotSTARTPARTCOUNTERS;
525
526 ✗ sotDEBUGF(5, "Rank %d.", iterTask);
527 ✗ TaskAbstract &taskA = **iter;
528 ✗ Task *task = dynamic_cast<Task *>(*iter);
529
530 ✗ bool last = (iterTask + 1 == stack.size());
531 ✗ bool fullPostureTask = (last && enablePostureTaskAcceleration &&
532 ✗ isFullPostureTask(task, nbJoints, iterTime));
533
534 sotDEBUG(15) << "Task: e_" << taskA.getName() << std::endl;
535
536 /// Computing first the jacobian may be a little faster overall.
537 ✗ if (!fullPostureTask) taskA.jacobianSOUT.recompute(iterTime);
538 ✗ taskA.taskSOUT.recompute(iterTime);
539 ✗ const Matrix::Index dim = taskA.taskSOUT.accessCopy().size();
540 sotCOUNTER(0, 1); // Direct Dynamic
541
542 /* Init memory. */
543 ✗ MemoryTaskSOT *mem = getMemory(taskA, dim, nbJoints);
544 /***/ sotCOUNTER(1, 2); // first allocs
545
546 ✗ Matrix::Index rankJ = -1;
547 ✗ taskVectorToMlVector(taskA.taskSOUT(iterTime), mem->err);
548
549 ✗ if (fullPostureTask) {
550 /***/ sotCOUNTER(2, 3); // compute JK*S
551 /***/ sotCOUNTER(3, 4); // compute Jt
552
553 // Jp = kernel.transpose()
554 ✗ rankJ = kernel.cols();
555 /***/ sotCOUNTER(4, 5); // SVD and rank
556
557 /* --- COMPUTE QDOT AND P --- */
558 ✗ if (!controlIsZero) mem->err.noalias() -= control.tail(nbJoints - 6);
559 ✗ mem->tmpVar.head(rankJ).noalias() =
560 ✗ kernel.transpose().rightCols(nbJoints - 6) * mem->err;
561 ✗ control.noalias() += kernel * mem->tmpVar.head(rankJ);
562 ✗ controlIsZero = false;
563 } else {
564 ✗ assert(taskA.jacobianSOUT.accessCopy().cols() == nbJoints);
565
566 /* --- COMPUTE S * JK --- */
567 const Matrix &JK =
568 ✗ computeJacobianActivated(&taskA, task, mem->JK, iterTime);
569 /***/ sotCOUNTER(2, 3); // compute JK*S
570
571 /* --- COMPUTE Jt --- */
572 ✗ const Matrix *Jt = &mem->Jt;
573 ✗ if (has_kernel)
574 ✗ mem->Jt.noalias() = JK * kernel;
575 else
576 ✗ Jt = &JK;
577 /***/ sotCOUNTER(3, 4); // compute Jt
578
579 /* --- SVD and RANK--- */
580 ✗ SVD_t &svd = mem->svd;
581 ✗ if (last)
582 ✗ svd.compute(*Jt, Eigen::ComputeThinU | Eigen::ComputeThinV);
583 else
584 ✗ svd.compute(*Jt, Eigen::ComputeThinU | Eigen::ComputeFullV);
585 ✗ rankJ = 0;
586 ✗ while (rankJ < svd.singularValues().size() &&
587 ✗ th < svd.singularValues()[rankJ])
588 ✗ ++rankJ;
589 /***/ sotCOUNTER(4, 5); // SVD and rank
590
591 /* --- COMPUTE QDOT AND P --- */
592 ✗ if (!controlIsZero) mem->err.noalias() -= JK * control;
593
594 ✗ bool success = updateControl(mem, rankJ, has_kernel, kernel, control,
595 ✗ maxControlIncrementSquaredNorm);
596 ✗ if (success) {
597 ✗ controlIsZero = false;
598
599 ✗ if (!last) {
600 ✗ Matrix::Index cols = svd.matrixV().cols() - rankJ;
601 ✗ if (has_kernel)
602 ✗ mem->getKernel(nbJoints, cols).noalias() =
603 ✗ kernel * svd.matrixV().rightCols(cols);
604 else
605 ✗ mem->getKernel(nbJoints, cols).noalias() =
606 ✗ svd.matrixV().rightCols(cols);
607 ✗ makeMap(kernel, mem->kernel);
608 ✗ has_kernel = true;
609 }
610 } else {
611 ✗ DYNAMIC_GRAPH_ENTITY_ERROR(*this)
612 ✗ << iterTime << ": SOT " << getName() << " disabled task "
613 ✗ << taskA.getName() << " at level " << iterTask
614 ✗ << " because norm exceeded the limit.\n";
615 ✗ DYNAMIC_GRAPH_ENTITY_DEBUG(*this)
616 ✗ << "control = " << control.transpose().format(python)
617 ✗ << "\nJ = " << JK.format(python)
618 ✗ << "\nerr - J * control = " << mem->err.transpose().format(python)
619 ✗ << "\nJ * kernel = " << Jt->format(python) << "\ncontrol_update = "
620 ✗ << mem->tmpControl.transpose().format(python) << '\n';
621 }
622 }
623 /***/ sotCOUNTER(5, 6); // QDOT + Projector
624
625 sotDEBUG(2) << "Proj non optimal (rankJ= " << rankJ
626 << ", iterTask =" << iterTask << ")";
627
628 ✗ iterTask++;
629
630 sotPRINTCOUNTER(1);
631 sotPRINTCOUNTER(2);
632 sotPRINTCOUNTER(3);
633 sotPRINTCOUNTER(4);
634 sotPRINTCOUNTER(5);
635 sotPRINTCOUNTER(6);
636 ✗ if (last || kernel.cols() == 0) break;
637 }
638
639 sotCHRONO1;
640
641 sotDEBUGOUT(15);
642 ✗ return control;
643 }
644
645 /* --------------------------------------------------------------------- */
646 /* --- DISPLAY --------------------------------------------------------- */
647 /* --------------------------------------------------------------------- */
648 ✗ void Sot::display(std::ostream &os) const {
649 ✗ os << "+-----------------" << std::endl
650 ✗ << "+ SOT " << std::endl
651 ✗ << "+-----------------" << std::endl;
652 ✗ for (StackType::const_iterator it = this->stack.begin();
653 ✗ this->stack.end() != it; ++it) {
654 ✗ os << "| " << (*it)->getName() << std::endl;
655 }
656 ✗ os << "+-----------------" << std::endl;
657 }
658
659 ✗ std::ostream &operator<<(std::ostream &os, const Sot &sot) {
660 ✗ sot.display(os);
661 ✗ return os;
662 }
663
664 /* --------------------------------------------------------------------- */
665 /* --- COMMAND --------------------------------------------------------- */
666 /* --------------------------------------------------------------------- */
667
668 ✗ std::ostream &Sot::writeGraph(std::ostream &os) const {
669 ✗ StackType::const_iterator iter;
670 ✗ for (iter = stack.begin(); iter != stack.end(); ++iter) {
671 ✗ const TaskAbstract &task = **iter;
672 ✗ StackType::const_iterator nextiter = iter;
673 ✗ nextiter++;
674
675 ✗ if (nextiter != stack.end()) {
676 ✗ TaskAbstract &nexttask = **nextiter;
677 ✗ os << "\t\t\t\"" << task.getName() << "\" -> \"" << nexttask.getName()
678 ✗ << "\" [color=red]" << endl;
679 }
680 }
681
682 ✗ os << "\t\tsubgraph cluster_Tasks {" << endl;
683 ✗ os << "\t\t\tsubgraph \"cluster_" << getName() << "\" {" << std::endl;
684 ✗ os << "\t\t\t\tcolor=lightsteelblue1; label=\"" << getName()
685 ✗ << "\"; style=filled;" << std::endl;
686 ✗ for (iter = stack.begin(); iter != stack.end(); ++iter) {
687 ✗ const TaskAbstract &task = **iter;
688 ✗ os << "\t\t\t\t\"" << task.getName() << "\" [ label = \"" << task.getName()
689 ✗ << "\" ," << std::endl
690 << "\t\t\t\t fontcolor = black, color = black, fillcolor = magenta, "
691 ✗ "style=filled, shape=box ]"
692 ✗ << std::endl;
693 }
694 ✗ os << "\t\t\t}" << std::endl;
695 ✗ os << "\t\t\t}" << endl;
696 ✗ return os;
697 }
698