Add Sot::maxControlIncrementSquaredNorm to avoid generating large controls.

include/sot/core/memory-task-sot.hh

@@ -30,7 +30,7 @@ public: //   protected:
       KernelConst_t;
 
   /* Internal memory to reduce the dynamic allocation at task resolution. */
-  dg::Vector err, tmpTask, tmpVar;
+  dg::Vector err, tmpTask, tmpVar, tmpControl;
   dg::Matrix Jt; //( nJ,mJ );
 
   dg::Matrix JK; //(nJ,mJ);

include/sot/core/sot.hh

@@ -79,6 +79,12 @@ protected:
     if this task is a Task with a single FeaturePosture */
   bool enablePostureTaskAcceleration;
 
+  /*! \brief Maximum allowed squared norm of control increment.
+    A task whose control increment is above this value is discarded.
+    It defaults to \c std::numeric_limits<double>::max().
+    */
+  double maxControlIncrementSquaredNorm;
+
 public:
   /*! \brief Threshold to compute the dumped pseudo inverse. */
   static const double INVERSION_THRESHOLD_DEFAULT; // = 1e-4;

src/sot/memory-task-sot.cpp

@@ -22,6 +22,7 @@ void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ) {
   err.resize(nJ);
   tmpTask.resize(nJ);
   tmpVar.resize(mJ);
+  tmpControl.resize(mJ);
   Jt.resize(nJ, mJ);
 
   JK.resize(nJ, mJ);

src/sot/sot.cpp

@@ -49,12 +49,19 @@ using namespace dynamicgraph;
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sot, "SOT");
 
 const double Sot::INVERSION_THRESHOLD_DEFAULT = 1e-4;
+const Eigen::IOFormat python(Eigen::FullPrecision, 0,
+                             ", ",     // coeff sep
+                             ",\n",    // row sep
+                             "[", "]", // row prefix and suffix
+                             "[", "]"  // mat prefix and suffix
+);
 
 /* --------------------------------------------------------------------- */
 /* --- CONSTRUCTION ---------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 Sot::Sot(const std::string &name)
     : Entity(name), stack(), nbJoints(0), enablePostureTaskAcceleration(false),
+      maxControlIncrementSquaredNorm(std::numeric_limits<double>::max()),
       q0SIN(NULL, "sotSOT(" + name + ")::input(double)::q0"),
       proj0SIN(NULL, "sotSOT(" + name + ")::input(double)::proj0"),
       inversionThresholdSIN(NULL,
@@ -109,6 +116,24 @@ Sot::Sot(const std::string &name)
                      "level",
                      "boolean")));
 
+  addCommand("setMaxControlIncrementSquaredNorm",
+             dynamicgraph::command::makeDirectSetter(
+                 *this, &maxControlIncrementSquaredNorm,
+                 dynamicgraph::command::docDirectSetter(
+                     "A task whose control increment squared norm is above "
+                     "this value is discarded."
+                     "squaredNorm",
+                     "double")));
+
+  addCommand("getMaxControlIncrementSquaredNorm",
+             dynamicgraph::command::makeDirectGetter(
+                 *this, &maxControlIncrementSquaredNorm,
+                 dynamicgraph::command::docDirectGetter(
+                     "A task whose control increment squared norm is above "
+                     "this value is discarded."
+                     "squaredNorm",
+                     "double")));
+
   docstring = "    \n"
               "    push a task into the stack.\n"
               "    \n"
@@ -315,12 +340,13 @@ inline void makeMap(MapType &map, MatrixType &m) {
   new (&map) KernelConst_t(m.data(), m.rows(), m.cols());
 }
 
-void updateControl(MemoryTaskSOT *mem, const Matrix::Index rankJ,
+bool updateControl(MemoryTaskSOT *mem, const Matrix::Index rankJ,
                    bool has_kernel, const KernelConst_t &kernel,
-                   Vector &control) {
+                   Vector &control, const double &threshold) {
   const SVD_t &svd(mem->svd);
   Vector &tmpTask(mem->tmpTask);
   Vector &tmpVar(mem->tmpVar);
+  Vector &tmpControl(mem->tmpControl);
   const Vector &err(mem->err);
 
   // tmpTask <- S^-1 * U^T * err
@@ -332,9 +358,13 @@ void updateControl(MemoryTaskSOT *mem, const Matrix::Index rankJ,
   if (has_kernel) {
     tmpVar.head(kernel.cols()).noalias() =
         svd.matrixV().leftCols(rankJ) * tmpTask.head(rankJ);
-    control.noalias() += kernel * tmpVar.head(kernel.cols());
+    tmpControl.noalias() = kernel * tmpVar.head(kernel.cols());
   } else
-    control.noalias() += svd.matrixV().leftCols(rankJ) * tmpTask.head(rankJ);
+    tmpControl.noalias() = svd.matrixV().leftCols(rankJ) * tmpTask.head(rankJ);
+  if (tmpControl.squaredNorm() > threshold)
+    return false;
+  control += tmpControl;
+  return true;
 }
 
 bool isFullPostureTask(Task *task, const Matrix::Index &nDof,
@@ -541,19 +571,33 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
       if (!controlIsZero)
         mem->err.noalias() -= JK * control;
 
-      updateControl(mem, rankJ, has_kernel, kernel, control);
-      controlIsZero = false;
-
-      if (!last) {
-        Matrix::Index cols = svd.matrixV().cols() - rankJ;
-        if (has_kernel)
-          mem->getKernel(nbJoints, cols).noalias() =
-              kernel * svd.matrixV().rightCols(cols);
-        else
-          mem->getKernel(nbJoints, cols).noalias() =
-              svd.matrixV().rightCols(cols);
-        makeMap(kernel, mem->kernel);
-        has_kernel = true;
+      bool success = updateControl(mem, rankJ, has_kernel, kernel, control,
+                                   maxControlIncrementSquaredNorm);
+      if (success) {
+        controlIsZero = false;
+
+        if (!last) {
+          Matrix::Index cols = svd.matrixV().cols() - rankJ;
+          if (has_kernel)
+            mem->getKernel(nbJoints, cols).noalias() =
+                kernel * svd.matrixV().rightCols(cols);
+          else
+            mem->getKernel(nbJoints, cols).noalias() =
+                svd.matrixV().rightCols(cols);
+          makeMap(kernel, mem->kernel);
+          has_kernel = true;
+        }
+      } else {
+        DYNAMIC_GRAPH_ENTITY_ERROR(*this)
+            << iterTime << ": SOT " << getName() << " disabled task "
+            << taskA.getName() << " at level " << iterTask
+            << " because norm exceeded the limit.\n";
+        DYNAMIC_GRAPH_ENTITY_DEBUG(*this)
+            << "control = " << control.transpose().format(python)
+            << "\nJ = " << JK.format(python)
+            << "\nerr - J * control = " << mem->err.transpose().format(python)
+            << "\nJ * kernel = " << Jt->format(python) << "\ncontrol_update = "
+            << mem->tmpControl.transpose().format(python) << '\n';
       }
     }
     /***/ sotCOUNTER(5, 6); // QDOT + Projector