IVIGIT.

include/sot-dyninv/stack-template.h

+/*
+ * 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__

include/sot-dyninv/stack-template.t.cpp

+/*
+ * 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__