Commit 7761302a authored by Joseph Mirabel's avatar Joseph Mirabel Committed by olivier stasse
Browse files

Add Sot::maxControlIncrementSquaredNorm to avoid generating large controls.

parent 7fb0e1cc
......@@ -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);
......
......@@ -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;
......
......@@ -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);
......
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment