diff --git a/include/sot-dyninv/stack-template.h b/include/sot-dyninv/stack-template.h
new file mode 100644
index 0000000000000000000000000000000000000000..4edb9254ef9adba25b54102e8bec08c1f1af51c4
--- /dev/null
+++ b/include/sot-dyninv/stack-template.h
@@ -0,0 +1,192 @@
+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-dyninv is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_dyninv_StackTemplate_H__
+#define __sot_dyninv_StackTemplate_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (dynamic_interpretor_EXPORTS)
+# define SOTSTACKTEMPLATE_EXPORT __declspec(dllexport)
+# else
+# define SOTSTACKTEMPLATE_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTSTACKTEMPLATE_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <sot-dyninv/signal-helper.h>
+
+namespace sot {
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ namespace dg = dynamicgraph;
+
+ template< typename TaskGeneric >
+ class SOTSTACKTEMPLATE_EXPORT Stack
+ {
+
+ protected:
+
+ typedef std::vector<TaskGeneric*> Stack_t;
+ typedef typename Stack_t::iterator StackIterator_t;
+ typedef typename Stack_t::const_iterator StackConstIterator_t;
+
+ /*! \brief List of task (controllers) managed by the stack of tasks. */
+ Stack_t stack;
+
+ /*! \brief Number of joints to be used to compute the control law. */
+ int nbDofs;
+
+ public:
+
+ /*! \brief Number of joints by default. */
+ static const unsigned int NB_JOINTS_DEFAULT; // = 48;
+
+ public: /* --- Constructors --- */
+
+ Stack( void );
+ ~Stack( void ) { /* TODO!! */ }
+
+ public: /* --- Modifiors --- */
+
+ /*! \name Methods to handle the stack.
+ @{
+ */
+
+ /*! \brief Push the task in the stack.
+ It has a lowest priority than the previous ones.
+ If this is the first task, then it has the highest
+ priority. */
+ virtual void push( TaskGeneric& task );
+ /*! \brief Pop the task from the stack.
+ This method removes the task with the smallest
+ priority in the task. The other are projected
+ in the null-space of their predecessors. */
+ virtual TaskGeneric& pop( void );
+
+ /*! \brief This method allows to know if a task exists or not */
+ virtual bool exist( const TaskGeneric& task );
+
+ /*! \brief Remove a task regardless to its position in the stack.
+ It removes also the signals connected to the output signal of this stack.*/
+ virtual void remove( const TaskGeneric& task );
+
+ /*! \brief This method makes the task to swap with the task having the
+ immediate superior priority. */
+ virtual void up( const TaskGeneric& task );
+
+ /*! \brief This method makes the task to swap with the task having the
+ immediate inferior priority. */
+ virtual void down( const TaskGeneric& task );
+
+ /*! \brief Remove all the tasks from the stack. */
+ virtual void clear( void );
+
+ /*! Change the number of DOF, ie the field nbDofs. */
+ virtual void defineNbDof( const int& nbDof );
+
+ /*! @} */
+
+ dg::SignalBase<int>* tatata;
+
+ int titit;
+ typedef std::list< const dg::SignalBase<int>* > TaskDependancyList_t;
+
+ int totol;
+
+ /*! \brief Return the signal to be added/removed from the dependancy
+ * list of the control signal. */
+ virtual TaskDependancyList_t getTaskDependancyList( const TaskGeneric& task ) = 0;
+ virtual void addDependancy( const TaskDependancyList_t& depList ) = 0;
+ virtual void removeDependancy( const TaskDependancyList_t& depList ) = 0;
+ virtual void resetReady( void ) = 0;
+
+ public: /* --- DISPLAY --- */
+
+ /*! \name Methods to display the stack of tasks.
+ @{
+ */
+
+ /*! Display the stack of tasks in text mode as a tree. */
+ virtual void display( std::ostream& os ) const;
+ /*! \brief Write the priority between tasks in the outstream os. */
+ virtual std::ostream &
+ writeGraph(const std::string & name, std::ostream & os) const;
+
+ /*! @} */
+
+ public: /* --- COMMANDS --- */
+
+ /*! @{ */
+
+ /* To use the macro, the typedef sot::Stack<TaskSpec> stack_t has to be set,
+ * as well as the EntityClassName. */
+ //#define ADD_COMMANDS_FOR_THE_STACK /// Code in the .t.cpp
+
+
+ /*! \brief This method deals with the command line.
+ The command given in argument is send to the stack of tasks by the shell.
+ The command understood by sot are:
+ <ul>
+ <li> Tasks
+ <ul>
+ <li> push <task> : Push a task in the stack (FILO).
+ <li> pop : Remove the task push in the stack.
+ <li> down <task> : Make the task have a higher priority, i.e.
+ swap with the task immediatly superior in priority.
+ <li> up <task> : Make the task have a lowest priority, i.e.
+ swap with the task immediatly inferior in priority.
+ <li> rm <task> : Remove the task from the stack.
+ </ul>
+ */
+ virtual bool stackCommandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+ void pushByTaskName( const std::string& taskName );
+ void removeByTaskName( const std::string& taskName );
+ void upByTaskName( const std::string& taskName );
+ void downByTaskName( const std::string& taskName );
+
+ /*! @} */
+
+ }; // class StackTemplate
+
+
+ template< typename TaskGeneric >
+ std::ostream&
+ operator<< ( std::ostream& os,const Stack<TaskGeneric>& sot )
+ {
+ sot.display(os);
+ return os;
+ }
+
+
+} // namespace sot
+
+#include <sot-dyninv/stack-template.t.cpp>
+
+#endif // #ifndef __sot_dyninv_StackTemplate_H__
diff --git a/include/sot-dyninv/stack-template.t.cpp b/include/sot-dyninv/stack-template.t.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6aca8c6d6d9cc207387c172b9bac47d21571b181
--- /dev/null
+++ b/include/sot-dyninv/stack-template.t.cpp
@@ -0,0 +1,368 @@
+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-dyninv is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_dyninv_StackTemplate_TCC__
+#define __sot_dyninv_StackTemplate_TCC__
+
+#include <dynamic-graph/pool.h>
+#include <sot-core/debug.h>
+#include <sot-core/task-abstract.h>
+
+namespace sot
+{
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ template< typename TaskGeneric >
+ const unsigned int Stack<TaskGeneric>::NB_JOINTS_DEFAULT = 46;
+
+ /* --------------------------------------------------------------------- */
+ /* --- CONSTRUCTION ---------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ template< typename TaskGeneric >
+ Stack<TaskGeneric>::
+ Stack( void )
+ :stack()
+ ,nbDofs( NB_JOINTS_DEFAULT )
+ {
+ }
+
+ /* --------------------------------------------------------------------- */
+ /* --- STACK MANIPULATION --- */
+ /* --------------------------------------------------------------------- */
+
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ push( TaskGeneric& task )
+ {
+ stack.push_back( &task );
+ addDependancy( getTaskDependancyList( task ) );
+ resetReady();
+ }
+ template< typename TaskGeneric >
+ TaskGeneric& Stack<TaskGeneric>::
+ pop( void )
+ {
+ TaskGeneric* res = stack.back();
+ stack.pop_back();
+ removeDependancy( getTaskDependancyList( *res ) );
+ resetReady();
+ return *res;
+ }
+ template< typename TaskGeneric >
+ bool Stack<TaskGeneric>::
+ exist( const TaskGeneric& key )
+ {
+ StackIterator_t it;
+ for ( it=stack.begin();stack.end()!=it;++it )
+ {
+ if( *it == &key ) { return true; }
+ }
+ return false;
+ }
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ remove( const TaskGeneric& key )
+ {
+ bool find =false;
+ StackIterator_t it;
+ for ( it=stack.begin();stack.end()!=it;++it )
+ {
+ if( *it == &key ) { find=true; break; }
+ }
+ if(! find ){ return; }
+
+ stack.erase( it );
+ removeDependancy( getTaskDependancyList( key ) );
+ resetReady();
+ }
+
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ up( const TaskGeneric& key )
+ {
+ bool find =false;
+ StackIterator_t it;
+ for ( it=stack.begin();stack.end()!=it;++it )
+ {
+ if( *it == &key ) { find=true; break; }
+ }
+ if( stack.begin()==it ) { return; }
+ if(! find ){ return; }
+
+ StackIterator_t pos=it; pos--;
+ TaskGeneric * task = *it;
+ stack.erase( it );
+ stack.insert( pos,task );
+ resetReady();
+ }
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ down( const TaskGeneric& key )
+ {
+ bool find =false;
+ StackIterator_t it;
+ for ( it=stack.begin();stack.end()!=it;++it )
+ {
+ if( *it == &key ) { find=true; break; }
+ }
+ if( stack.end()==it ) { return; }
+ if(! find ){ return; }
+
+ StackIterator_t pos=it; pos++;
+ TaskGeneric* task=*it;
+ stack.erase( it );
+ if( stack.end()==pos ){ stack.push_back(task); }
+ else
+ {
+ pos++;
+ stack.insert( pos,task );
+ }
+ resetReady();
+ }
+
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ clear( void )
+ {
+ for ( StackIterator_t it=stack.begin();stack.end()!=it;++it )
+ {
+ removeDependancy( getTaskDependancyList( **it ) );
+ }
+ stack.clear();
+ resetReady();
+ }
+
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ defineNbDof( const int& nbDof )
+ {
+ assert(nbDof >= 0);
+ nbDofs = nbDof;
+ resetReady();
+ }
+
+ /* --------------------------------------------------------------------- */
+ /* --- DISPLAY --------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ display( std::ostream& os ) const
+ {
+
+ os << "+-----------------"<<std::endl<<"+ SOT "
+ << std::endl<< "+-----------------"<<std::endl;
+ for ( StackConstIterator_t it=this->stack.begin();
+ this->stack.end()!=it;++it )
+ //BOOST_FOREACH( TaskGeneric* task, stack )
+ {
+ os << "| " << (*it)->getName() <<std::endl;
+ }
+ }
+
+
+ /* --------------------------------------------------------------------- */
+ /* --- COMMAND --------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+#define ACT_BY_TASK_NAME(ACT) \
+ template< typename TaskGeneric > \
+ void Stack<TaskGeneric>:: \
+ ACT##ByTaskName( const std::string& tname ) \
+ { \
+ dg::Entity & taska = dg::g_pool.getEntity( tname ); \
+ TaskGeneric & task = dynamic_cast<TaskGeneric&>( taska ); \
+ ACT(task); \
+ }
+
+ ACT_BY_TASK_NAME(push)
+ ACT_BY_TASK_NAME(remove)
+ ACT_BY_TASK_NAME(up)
+ ACT_BY_TASK_NAME(down)
+
+ template< typename TaskGeneric >
+ bool Stack<TaskGeneric>::
+ stackCommandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+ {
+ sotDEBUGIN(15);
+
+ if( cmdLine == "help")
+ {
+ os << "Stack of Tasks: "<<std::endl
+ << " - push <task>"<< std::endl
+ << " - pop"<< std::endl
+ << " - down <task>"<< std::endl
+ << " - up <task>"<< std::endl
+ << " - rm <task>"<< std::endl
+ << " - clear"<< std::endl
+ << " - display"<< std::endl
+
+ << " - nbDofs <nb> "<<std::endl;
+ }
+ else if( cmdLine == "clear")
+ {
+ clear();
+ }
+ else if( cmdLine == "push")
+ {
+ std::string tname; cmdArgs >> tname;
+ pushByTaskName(tname);
+ }
+ else if( cmdLine == "up")
+ {
+ std::string tname; cmdArgs >> tname;
+ upByTaskName(tname);
+ }
+ else if( cmdLine == "down")
+ {
+ std::string tname; cmdArgs >> tname;
+ downByTaskName(tname);
+ }
+ else if( cmdLine == "rm")
+ {
+ std::string tname; cmdArgs >> tname;
+ removeByTaskName(tname);
+ }
+ else if( cmdLine == "pop")
+ {
+ TaskGeneric& task = pop();
+ os << "Remove : "<< task << std::endl;
+ }
+
+ else if( cmdLine == "nbDofs")
+ {
+ cmdArgs>>std::ws;
+ if( cmdArgs.good() )
+ {
+ unsigned int nbDof;
+ cmdArgs >> nbDof;
+ defineNbDof(nbDof);
+ } else { os << "nbDofs = "<< nbDofs <<std::endl; }
+ }
+ else if( cmdLine == "display")
+ {
+ display(os);
+ }
+ else return false;
+
+ return true;
+
+ sotDEBUGOUT(15);
+ }
+
+ template< typename TaskGeneric >
+ std::ostream& Stack<TaskGeneric>::
+ writeGraph( const std::string & name, std::ostream& os ) const
+ {
+ StackConstIterator_t iter;
+ for( iter = stack.begin(); iter!=stack.end();++iter )
+ {
+ const TaskGeneric & task = **iter;
+ StackConstIterator_t nextiter =iter;
+ nextiter++;
+
+ if (nextiter!=stack.end())
+ {
+ TaskGeneric & nexttask = **nextiter;
+ os << "\t\t\t" << task.getName() << " -> " << nexttask.getName()
+ << " [color=red]" << std::endl;
+ }
+ }
+
+ os << "\t\tsubgraph cluster_Tasks {" <<std::endl
+ << "\t\t\tsubgraph cluster_" << name << " {" << std::endl
+ << "\t\t\t\tcolor=lightsteelblue1; label=\"" << name
+ << "\"; style=filled;" << std::endl;
+
+ for( iter = stack.begin(); iter!=stack.end();++iter )
+ {
+ const TaskGeneric & task = **iter;
+ os << "\t\t\t\t" << task.getName()
+ << " [ label = \"" << task.getName() << "\" ," << std::endl
+ << "\t\t\t\t fontcolor = black, color = black, "
+ << "fillcolor = magenta, style=filled, shape=box ]" << std::endl;
+
+ }
+ os << "\t\t\t}" << std::endl;
+ os << "\t\t\t}" <<std::endl;
+ return os;
+ }
+
+
+
+
+ /* To use the macro, the typedef sot::Stack<TaskSpec> stack_t has to be set,
+ * as well as the EntityClassName. */
+#define ADD_COMMANDS_FOR_THE_STACK \
+ addCommand("dispStack", \
+ makeCommandVerbose(*this,&stack_t::display, \
+ docCommandVerbose("Guess what?"))); \
+ addCommand("up", \
+ makeCommandVoid1(*this, \
+ (void (EntityClassName::*)(const std::string&)) \
+ &stack_t::upByTaskName, \
+ docCommandVoid1("Up the named task.", \
+ "string (task name)"))); \
+ addCommand("down", \
+ makeCommandVoid1(*this, \
+ (void (EntityClassName::*)(const std::string&)) \
+ &stack_t::downByTaskName, \
+ docCommandVoid1("Down the named task.", \
+ "string (task name)"))); \
+ addCommand("push", \
+ makeCommandVoid1(*this, \
+ (void (EntityClassName::*)(const std::string&)) \
+ &stack_t::pushByTaskName, \
+ docCommandVoid1("Push the named task.", \
+ "string (task name)"))); \
+ addCommand("rm", \
+ makeCommandVoid1(*this, \
+ (void (EntityClassName::*)(const std::string&)) \
+ &stack_t::removeByTaskName, \
+ docCommandVoid1("Remove the named task.", \
+ "string (task name)"))); \
+ addCommand("clear", \
+ makeCommandVoid0(*this, \
+ (void (EntityClassName::*)(void)) \
+ &stack_t::clear, \
+ docCommandVoid0("Clear the stack from all task."))); \
+ addCommand("pop", \
+ makeCommandVoid0(*this, \
+ (void (EntityClassName::*)(void)) \
+ &stack_t::pop, \
+ docCommandVoid0("Remove the last (lowest) task of the stack."))); \
+ \
+ addCommand("setSize", \
+ makeCommandVoid1(*this, \
+ (void (EntityClassName::*)(const int&)) \
+ &stack_t::defineNbDof, \
+ docCommandVoid1("??? TODO", \
+ "int"))); \
+ addCommand("getSize", \
+ makeDirectGetter(*this,&nbDofs, \
+ docDirectGetter("size","int")))
+
+
+
+} // namespace sot
+
+#endif // #ifndef __sot_dyninv_StackTemplate_TCC__