GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/sot/sot.cpp Lines: 0 302 0.0 %
Date: 2023-03-13 12:09:37 Branches: 0 712 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
}