24 sotSOT__INIT sotSOT_initiator;
25 #endif // #ifdef VP_DEBUG 43 #include "../src/sot/sot-command.h" 51 const double Sot::INVERSION_THRESHOLD_DEFAULT = 1e-4;
52 const Eigen::IOFormat
python(Eigen::FullPrecision, 0,
62 Sot::Sot(
const std::string &
name)
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") {
81 std::string docstring;
88 " - a positive integer : number of degrees of freedom of " 99 " - a positive integer : number of degrees of freedom of " 110 "option to bypass SVD computation for the posture task at " 115 addCommand(
"isPostureTaskAccelerationEnabled",
119 "option to bypass SVD computation for the posture task at " 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" 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" 135 addCommand(
"setMaxControlIncrementSquaredNorm",
138 docstring +
" Input:\n" 139 " - a strictly positive double\n" 142 addCommand(
"getMaxControlIncrementSquaredNorm",
145 docstring +
" Output:\n" 151 " push a task into the stack.\n" 154 " - a string : Name of the task.\n" 160 " remove a task into the stack.\n" 163 " - a string : Name of the task.\n" 169 " up a task into the stack.\n" 172 " - a string : Name of the task.\n" 178 " down a task into the stack.\n" 181 " - a string : Name of the task.\n" 188 " display the list of tasks pushed inside the stack.\n" 195 " clear the list of tasks pushed inside the stack.\n" 202 " returns the list of tasks pushed inside the stack.\n" 212 throw std::logic_error(
"Set joint size of " +
getClassName() +
" \"" +
214 stack.push_back(&task);
228 StackType::iterator it;
229 for (it =
stack.begin();
stack.end() != it; ++it) {
238 StackType::iterator it;
239 for (it =
stack.begin();
stack.end() != it; ++it) {
261 StackType::iterator it;
262 for (it =
stack.begin();
stack.end() != it; ++it) {
268 if (
stack.begin() == it) {
275 StackType::iterator
pos = it;
279 stack.insert(pos, task);
284 StackType::iterator it;
285 for (it =
stack.begin();
stack.end() != it; ++it) {
291 if (
stack.end() == it) {
298 StackType::iterator
pos = it;
302 if (
stack.end() == pos) {
303 stack.push_back(task);
306 stack.insert(pos, task);
312 for (StackType::iterator it =
stack.begin();
stack.end() != it; ++it) {
329 const int &iterTime) {
332 sotDEBUG(25) <<
"Control selection = " << controlSelec << endl;
336 for (
int i = 0;
i < Jmem.cols(); ++
i)
337 if (!controlSelec(
i)) Jmem.col(
i).setZero();
342 sotDEBUG(15) <<
"Task not activated." << endl;
344 Jmem = Matrix::Zero(Jac.rows(), Jac.cols());
354 template <
typename MapType,
typename MatrixType>
355 inline void makeMap(MapType &map, MatrixType &m) {
363 Vector &control,
const double &threshold) {
371 tmpTask.head(rankJ).noalias() = svd.matrixU().leftCols(rankJ).adjoint() * err;
372 tmpTask.head(rankJ).array() *=
373 svd.singularValues().head(rankJ).array().inverse();
377 tmpVar.head(kernel.cols()).noalias() =
378 svd.matrixV().leftCols(rankJ) * tmpTask.head(rankJ);
379 tmpControl.noalias() = kernel * tmpVar.head(kernel.cols());
381 tmpControl.noalias() = svd.matrixV().leftCols(rankJ) * tmpTask.head(rankJ);
382 if (tmpControl.squaredNorm() > threshold)
return false;
383 control += tmpControl;
388 const int &iterTime) {
396 return posture != NULL && posture->
dimensionSOUT(iterTime) == nDof - 6;
418 #include <sys/time.h> 425 #define TIME_STREAM DYNAMIC_GRAPH_ENTITY_DEBUG(*this) 426 #define sotINIT_CHRONO1 \ 427 struct timespec t0, t1; \ 429 #define sotSTART_CHRONO1 clock_gettime(CLOCK_THREAD_CPUTIME_ID, &t0) 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; \ 444 #define sotPRINTCOUNTER(nbc1) \ 445 TIME_STREAM << "dt" << iterTask << '_' << nbc1 << ' ' << (long int)dt##nbc1 \ 447 #else // #ifdef WITH_CHRONO 448 #define sotINIT_CHRONO1 449 #define sotSTART_CHRONO1 451 #define sotINITPARTCOUNTERS 452 #define sotSTARTPARTCOUNTERS 453 #define sotCOUNTER(nbc1, nbc2) 454 #define sotINITCOUNTER(nbc1) 455 #define sotPRINTCOUNTER(nbc1) 456 #endif // #ifdef WITH_CHRONO 460 res.resize(taskVector.size());
463 for (VectorMultiBound::const_iterator iter = taskVector.begin();
464 iter != taskVector.end(); ++iter, ++i) {
465 res(i) = iter->getSingleBound();
470 const int &iterTime) {
486 bool controlIsZero =
true;
488 control =
q0SIN(iterTime);
489 controlIsZero =
false;
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());
497 if (
stack.size() == 0) {
498 std::ostringstream oss;
500 <<
") contains no task and q0SIN is not plugged.";
501 throw std::logic_error(oss.str());
504 sotDEBUG(25) <<
"No initial velocity." << endl;
507 sotDEBUGF(5,
" --- Time %d -------------------", iterTime);
508 unsigned int iterTask = 0;
510 bool has_kernel =
false;
519 <<
"Projector of " <<
getName() <<
" has " << K.rows()
520 <<
" rows while " <<
nbJoints <<
" expected.\n";
523 for (StackType::iterator iter =
stack.begin(); iter !=
stack.end(); ++iter) {
528 Task *task =
dynamic_cast<Task *
>(*iter);
530 bool last = (iterTask + 1 ==
stack.size());
549 if (fullPostureTask) {
554 rankJ = kernel.cols();
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;
574 mem->
Jt.noalias() = JK * kernel;
582 svd.compute(*Jt, Eigen::ComputeThinU | Eigen::ComputeThinV);
584 svd.compute(*Jt, Eigen::ComputeThinU | Eigen::ComputeFullV);
586 while (rankJ < svd.singularValues().size() &&
587 th < svd.singularValues()[rankJ])
592 if (!controlIsZero) mem->
err.noalias() -= JK * control;
597 controlIsZero =
false;
603 kernel * svd.matrixV().rightCols(cols);
606 svd.matrixV().rightCols(cols);
612 << iterTime <<
": SOT " <<
getName() <<
" disabled task " 613 << taskA.
getName() <<
" at level " << iterTask
614 <<
" because norm exceeded the limit.\n";
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 = " 625 sotDEBUG(2) <<
"Proj non optimal (rankJ= " << rankJ
626 <<
", iterTask =" << iterTask <<
")";
636 if (last || kernel.cols() == 0)
break;
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;
656 os <<
"+-----------------" << std::endl;
669 StackType::const_iterator iter;
670 for (iter =
stack.begin(); iter !=
stack.end(); ++iter) {
672 StackType::const_iterator nextiter = iter;
675 if (nextiter !=
stack.end()) {
677 os <<
"\t\t\t\"" << task.
getName() <<
"\" -> \"" << nexttask.
getName()
678 <<
"\" [color=red]" << endl;
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) {
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 ]" 694 os <<
"\t\t\t}" << std::endl;
695 os <<
"\t\t\t}" << endl;
std::string docDirectGetter(const std::string &name, const std::string &type)
virtual dynamicgraph::Vector & computeControlLaw(dynamicgraph::Vector &control, const int &time)
Compute the control law.
#define sotCOUNTER(nbc1, nbc2)
DirectGetter< E, T > * makeDirectGetter(E &entity, T *ptr, const std::string &docString)
SignalTimeDependent< dynamicgraph::Vector, int > controlSOUT
Allow to get the result of the computed control law.
#define DYNAMIC_GRAPH_ENTITY_DEBUG(entity)
virtual std::ostream & writeGraph(std::ostream &os) const
This method write the priority between tasks in the output stream os.
virtual void down(const TaskAbstract &task)
This method makes the task to swap with the task having the immediate inferior priority.
SignalTimeDependent< unsigned int, int > dimensionSOUT
Returns the dimension of the feature as an output signal.
Eigen::Map< const Matrix, Eigen::internal::traits< Matrix >::Alignment > KernelConst_t
void signalRegistration(const SignalArray< int > &signals)
dynamicgraph::SignalTimeDependent< VectorMultiBound, int > taskSOUT
#define sotDEBUGOUT(level)
AD< Scalar > max(const AD< Scalar > &x, const AD< Scalar > &y)
std::vector< MultiBound > VectorMultiBound
virtual const unsigned int & getNbDof() const
dynamicgraph::SignalPtr< Flags, int > controlSelectionSIN
static void taskVectorToMlVector(const VectorMultiBound &taskVector, Vector &err)
Number of joints by default.
SignalPtr< double, int > inversionThresholdSIN
This signal allow to change the threshold for the damped pseudo-inverse on-line.
#define DYNAMIC_GRAPH_ENTITY_ERROR(entity)
static void openFile(const char *filename=DEBUG_FILENAME_DEFAULT)
#define sotDEBUGIN(level)
#define sotINITPARTCOUNTERS
bool updateControl(MemoryTaskSOT *mem, const Matrix::Index rankJ, bool has_kernel, const KernelConst_t &kernel, Vector &control, const double &threshold)
virtual void push(TaskAbstract &task)
Push the task in the stack. It has a lowest priority than the previous ones. If this is the first tas...
#define sotSTARTPARTCOUNTERS
double maxControlIncrementSquaredNorm
Maximum allowed squared norm of control increment. A task whose control increment is above this value...
SignalPtr< dynamicgraph::Vector, int > q0SIN
Intrinsec velocity of the robot, that is used to initialized the recurence of the SOT (e...
SignalPtr< dynamicgraph::Matrix, int > proj0SIN
A matrix K whose columns are a base of the desired velocity. In other words, where is the free para...
#define DYNAMIC_GRAPH_ENTITY_ERROR_STREAM(entity)
Eigen::Map< Matrix, Eigen::internal::traits< Matrix >::Alignment > Kernel_t
virtual TaskAbstract & pop(void)
Pop the task from the stack. This method removes the task with the smallest priority in the task...
bool enablePostureTaskAcceleration
Option to disable the computation of the SVD for the last task if this task is a Task with a single F...
static const double INVERSION_THRESHOLD_DEFAULT
Threshold to compute the dumped pseudo inverse.
virtual void remove(const TaskAbstract &task)
Remove a task regardless to its position in the stack. It removes also the signals connected to the o...
virtual const T & access(const Time &t)
virtual void removeDependency(const SignalBase< Time > &signal)
const Matrix & computeJacobianActivated(TaskAbstract *Ta, Task *T, Matrix &Jmem, const int &iterTime)
virtual void addDependency(const SignalBase< Time > &signal)
#define sotPRINTCOUNTER(nbc1)
MemoryTaskSOT::Kernel_t Kernel_t
virtual void display(std::ostream &os) const
dynamicgraph::SignalTimeDependent< dynamicgraph::Matrix, int > jacobianSOUT
Eigen::JacobiSVD< Matrix > SVD_t
SOTSOT_CORE_EXPORT friend std::ostream & operator<<(std::ostream &os, const Sot &sot)
virtual void recompute(const Time &t)
virtual void defineNbDof(const unsigned int &nbDof)
This method defines the part of the state vector which correspond to the free flyer of the robot...
virtual bool isPlugged() const
StackType stack
This field is a list of controllers managed by the stack of tasks.
unsigned int nbJoints
Store the number of joints to be used in the command computed by the stack of tasks.
#define sotINITCOUNTER(nbc1)
MemoryTaskSOT * getMemory(TaskAbstract &t, const Matrix::Index &tDim, const Matrix::Index &nDof)
const std::string & getName() const
virtual bool exist(const TaskAbstract &task)
This method allows to know if a task exists or not.
MemoryTaskSOT::KernelConst_t KernelConst_t
Kernel_t & getKernel(const Matrix::Index r, const Matrix::Index c)
This class implements the Stack of Task. It allows to deal with the priority of the controllers throu...
virtual void removeDependency(const TaskAbstract &key)
This method removes the output signals depending on this task.
void sotDEBUGF(const int, const char *,...)
Class that defines the basic elements of a task.
virtual const T & accessCopy() const
dynamicgraph::Vector tmpControl
dynamicgraph::Vector tmpVar
bool isFullPostureTask(Task *task, const Matrix::Index &nDof, const int &iterTime)
FeatureList_t & getFeatureList(void)
virtual void up(const TaskAbstract &task)
This method makes the task to swap with the task having the immediate superior priority.
void setReady(const bool sready=true)
virtual void clear(void)
Remove all the tasks from the stack.
void addCommand(const std::string &name, command::Command *command)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sot, "SOT")
void makeMap(MapType &map, MatrixType &m)
std::string docDirectSetter(const std::string &name, const std::string &type)
dynamicgraph::Vector tmpTask
const Eigen::IOFormat python(Eigen::FullPrecision, 0, ", ", ",\, "[", "]", "[", "]")
DirectSetter< E, T > * makeDirectSetter(E &entity, T *ptr, const std::string &docString)
MemoryTaskAbstract * memoryInternal
virtual const std::string & getClassName() const
Returns the name of this class.