include/CMakeLists.txt

-# 
-#  Copyright 
-# 
-
-# Generate header with default script directory.
-SET(SOT_IMPORT_DEFAULT_PATHS \"${CMAKE_INSTALL_PREFIX}/script\")
-CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/sot-core/import-default-paths.h.cmake
-		       ${CMAKE_CURRENT_SOURCE_DIR}/sot-core/import-default-paths.h)
-
-SET(${PROJECT_NAME}_HEADERS
-sot-core-api.h
-
-#debug
-contiifstream.h
-debug.h
-
-#exception
-exception-abstract.h
-exception-dynamic.h
-exception-factory.h
-exception-feature.h
-exception-signal.h
-exception-task.h
-exception-tools.h
-
-#signal
-signal-cast.h
-
-#matrix
-binary-op.h
-derivator.h
-fir-filter.h
-integrator-abstract.h
-integrator-euler.h
-matrix-constant.h
-unary-op.h
-vector-constant.h
-vector-to-rotation.h
-
-#factory
-additional-functions.h
-factory.h
-macros-signal.h
-pool.h
-import-default-paths.h
-
-#math
-matrix-force.h
-matrix-homogeneous.h
-matrix-rotation.h
-matrix-twist.h
-op-point-modifier.h
-vector-quaternion.h
-vector-roll-pitch-yaw.h
-vector-rotation.h
-vector-utheta.h
-
-#feature
-feature-point6d.h
-feature-vector3.h
-feature-abstract.h
-feature-generic.h
-feature-joint-limits.h
-feature-1d.h
-feature-point6d-relative.h
-feature-visual-point.h
-feature-task.h
-feature-line-distance.h
-
-#task
-task-abstract.h
-task-unilateral.h
-task-pd.h
-task-conti.h
-multi-bound.h
-constraint.h
-gain-adaptive.h
-task.h
-gain-hyperbolic.h
-
-#sot
-flags.h
-sot-qr.h
-memory-task-sot.h
-sot-h.h
-sot.h
-rotation-simple.h
-weighted-sot.h
-solver-hierarchical-inequalities.h
-
-#traces
-reader.h
-
-#tools
-periodic-call.h
-utils-windows.h
-time-stamp.h
-timer.h
-periodic-call-entity.h
-neck-limitation.h
-motion-period.h
-binary-int-to-uint.h
-clamp-workspace.h
-com-freezer.h
-gripper-control.h
-joint-limitator.h
-kalman.h
-
-#these need the boost thread library - reminder
-mailbox.h
-mailbox.t.cpp
-distant-shell.h
-)
-
-# Recreate correct path for the headers
-#--------------------------------------
-
-SET(fullpath_${PROJECT_NAME}_HEADERS)
-FOREACH(lHeader ${${PROJECT_NAME}_HEADERS})
-  SET(fullpath_${PROJECT_NAME}_HEADERS 
-    ${fullpath_${PROJECT_NAME}_HEADERS}
-    ./${PROJECT_NAME}/${lHeader}
-  )
-
-ENDFOREACH(lHeader)
-
-#----------------------------------------------------
-# Install procedure for the header files
-#----------------------------------------------------
-INSTALL(FILES ${fullpath_${PROJECT_NAME}_HEADERS}
-	DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}
-        PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
-)
+# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST

include/sot-core/binary-op.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      binary-op.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-#ifndef __SOT_BINARYOP_H__
-#define __SOT_BINARYOP_H__
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-/* SOT */
-#include <sot-core/flags.h>
-#include <dynamic-graph/entity.h>
-#include <sot-core/pool.h>
-#include <dynamic-graph/all-signals.h>
-#include <sot-core/vector-quaternion.h>
-
-/* STD */
-#include <string>
-
-#include <boost/function.hpp>
-
-namespace sot {
-namespace dg = dynamicgraph;
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-template< class Tin1,class Tin2,class Tout,typename Operator >
-class BinaryOp
-:public dg::Entity
-{
-  Operator op;
-
- public: /* --- CONSTRUCTION --- */
-
-  static std::string getTypeIn1Name( void ) { return "UnknownIn1"; }
-  static std::string getTypeIn2Name( void ) { return "UnknownIn2"; }
-  static std::string getTypeOutName( void ) { return "UnknownOut"; }
-  static const std::string CLASS_NAME;
-
-  BinaryOp( const std::string& name )
-    : dg::Entity(name)
-    ,SIN1(NULL,BinaryOp::CLASS_NAME+"("+name+")::input("+getTypeIn1Name()+")::in1") 
-    ,SIN2(NULL,CLASS_NAME+"("+name+")::input("+getTypeIn2Name()+")::in2") 
-    ,SOUT( boost::bind(&BinaryOp<Tin1,Tin2,Tout,Operator>::computeOperation,this,_1,_2), 
-	   SIN1<<SIN2,CLASS_NAME+"("+name+")::output("+getTypeOutName()+")::out") 
-    {
-      signalRegistration( SIN1<<SIN2<<SOUT );
-    }
-
-
-  virtual ~BinaryOp( void ) {};
-
- public: /* --- SIGNAL --- */
-
-  dg::SignalPtr<Tin1,int> SIN1;
-  dg::SignalPtr<Tin2,int> SIN2;
-  dg::SignalTimeDependent<Tout,int> SOUT;
-
- protected:
-  Tout& computeOperation( Tout& res,int time )
-    {
-      const Tin1 &x1 = SIN1(time);
-      const Tin2 &x2 = SIN2(time);
-      op(x1,x2,res);
-      return res;
-    }
-
- public: /* --- PARAMS --- */
-   virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs, 
-			     std::ostream& os );
-    
-
-};
-
-
-} // namespace sot
-
-
-#endif // #ifndef __SOT_BINARYOP_H__

include/sot-core/constraint.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      constraint.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-#ifndef __SOT_CONSTRAINT_H__
-#define __SOT_CONSTRAINT_H__
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-/* STD */
-#include <string>
-
-/* SOT */
-#include <sot-core/feature-abstract.h>
-#include <sot-core/flags.h>
-#include <sot-core/task-abstract.h>
-#include <dynamic-graph/all-signals.h>
-#include <sot-core/exception-task.h>
-#include <sot-core/exception-signal.h>
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32) 
-#  if defined (constraint_EXPORTS)
-#    define SOTCONSTRAINT_EXPORT __declspec(dllexport)
-#  else  
-#    define SOTCONSTRAINT_EXPORT __declspec(dllimport)
-#  endif 
-#else
-#  define SOTCONSTRAINT_EXPORT
-#endif
-
-
-namespace sot {
-namespace dg = dynamicgraph;
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-
-
-class SOTCONSTRAINT_EXPORT Constraint
-: public TaskAbstract
-{
- protected:
-  typedef std::list< dg::Signal<ml::Matrix,int>* > JacobianList;
-  JacobianList jacobianList;
-  
- public: 
-  static const std::string CLASS_NAME;
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- public:
-  Constraint( const std::string& n );
-
-  void addJacobian( dg::Signal<ml::Matrix,int>& sig );
-  void clearJacobianList( void );
-
-  void setControlSelection( const Flags& act );
-  void addControlSelection( const Flags& act );
-  void clearControlSelection( void );
-
-  /* --- COMPUTATION --- */
-  ml::Matrix& computeJacobian( ml::Matrix& J,int time );
-
-  /* --- DISPLAY ------------------------------------------------------------ */
-  SOTCONSTRAINT_EXPORT friend std::ostream& operator<< ( std::ostream& os,const Constraint& t );
-
-  /* --- PARAMS --- */
-  virtual void commandLine( const std::string& cmdLine
-			    ,std::istringstream& cmdArgs
-			    ,std::ostream& os );
-};
-
-} // namespace sot
-
-
-
-#endif /* #ifndef __SOT_CONSTRAINT_H__ */
-
-

include/sot-core/debug.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet Lagadic, 2005
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      debug.h
- * Project:   STack of Tasks
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- * Macro de trace et de debugage
- *
- *   - TRACAGE:    TRACE et ERROR_TRACE fonctionnent comme des printf
- * avec retour chariot en fin de fonction.
- *                 CERROR et CTRACE fonctionnent comme les flux de sortie
- * C++ cout et cerr.
- *   - DEBUGAGE:   DEBUG_TRACE(niv,  et DERROR_TRACE(niv, fonctionnent
- * comme des printf, n'imprimant que si le niveau de trace 'niv' est
- * superieur au mode de debugage VP_DEBUG_MODE.
- *                 CDEBUG(niv) fonctionne comme le flux de sortie C++ cout.
- *                 DEBUG_ENABLE(niv) vaut 1 ssi le niveau de tracage 'niv'
- * est superieur au  mode de debugage DEBUG_MODE. Il vaut 0 sinon.
- *   - PROG DEFENSIVE: DEFENSIF(a) vaut a ssi le mode defensif est active,
- * et vaut 0 sinon.
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-
-#ifndef __VS_DEBUG_HH
-#define __VS_DEBUG_HH
-
-#include <stdio.h>
-#include <iostream>
-#include <fstream>
-#include <sstream>
-#include <stdarg.h>
-#include <sot-core/sot-core-api.h>
-
-namespace sot {
-
- 
-/* -------------------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-/* -------------------------------------------------------------------------- */
-
-#ifndef VP_DEBUG_MODE
-#define VP_DEBUG_MODE 0
-#endif 
-#ifndef VP_TEMPLATE_DEBUG_MODE
-#define VP_TEMPLATE_DEBUG_MODE 0
-#endif
-
-#define SOT_COMMON_TRACES do {  \
-		    va_list arg; \
-		    va_start(arg,format); \
-		    vsnprintf( charbuffer,SIZE,format,arg ); \
-		    va_end(arg); \
-		    outputbuffer << tmpbuffer.str() << charbuffer <<std::endl; \
-		} while(0)
-
-class SOT_CORE_EXPORT DebugTrace
-{
- public:
-    static const int SIZE = 512;
-
-    std::stringstream tmpbuffer;
-    std::ostream& outputbuffer;
-    char charbuffer[SIZE+1];
-    int traceLevel;
-    int traceLevelTemplate; 
-
-    DebugTrace( std::ostream& os ): outputbuffer(os) {}
-
-    inline void trace( const int level,const char* format,...)
-	{ if( level<=traceLevel ) SOT_COMMON_TRACES; tmpbuffer.str(""); }
-    inline void trace( const char* format,...){ SOT_COMMON_TRACES;  tmpbuffer.str(""); }
-    inline void trace( const int level=-1 ) 
-	{ if( level<=traceLevel ) outputbuffer << tmpbuffer.str(); tmpbuffer.str("");  }
-
-    inline void traceTemplate( const int level,const char* format,...)
-	{ if( level<=traceLevelTemplate ) SOT_COMMON_TRACES; tmpbuffer.str(""); }
-    inline void traceTemplate( const char* format,...)
-	{ SOT_COMMON_TRACES; tmpbuffer.str("");  }
-
-    inline DebugTrace& pre( const std::ostream& dummy ) { return *this; }
-    inline DebugTrace& pre( const std::ostream& dummy,int level ) 
-	{ traceLevel = level; return *this; }
-/*     inline DebugTrace& preTemplate( const std::ostream& dummy,int level )  */
-/* 	{ traceLevelTemplate = level; return *this; } */
-
-
-    static const char * DEBUG_FILENAME_DEFAULT;
-    static void openFile( const char * filename = DEBUG_FILENAME_DEFAULT );
-    static void closeFile( const char * filename = DEBUG_FILENAME_DEFAULT );
-
-};
-
-SOT_CORE_EXPORT extern DebugTrace sotDEBUGFLOW;
-SOT_CORE_EXPORT extern DebugTrace sotERRORFLOW;
-
-#ifdef VP_DEBUG
-#define sotPREDEBUG  __FILE__ << ": " <<__FUNCTION__  \
-                              << "(#" << __LINE__ << ") :" 
-#define sotPREERROR  "\t!! "<<__FILE__ << ": " <<__FUNCTION__  \
-                            << "(#" << __LINE__ << ") :" 
-
-#  define sotDEBUG(level) if( (level>VP_DEBUG_MODE)||(!sot::sotDEBUGFLOW.outputbuffer.good()) ) ;\
-    else sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
-#  define sotDEBUGMUTE(level) if( (level>VP_DEBUG_MODE)||(!sot::sotDEBUGFLOW.outputbuffer.good()) ) ;\
-    else sot::sotDEBUGFLOW.outputbuffer
-#  define sotERROR  if(!sot::sotDEBUGFLOW.outputbuffer.good()); else sot::sotERRORFLOW.outputbuffer << sotPREERROR
-#  define sotDEBUGF if(!sot::sotDEBUGFLOW.outputbuffer.good()); else sot::sotDEBUGFLOW.pre(sot::sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_DEBUG_MODE).trace
-#  define sotERRORF if(!sot::sotDEBUGFLOW.outputbuffer.good()); else sot::sotERRORFLOW.pre(sot::sotERRORFLOW.tmpbuffer<<sotPREERROR).trace
-// TEMPLATE
-#  define sotTDEBUG(level) if( (level>VP_TEMPLATE_DEBUG_MODE)||(!sot::sotDEBUGFLOW.outputbuffer.good()) ) ;\
-    else sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
-#  define sotTDEBUGF  if(!sot::sotDEBUGFLOW.outputbuffer.good()); else sot::sotDEBUGFLOW.pre(sot::sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_TEMPLATE_DEBUG_MODE).trace
-inline bool sotDEBUG_ENABLE( const int & level ) { return level<=VP_DEBUG_MODE; }
-inline bool sotTDEBUG_ENABLE( const int & level ) { return level<=VP_TEMPLATE_DEBUG_MODE; }
-
-/* -------------------------------------------------------------------------- */
-#else // #ifdef VP_DEBUG
-#define sotPREERROR  "\t!! "<<__FILE__ << ": " <<__FUNCTION__  \
-                            << "(#" << __LINE__ << ") :" 
-#  define sotDEBUG(level) if( 1 ) ; else std::cout 
-#  define sotDEBUGMUTE(level) if( 1 ) ; else std::cout 
-#  define sotERROR sotERRORFLOW.outputbuffer << sotPREERROR
-inline void sotDEBUGF( const int level,const char* format,...) { return; }
-inline void sotDEBUGF( const char* format,...) { return; }
-inline void sotERRORF( const int level,const char* format,...) { return; }
-inline void sotERRORF( const char* format,...) { return; }
-// TEMPLATE
-#  define sotTDEBUG(level) if( 1 ) ; else std::cout 
-inline void sotTDEBUGF( const int level,const char* format,...) { return; }
-inline void sotTDEBUGF( const char* format,...) { return; }
-#define sotDEBUG_ENABLE(level) false
-#define sotTDEBUG_ENABLE(level) false
-
-#endif // #ifdef VP_DEBUG
-/* -------------------------------------------------------------------------- */
-
-#define sotDEBUGIN(level) sotDEBUG(level) << "# In {" << std::endl
-#define sotDEBUGOUT(level) sotDEBUG(level) << "# Out }" << std::endl
-#define sotDEBUGINOUT(level) sotDEBUG(level) << "# In/Out { }" << std::endl
-
-#define sotTDEBUGIN(level) sotTDEBUG(level) << "# In {" << std::endl
-#define sotTDEBUGOUT(level) sotTDEBUG(level) << "# Out }" << std::endl
-#define sotTDEBUGINOUT(level) sotTDEBUG(level) << "# In/Out { }" << std::endl
-
-} // namespace sot
-
-#endif /* #ifdef __VS_DEBUG_HH */
-
-/*
- * Local variables:
- * c-basic-offset: 4
- * End:
- */

include/sot-core/derivator.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      derivator.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-#ifndef __SOT_DERIVATOR_H__
-#define __SOT_DERIVATOR_H__
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-/* SOT */
-#include <sot-core/flags.h>
-#include <sot-core/pool.h>
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/all-signals.h>
-#include <sot-core/vector-quaternion.h>
-
-/* STD */
-#include <string>
-
-namespace sot {
-namespace dg = dynamicgraph;
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-template< class T >
-class Derivator
-:public dg::Entity
-{
- protected:
-  T memory;
-  bool initialized;
-  double timestep;
-  static const double TIMESTEP_DEFAULT ; //= 1.;
-
- public: /* --- CONSTRUCTION --- */
-
-  static std::string getTypeName( void ) { return "Unknown"; }
-  static const std::string CLASS_NAME;
-
-  Derivator( const std::string& name )
-    : dg::Entity(name)
-    ,memory(),initialized(false)
-    ,timestep(TIMESTEP_DEFAULT)
-    ,SIN(NULL,"sotDerivator<"+getTypeName()+">("+name+")::input("+getTypeName()+")::in") 
-    ,SOUT( boost::bind(&Derivator<T>::computeDerivation,this,_1,_2), 
-	   SIN,"sotDerivator<"+getTypeName()+">("+name+")::output("+getTypeName()+")::out")
-    ,timestepSIN("sotDerivator<"+getTypeName()+">("+name+")::input(double)::dt")
-    {
-      signalRegistration( SIN<<SOUT<<timestepSIN );
-      timestepSIN.setReferenceNonConstant( &timestep );
-      timestepSIN.setKeepReference(true);
-    }
-
-
-  virtual ~Derivator( void ) {};
-
- public: /* --- SIGNAL --- */
-
-  dg::SignalPtr<T,int> SIN;
-  dg::SignalTimeDependent<T,int> SOUT;
-  dg::Signal<double,int> timestepSIN;
-
- protected:
-  T& computeDerivation( T& res,int time )
-    {
-      if(initialized)
-	{
-	  res = memory; res *= -1;
-	  memory = SIN(time);
-	  res += memory;
-	  if( timestep!=1. ) res*=timestep;
-	} else {
-	  initialized = true; 
-	  memory = SIN(time); 
-	  res = memory; 
-	  res *= 0; 
-	}
-      return res;
-    }
-
- public: /* --- PARAMS --- */
-/*   virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs, */
-/* 			    std::ostream& os ) {} */
-    
-
-};
-
-} // using namespace sot
-
-
-
-#endif // #ifndef __SOT_DERIVATOR_H__

include/sot-core/distant-shell.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      DistantShell.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-#ifndef __SOT_DISTANTSHELL_HH__
-#define __SOT_DISTANTSHELL_HH__
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-
-#include <dynamic-graph/interpreter.h>
-#include <sot-core/contiifstream.h>
-#include <dynamic-graph/entity.h>
-
-//#include <pthread.h>
-
-#ifdef HAVE_LIBBOOST_THREAD
-#include <boost/thread.hpp>
-#endif
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32) 
-#  if defined (distant_shell_EXPORTS)
-#    define SOTDISTANTSHELL_EXPORT __declspec(dllexport)
-#  else  
-#    define SOTDISTANTSHELL_EXPORT __declspec(dllimport)
-#  endif 
-#else
-#  define SOTDISTANTSHELL_EXPORT
-#endif
-
-
-namespace sot {
-namespace dg = dynamicgraph;
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-class SOTDISTANTSHELL_EXPORT DistantShell
-{
- public:
-  std::ofstream osOwn;
- protected:
-  Contiifstream filein;
-  dynamicgraph::Interpreter& shell;
-  std::ostream & os;
-/*   pthread_t loopThread; */
-/*   pthread_attr_t loopThreadAttr; */
-
-#ifdef HAVE_LIBBOOST_THREAD
-  boost::thread* threadLoop;
-#else
-  int* threadLoop;
-#endif 
-
-  bool endLoop;
-  
-  unsigned int latency;
-  static const unsigned int LATENCY_DEFAULT = 10;
-
- public:
-  DistantShell(dynamicgraph::Interpreter& shell_,const std::string& fn  )
-    :osOwn(fn.c_str()),shell(shell_),os(osOwn)
-    ,endLoop(false),latency(LATENCY_DEFAULT){ }
-
-  DistantShell( const std::string& filename
-		   ,dynamicgraph::Interpreter& shell_
-		   ,std::ostream& os_ )
-    : filein( filename ),shell(shell_),os(os_)
-    ,endLoop(false),latency(LATENCY_DEFAULT)
-    {}
-
-  DistantShell( const std::string& filename
-		   ,dynamicgraph::Interpreter& shell_
-		   ,const std::string& fn )
-    : osOwn(fn.c_str()),filein( filename ),shell(shell_),os(osOwn)
-    ,endLoop(false),latency(LATENCY_DEFAULT)
-    {}
-
-  ~DistantShell( void );
-
-  void inputFile( const std::string& filename ) { filein = filename; }
-
-  void loop( const int latency = LATENCY_DEFAULT ); // in ms
-  void loopInThread( void );
-  void terminate( void ) { endLoop = true; }
-  void setLatency( const unsigned int& l ) { latency = l; }
-};
-
-
-class SOTDISTANTSHELL_EXPORT DistantShellPlugin
-: public dynamicgraph::Entity
-{
- protected:
-  DistantShell dshell;
-
- public:
-  static const std::string CLASS_NAME;
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- public:
-  DistantShellPlugin( const std::string& name );
-  virtual ~DistantShellPlugin( void );
-
- public:
-  virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
-			    std::ostream& os );
-
-
-};
-
-} // namespace sot
-
-#endif /* #ifndef __SOT_DISTANTSHELL_HH__ */
-
-
-
-

include/sot-core/factory.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      factory.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-#ifndef __SOT_FACTORY_HH__
-#define __SOT_FACTORY_HH__
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* --- STD --- */
-#include <map>
-#include <string>
-
-/* --- SOT --- */
-#include <sot-core/exception-factory.h>
-#include <sot-core/sot-core-api.h>
-#include <dynamic-graph/factory.h>
-
-namespace sot {
-
-class FeatureAbstract;
-class TaskAbstract;
-
-/* --------------------------------------------------------------------- */
-/* --- FACTORY ---------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/*! @ingroup factory
-  \brief This class implements the factory that allows creation of
-  objects loaded from dynamic link libraries. Objects are referenced by their
-  class names, and can be created by passing this string to one of the three
-  public "new" methods (newEntity, newFeature or newTask).
-  The factory instance (singleton) is publicly available under the name sotFactory
-  (include factory.h).  A task, feature or entity can register itself by
-   using the SOT_FACTORY_{ENTITY,TASK,FEATURE}_PLUGIN macro. See Task.cpp for
-   an example.
-*/
-class SOT_CORE_EXPORT FactoryStorage
-{
- public:
-  typedef FeatureAbstract* (*FeatureConstructor_ptr)( const std::string& );
-  typedef TaskAbstract* (*TaskConstructor_ptr)( const std::string& );
-  
- protected:
-  typedef std::map< std::string,TaskConstructor_ptr > TaskMap;
-  typedef std::map< std::string,FeatureConstructor_ptr > FeatureMap;
-
-  FeatureMap featureMap;
-  TaskMap taskMap;
-
- public:
-
-  ~FactoryStorage( void );
-
-  void registerTask( const std::string& entname,TaskConstructor_ptr ent );
-  TaskAbstract* newTask( const std::string& name,const std::string& objname );
-  bool existTask( const std::string& name, TaskMap::iterator& entPtr );
-  bool existTask( const std::string& name );
-
-  void registerFeature( const std::string& entname,FeatureConstructor_ptr ent );
-  FeatureAbstract* newFeature( const std::string& name,const std::string& objname );
-  bool existFeature( const std::string& name, FeatureMap::iterator& entPtr );
-  bool existFeature( const std::string& name );
-
-  void  commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
-		     std::ostream& os );
-
-
-};
-
-SOT_CORE_EXPORT extern FactoryStorage sotFactory;
-
-/* --- REGISTERER ----------------------------------------------------------- */
-/* --- REGISTERER ----------------------------------------------------------- */
-/* --- REGISTERER ----------------------------------------------------------- */
-
-/* -------------------------------------------------------------------------- */
-/* --- ENTITY REGISTERER ---------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-typedef dynamicgraph::EntityRegisterer sotEntityRegisterer;
-
-#define SOT_FACTORY_ENTITY_PLUGIN(sotClassType,className) \
-	DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(sotClassType, className)
-
-/* -------------------------------------------------------------------------- */
-/* --- FEATURE REGISTERER --------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-class SOT_CORE_EXPORT sotFeatureRegisterer
-{
- private:
-  sotFeatureRegisterer( void );
-
- public:
-  sotFeatureRegisterer( const std::string& featureClassName,
-			FactoryStorage::FeatureConstructor_ptr maker);
-};
-
-#define SOT_FACTORY_FEATURE_PLUGIN(sotFeatureType,className) \
-  const std::string sotFeatureType::CLASS_NAME = className;  \
-  extern "C" {                                               \
-    FeatureAbstract *sotFeatureMaker##_##sotFeatureType( const std::string& objname )\
-    {                                                        \
-      return new sotFeatureType( objname );                  \
-    }                                                        \
-  sotFeatureRegisterer reg##_##sotFeatureType( className,    \
-					       &sotFeatureMaker##_##sotFeatureType );   \
-  }                                                          \
-
-
-/* -------------------------------------------------------------------------- */
-/* --- TASK REGISTERER ------------------------------------------------------ */
-/* -------------------------------------------------------------------------- */
-
-class SOT_CORE_EXPORT sotTaskRegisterer
-{
- private:
-  sotTaskRegisterer( void );
-
- public:
-  sotTaskRegisterer( const std::string& taskClassName,
-		     FactoryStorage::TaskConstructor_ptr maker);
-};
-
-
-#define SOT_FACTORY_TASK_PLUGIN(sotTaskType,className) \
-  const std::string sotTaskType::CLASS_NAME = className; \
-  extern "C" {                                            \
-    TaskAbstract *sotTaskMaker##_##sotTaskType( const std::string& objname )    \
-    {                                                     \
-      return new sotTaskType( objname );                 \
-    }                                                     \
-  sotTaskRegisterer reg##_##sotTaskType( className,&sotTaskMaker##_##sotTaskType );   \
-  }                                                       \
-
-} // namespace sot
-
-#endif /* #ifndef __SOT_FACTORY_HH__ */
-
-
-
-

include/sot-core/feature-abstract.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      feature-abstract.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-#ifndef __SOT_FEATURE_ABSTRACT_H__
-#define __SOT_FEATURE_ABSTRACT_H__
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-/* SOT */
-#include <sot-core/flags.h>
-#include <dynamic-graph/all-signals.h>
-#include <dynamic-graph/entity.h>
-#include <sot-core/sot-core-api.h>
-
-/* STD */
-#include <string>
-
-namespace sot {
-namespace dg = dynamicgraph;
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/*! \class FeatureAbstract
-  \ingroup features
-  \brief This class gives the abstract definition of a feature.
-
-  A feature is a data evolving according to time.
-  It is defined by a vector \f${\bf s}(t) \in \mathbb{R}^n \f$ where \f$ t \f$ is the time.
-  By default a feature has a desired \f${\bf s}^*(t) \f$.
-  The feature is in charge of collecting its own current state.
-  A feature is supposed to compute an error between its current state and the desired one:
-  \f$ e(t) = {\bf s}^*(t) - {\bf s}(t) \f$.
-  A feature is supposed to compute a Jacobian according to the robot state vector
-  \f$ \frac{\delta {\bf s}(t)}{\delta {\bf q}(t)}\f$.
- */
-class SOT_CORE_EXPORT FeatureAbstract
-:public dg::Entity
-{
- public:
-  /*! \brief Store the name of the class. */
-  static const std::string CLASS_NAME;
-
-  /*! \brief Returns the name class. */
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
-  /*! \brief Register the feature in the stack of tasks. */
-  void featureRegistration( void );
-
- public:
-  /*! \brief Default constructor: the name of the class should be given. */
-  FeatureAbstract( const std::string& name );
-  /*! \brief Default destructor */
-  virtual ~FeatureAbstract( void ) {};
-
-  /*! \name Methods returning the dimension of the feature.
-    @{ */
-
-  /*! \brief Verbose method.
-    \par res: The integer in which the dimension will be return.
-    \par time: The time at which the feature should be considered.
-    \return Dimension of the feature.
-    \note Be careful with features changing their dimension according to time.
-   */
-  virtual unsigned int& getDimension( unsigned int& res,int time ) = 0;
-
-  /*! \brief Short method 
-    \par time: The time at which the feature should be considered.
-    \return Dimension of the feature.
-    \note Be careful with features changing their dimension according to time.
-   */
-  inline unsigned int getDimension( int time )
-    {unsigned int res; getDimension(res,time); return res;}
-
-  /*! \brief Shortest method 
-    \return Dimension of the feature.
-    \note The feature is not changing its dimension according to time.
-   */
-  inline unsigned int getDimension( void ) const 
-    { return dimensionSOUT; }
-  /*! @} */
-
-  /*! \name Methods to control internal computation.
-    The main idea is that some feature may have a lower frequency
-    than the internal control loop. In this case, the methods for
-    computation are called only when needed.
-    
-   @{*/
-
-  /*! \brief Compute the error between the desired feature and
-   the current value of the feature measured or deduced from the robot state. 
-
-   \par[out] res: The error will be set into res.
-   \par[in] time: The time at which the error is computed.
-   \return The vector res with the appropriate value.
-  */
-  virtual ml::Vector& computeError( ml::Vector& res,int time ) = 0; 
-
-  /*! \brief Compute the Jacobian of the error according the robot state. 
-
-    \par[out] res: The matrix in which the error will be written.
-    \par[in] time: The time at which the Jacobian is computed \f$ {\bf J}(q(t)) \f$.
-    \return The matrix res with the appropriate values. 
-   */
-  virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time ) = 0; 
-
-  /*! \brief Reevaluate the current value of the feature 
-    according to external measurement provided through a mailbox,
-    or deduced from the estimated state of the robot at the time specified.
-
-    \par[out] res: The vector in which the value will be written.
-    \par[in] time: The time at which the feature is evaluated \f$ {\bf s}(t)) \f$.
-    \return The vector res with the appropriate values. 
-   */  
-  virtual ml::Vector& computeActivation( ml::Vector& res,int time ) = 0; 
-
-  /*! @} */
-
-  /* --- SIGNALS ------------------------------------------------------------ */
- public:
-  
-  /*! \name Signals 
-   @{
-  */
-
-  /*! \name Input signals: 
-   @{ */
-  /*! \brief This signal specifies the desired value \f$ {\bf s}^*(t) \f$ */
-  dg::SignalPtr< FeatureAbstract*,int > desiredValueSIN;
-
-  /*! \brief This vector specifies which dimension are used to perform the computation.
-   For instance let us assume that the feature is a 3D point. If only the Y-axis should
-   be used for computing error, activation and Jacobian, then the vector to specify
-   is \f$ [ 0 1 0] \f$.*/
-  dg::SignalPtr< Flags,int > selectionSIN;
-  /*! @} */
-
-  /*! \name Output signals: 
-   @{ */
-
-  /*! \brief This signal returns the error between the desired value and
-    the current value : \f$ {\bf s}^*(t) - {\bf s}(t)\f$ */
-  dg::SignalTimeDependent<ml::Vector,int> errorSOUT;
-  
-  /*! \brief This signal returns the Jacobian of the current value 
-    according to the robot state: \f$ J(t) = \frac{\delta{\bf s}^*(t)}{\delta {\bf q}(t)}\f$ */
-  dg::SignalTimeDependent<ml::Matrix,int> jacobianSOUT;
-  
-  /*! \brief Compute the new value of the feature \f$ {\bf s}(t)\f$ */
-  dg::SignalTimeDependent<ml::Vector,int> activationSOUT;
-
-  /*! \brief Returns the dimension of the feature as an output signal. */
-  dg::SignalTimeDependent<unsigned int,int> dimensionSOUT;
-
-  /*! \brief This method write a graph description on the file named FileName. */
-  virtual std::ostream & writeGraph(std::ostream & os) const;
-
-  /*! @} */
-
-};
-
-} // namespace sot
-
-
-
-#endif // #ifndef __SOT_FEATURE_ABSTRACT_H__
-

include/sot-core/feature-point6d.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      feature-point6d.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-#ifndef __SOT_FEATURE_POINT6D_HH__
-#define __SOT_FEATURE_POINT6D_HH__
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* SOT */
-#include <sot-core/feature-abstract.h>
-#include <sot-core/exception-task.h>
-#include <sot-core/matrix-homogeneous.h>
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32) 
-#  if defined (feature_point_6d_EXPORTS)
-#    define SOTFEATUREPOINT6D_EXPORT __declspec(dllexport)
-#  else  
-#    define SOTFEATUREPOINT6D_EXPORT __declspec(dllimport)
-#  endif 
-#else
-#  define SOTFEATUREPOINT6D_EXPORT
-#endif
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-namespace sot {
-namespace dg = dynamicgraph;
-
-/*!
-  \class FeaturePoint6d
-  \brief Class that defines point-3d control feature
-*/
-class SOTFEATUREPOINT6D_EXPORT FeaturePoint6d 
-: public FeatureAbstract
-{
-
- public: 
-  static const std::string CLASS_NAME;
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- protected:
-  enum ComputationFrameType 
-    {
-      FRAME_DESIRED
-      ,FRAME_CURRENT 
-    };
-  static const ComputationFrameType COMPUTATION_FRAME_DEFAULT;
-  ComputationFrameType computationFrame;
-
-  /* --- SIGNALS ------------------------------------------------------------ */
- public:
-  dg::SignalPtr< MatrixHomogeneous,int > positionSIN;
-  dg::SignalPtr< ml::Matrix,int > articularJacobianSIN;
-
-  using FeatureAbstract::desiredValueSIN;
-  using FeatureAbstract::selectionSIN;
-
-  using FeatureAbstract::jacobianSOUT;
-  using FeatureAbstract::errorSOUT;
-  using FeatureAbstract::activationSOUT;
-
- public:
-  FeaturePoint6d( const std::string& name );
-  virtual ~FeaturePoint6d( void ) {}
-
-  virtual unsigned int& getDimension( unsigned int & dim, int time );
-  
-  virtual ml::Vector& computeError( ml::Vector& res,int time ); 
-  virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time ); 
-  virtual ml::Vector& computeActivation( ml::Vector& res,int time ); 
-
-  /** Static Feature selection. */
-  inline static Flags selectX( void )  { return FLAG_LINE_1; }
-  inline static Flags selectY( void )  { return FLAG_LINE_2; }
-  inline static Flags selectZ( void )  { return FLAG_LINE_3; }
-  inline static Flags selectRX( void ) { return FLAG_LINE_4; }
-  inline static Flags selectRY( void ) { return FLAG_LINE_5; }
-  inline static Flags selectRZ( void ) { return FLAG_LINE_6; }
-
-  inline static Flags selectTranslation( void ) { return Flags(7); }
-  inline static Flags selectRotation( void ) { return Flags(56); }
-
-  virtual void display( std::ostream& os ) const;
-
-  virtual void commandLine( const std::string& cmdLine,
-			    std::istringstream& cmdArgs,
-			    std::ostream& os );
-
-} ;
-
-} // namespace sot
-
-#endif // #ifndef __SOT_FEATURE_POINT6D_HH__
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */

include/sot-core/fir-filter.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2008
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      fir-filter.h
- * Project:   SOT
- * Author:    Paul Evrard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOT_FIRFILTER_HH__
-#define __SOT_FIRFILTER_HH__
-
-#include <cassert>
-
-#include <vector>
-#include <algorithm>
-#include <iterator>
-
-//#include <boost/circular_buffer.hpp>
-
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/all-signals.h>
-namespace dg = dynamicgraph;
-
-
-namespace detail
-{
-  /* GRMBL boost-sandox::circular_buffer smells... Why keep using 1.33?!
-   * As a workaround, only the part of circular_buffer's interface used here is implemented.
-   * Ugly, fatty piece of code.
-   */
-  template<class T>
-  class circular_buffer
-  {
-  public:
-    circular_buffer(): buf(1), start(0), numel(0) {}
-    void push_front(const T& data) {
-      if(start){ --start; } else { start = buf.size()-1; }
-      buf[start] = data;
-      if(numel < buf.size()){ ++numel; }
-    }
-    void reset_capacity(size_t n) {
-      buf.resize(n);
-      start = 0;
-      numel = 0;
-    }
-    void reset_capacity(size_t n, const T& el) {
-      buf.clear();
-      buf.resize(n, el);
-      start = 0;
-      numel = 0;
-    }
-    T& operator[](size_t i) {
-      assert((i<numel) && "Youre accessing an empty buffer");
-      size_t index = (start+i) % buf.size();
-      return buf[index];
-    }
-    size_t size() const { return numel; }
-  private:
-    std::vector<T> buf;
-    size_t start;
-    size_t numel;
-  };
-}
-
-template<class sigT, class coefT>
-class FIRFilter
-  : public dg::Entity
-{
-public:
-  virtual const std::string& getClassName() const { return dg::Entity::getClassName(); }
-  static std::string getTypeName( void ) { return "Unknown"; }
-  static const std::string CLASS_NAME;
-
-public:
-  FIRFilter( const std::string& name )
-    : dg::Entity(name)
-    , SIN(NULL,"sotFIRFilter("+name+")::input(T)::in")
-    , SOUT(boost::bind(&FIRFilter::compute,this,_1,_2),
-	   SIN,
-	   "sotFIRFilter("+name+")::output(T)::out")
-  {
-    signalRegistration( SIN<<SOUT );
-  }
-
-  virtual ~FIRFilter() {}
-
-  virtual sigT& compute( sigT& res,int time )
-  {
-    const sigT& in = SIN.access( time );
-    reset_signal( res, in );
-    data.push_front( in );
-
-    size_t SIZE = std::min(data.size(), coefs.size());
-    for(size_t i = 0; i < SIZE; ++i) {
-      res += coefs[i] * data[i];
-    }
-
-    return res;
-  }
-
-  virtual void commandLine( const std::string& cmdLine,
-			    std::istringstream& cmdArgs,
-			    std::ostream& os )
-  {
-    if(cmdLine == "setCoefs") {
-      coefT tmp;
-      std::vector<coefT> ncoefs;
-      while(cmdArgs>>tmp){ ncoefs.push_back(tmp); }
-      if(!ncoefs.empty()){
-	coefs.swap(ncoefs);
-	data.reset_capacity(coefs.size());
-      }
-      else {
-	std::copy( coefs.begin(), coefs.end(),
-		  std::ostream_iterator<coefT>(os, " ") );
-	os << std::endl;
-      }
-    }
-    else if(cmdLine == "printBuffer") {
-      for(size_t i = 0; i < data.size(); ++i){ os << data[i] << std::endl; }
-    }
-    else { dg::Entity::commandLine( cmdLine, cmdArgs, os); }
-  }
-
-  static void reset_signal(sigT& res, const sigT& sample) { }
-
-public:
-  dg::SignalPtr<sigT, int> SIN;
-  dg::SignalTimeDependent<sigT, int> SOUT;
-
-private:
-  std::vector<coefT> coefs;
-  detail::circular_buffer<sigT> data;
-};
-
-#endif

include/sot-core/flags.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      flags.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-#ifndef __SOT_FLAGS_H
-#define __SOT_FLAGS_H
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* STD */
-#include <vector>
-#include <iostream>
-
-/* SOT */
-#include <sot-core/sot-core-api.h>
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-namespace sot {
-
-class SOT_CORE_EXPORT Flags
-{
- protected:
-  std::vector<char> flags;
-  bool reverse;
-
-  char operator[]( const unsigned int& i ) const;
-    
-  
- public:
-  
-  Flags( const bool& b=false ) ;
-  Flags( const char& c ) ;
-  Flags( const int& c4 ) ;
-
-  void add( const char& c ) ;
-  void add( const int& c4 ) ;
-    
-  Flags operator! (void) const;
-  SOT_CORE_EXPORT friend Flags operator& ( const Flags& f1,const Flags& f2 ) ;
-  SOT_CORE_EXPORT friend Flags operator| ( const Flags& f1,const Flags& f2 ) ;
-  Flags& operator&= ( const Flags& f2 ) ;
-  Flags& operator|= ( const Flags& f2 ) ;
-   
-  SOT_CORE_EXPORT friend Flags operator& ( const Flags& f1,const bool& b ) ;
-  SOT_CORE_EXPORT friend Flags operator| ( const Flags& f1,const bool& b ) ;
-  Flags& operator&= ( const bool& b );
-  Flags& operator|= ( const bool& b );
-  
-  SOT_CORE_EXPORT friend std::ostream& operator<< (std::ostream& os, const Flags& fl );
-  SOT_CORE_EXPORT friend char operator>> (const Flags& flags, const int& i);
-  SOT_CORE_EXPORT friend std::istream& operator>> (std::istream& is, Flags& fl );
-  bool operator() (const int& i) const;
-
-  operator bool (void) const;
-
-  void unset( const unsigned int & i );
-  void set( const unsigned int & i );
-
- public:  /* Selec "matlab-style" : 1:15, 1:, :45 ... */
-  
-  static void readIndexMatlab( std::istream & iss,
-			       unsigned int & indexStart,
-			       unsigned int & indexEnd,
-			       bool& unspecifiedEnd );
-  static Flags readIndexMatlab( std::istream & iss );
-
-	  
-
-
-};
-
-SOT_CORE_EXPORT extern const Flags FLAG_LINE_1;
-SOT_CORE_EXPORT extern const Flags FLAG_LINE_2;
-SOT_CORE_EXPORT extern const Flags FLAG_LINE_3;
-SOT_CORE_EXPORT extern const Flags FLAG_LINE_4;
-SOT_CORE_EXPORT extern const Flags FLAG_LINE_5;
-SOT_CORE_EXPORT extern const Flags FLAG_LINE_6;
-SOT_CORE_EXPORT extern const Flags FLAG_LINE_7;
-SOT_CORE_EXPORT extern const Flags FLAG_LINE_8;
-
-} // namespace sot
-
-#endif /* #ifndef __SOT_FLAGS_H */

include/sot-core/import-default-paths.h

-// -*- c++ -*-
-#ifndef SOT_FACTORY_COMMAND_IMPORT_DEFAULT_PATHS_H
-# define SOT_FACTORY_COMMAND_IMPORT_DEFAULT_PATHS_H
-
-/// Default script path as known by CMake at configure time.
-# define SOT_IMPORT_DEFAULT_PATHS "/home/blue/sot-lib/script"
-
-#endif //! SOT_FACTORY_COMMAND_IMPORT_DEFAULT_PATHS_H

include/sot-core/import-default-paths.h.cmake

-// -*- c++ -*-
-#ifndef SOT_FACTORY_COMMAND_IMPORT_DEFAULT_PATHS_H
-# define SOT_FACTORY_COMMAND_IMPORT_DEFAULT_PATHS_H
-
-/// Default script path as known by CMake at configure time.
-# define SOT_IMPORT_DEFAULT_PATHS @SOT_IMPORT_DEFAULT_PATHS@
-
-#endif //! SOT_FACTORY_COMMAND_IMPORT_DEFAULT_PATHS_H

include/sot-core/import.h

-// -*- c++ -*-
-#ifndef SOT_FACTORY_COMMAND_IMPORT_H
-# define SOT_FACTORY_COMMAND_IMPORT_H
-# include <iosfwd>
-# include <string>
-# include <vector>
-
-class sotInterpretor;
-
-namespace sot
-{
-  namespace command
-  {
-    namespace
-    {
-      extern std::vector<std::string> importPaths;
-    } // end of anonymous namespace.
-
-    /// \brief Implement sot interpretor import command.
-    ///
-    /// The import command sources a file and searches automatically
-    /// for it in the importPaths.
-    void import (sotInterpretor& interpretor,
-		 const std::string& cmdLine,
-		 std::istringstream& cmdArg,
-		 std::ostream& os);
-
-    /// \brief Implement sot interpretor pushImportPaths command.
-    ///
-    /// Append a path to importPaths.
-    void pushImportPaths (sotInterpretor& interpretor,
-			  const std::string& cmdLine,
-			  std::istringstream& cmdArg,
-			  std::ostream& os);
-
-    /// \brief Implement sot interpretor popImportPaths command.
-    ///
-    /// Drop the last path of importPaths.
-    void popImportPaths (sotInterpretor& interpretor,
-			 const std::string& cmdLine,
-			 std::istringstream& cmdArg,
-			 std::ostream& os);
-
-  } // end of namespace command.
-} // end of namespace sot.
-
-#endif //! SOT_FACTORY_COMMAND_IMPORT_H

include/sot-core/integrator-euler.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      integrator-euler.h
- * Project:   SOT
- * Author:    Paul Evrard and Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-#ifndef __SOT_INTEGRATOR_EULER_H__
-#define __SOT_INTEGRATOR_EULER_H__
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* SOT */
-#include <sot-core/integrator-abstract.h>
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-namespace sot {
-namespace dg = dynamicgraph;
-
-/*!
- * \class IntegratorEuler
- * \brief integrates an ODE using a naive Euler integration.
- * TODO: change the integration method. For the moment, the highest
- * derivative of the output signal is computed using the
- * previous values of the other derivatives and the input
- * signal, then integrated n times, which will most certainly
- * induce a huge drift for ODEs with a high order at the denominator.
-*/
-template<class sigT,class coefT>
-class IntegratorEuler
-  : public IntegratorAbstract<sigT,coefT>
-{
-
- public: 
-  virtual const std::string& getClassName( void ) const { return dg::Entity::getClassName(); }
-  static std::string getTypeName( void ) { return "Unknown"; }
-  static const std::string CLASS_NAME;
-
- public:
-  using IntegratorAbstract<sigT,coefT>::SIN;
-  using IntegratorAbstract<sigT,coefT>::SOUT;
-  using IntegratorAbstract<sigT,coefT>::numerator;
-  using IntegratorAbstract<sigT,coefT>::denominator;
-
- public:
-  IntegratorEuler( const std::string& name )
-    : IntegratorAbstract<sigT,coefT>( name )
-  {
-    SOUT.addDependency(SIN);
-  }
-
-  virtual ~IntegratorEuler( void ) {}
-
-protected:
-  std::vector<sigT> inputMemory;
-  std::vector<sigT> outputMemory;
-
-public:
-  sigT& integrate( sigT& res, int time )
-  {
-    sotDEBUG(15)<<"# In {"<<std::endl;
-
-    const double dt = 0.005;
-    const double invdt = 200;
-
-    sigT sum;
-    sigT tmp1, tmp2;
-    const std::vector<coefT>& num = numerator;
-    const std::vector<coefT>& denom = denominator;
-
-    // Step 1
-    tmp1 = inputMemory[0];
-    inputMemory[0] = SIN.access(time);
-    sum.resize(tmp1.size());
-    sum = denom[0] * inputMemory[0];
-    // End of step 1. Here, sum is b_0 X
-
-    // Step 2
-    int denomsize = denom.size();
-    for(int i = 1; i < denomsize; ++i)
-    {
-      tmp2 = inputMemory[i-1] - tmp1;
-      tmp2 *= invdt;
-      tmp1 = inputMemory[i];
-      inputMemory[i] = tmp2;
-      sum += (denom[i] * inputMemory[i]);
-    }
-    // End of step 2. Here, sum is b_m * d(m)X / dt^m + ... - b_0 X
-
-    // Step 3
-    int numsize = num.size() - 1;
-    for(int i = 0; i < numsize; ++i)
-    {
-      sum -= (num[i] * outputMemory[i]);
-    }
-    // End of step 3. Here, sum is b_m * d(m)X / dt^m + ... - b_0 X - a_0 Y - ... a_n-1 d(n-1)Y / dt^(n-1)
-
-    // Step 4
-    outputMemory[numsize] = sum;
-    for(int i = numsize - 1; i >= 0; --i)
-    {
-      outputMemory[i] += (outputMemory[i+1] * dt);
-    }
-    // End of step 4. The ODE is integrated
-
-    inputMemory[0] = SIN.access(time);
-    res = outputMemory[0];
-
-    sotDEBUG(15)<<"# Out }"<<std::endl;
-    return res;
-  }
-};
-
-} // namespace sot
-
-
-
-
-#endif

include/sot-core/joint-limitator.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      JointLimitator.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-#ifndef __SOT_FEATURE_JOINTLIMITS_HH__
-#define __SOT_FEATURE_JOINTLIMITS_HH__
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-/* SOT */
-#include <dynamic-graph/entity.h>
-#include <sot-core/exception-task.h>
-#include <dynamic-graph/all-signals.h>
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32) 
-#  if defined (joint_limitator_EXPORTS)
-#    define SOTJOINTLIMITATOR_EXPORT __declspec(dllexport)
-#  else  
-#    define SOTJOINTLIMITATOR_EXPORT __declspec(dllimport)
-#  endif 
-#else
-#  define SOTJOINTLIMITATOR_EXPORT
-#endif
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-
-namespace sot {
-
-namespace dg = dynamicgraph;
-
-/*!
-  \class JointLimitator
-*/
-class SOTJOINTLIMITATOR_EXPORT JointLimitator
-: public dg::Entity
-{
-
- public: 
-  static const std::string CLASS_NAME;
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- protected:
-  
-  double threshold;
-  const static double THRESHOLD_DEFAULT; // = .99;
-
-  /* --- SIGNALS ------------------------------------------------------------ */
- public:
-
-  dg::SignalPtr< ml::Vector,int > jointSIN;
-  dg::SignalPtr< ml::Vector,int > upperJlSIN;
-  dg::SignalPtr< ml::Vector,int > lowerJlSIN;
-  dg::SignalPtr< ml::Vector,int > controlSIN;
-  dg::SignalTimeDependent< ml::Vector,int > controlSOUT;
-  dg::SignalTimeDependent< ml::Vector,int > widthJlSINTERN;
-
- public:
-  JointLimitator( const std::string& name );
-  virtual ~JointLimitator( void ) {}
-
-  virtual ml::Vector& computeControl( ml::Vector& res,int time ); 
-  ml::Vector& computeWidthJl( ml::Vector& res,const int& time );
-
-  virtual void display( std::ostream& os ) const;
-  void commandLine( const std::string& cmdLine,
-		    std::istringstream& cmdArgs,
-		    std::ostream& os );
-} ;
-
-
-} // namespace sot
-
-
-
-#endif // #ifndef __SOT_FEATURE_JOINTLIMITS_HH__
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */

include/sot-core/kalman.h

-
-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      Kalman.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOT_KALMAN_H
-#define __SOT_KALMAN_H
-
-/* -------------------------------------------------------------------------- */
-/* --- INCLUDE -------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-/* SOT */
-#include <dynamic-graph/all-signals.h>
-#include <dynamic-graph/entity.h>
-#include <sot-core/constraint.h>
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32) 
-#  if defined (kalman_EXPORTS)
-#    define SOTKALMAN_EXPORT __declspec(dllexport)
-#  else  
-#    define SOTKALMAN_EXPORT __declspec(dllimport)
-#  endif 
-#else
-#  define SOTKALMAN_EXPORT
-#endif
-
-/* -------------------------------------------------------------------------- */
-/* --- CLASSE --------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-namespace sot {
-
-namespace dg = dynamicgraph;
-
-class SOTKALMAN_EXPORT Kalman
-:public dg::Entity
-{
- public: 
-  static const std::string CLASS_NAME;
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-  
-
-protected:
-  unsigned int size_state ;
-  unsigned int size_measure ;
-  double dt;
-
- public: 
-
-  dg::SignalPtr< ml::Vector,int > measureSIN;         // Y=X_mes=H.X
-  dg::SignalPtr< ml::Matrix,int > modelTransitionSIN; // A
-  dg::SignalPtr< ml::Matrix,int > modelMeasureSIN;    // H
-  dg::SignalPtr< ml::Matrix,int > noiseTransitionSIN; // Q
-  dg::SignalPtr< ml::Matrix,int > noiseMeasureSIN;    // R
-  dg::SignalPtr< ml::Matrix,int > modelControlSIN;    // B
-  dg::SignalPtr< ml::Vector,int > controlSIN;         // u
-
-  dg::SignalPtr< ml::Vector,int > statePrecSIN;       // X(t=0)
-  dg::SignalPtr< ml::Matrix,int > variancePrecSIN;    // P(t=0)
-
-  dg::SignalTimeDependent< ml::Vector,int > statePredictedSOUT; // Xpre
-  dg::SignalTimeDependent< ml::Matrix,int > variancePredictedSOUT; // Ppre
-  dg::SignalTimeDependent< ml::Vector,int > stateUpdatedSOUT;  // Xest
-/*   dg::SignalTimeDependent< ml::Matrix,int > varianceUpdatedSOUT;   // Pest */
-
-/*   dg::SignalTimeDependent< ml::Matrix,int > gainSINTERN;         // K */
-/*   dg::SignalTimeDependent< ml::Matrix,int > innovationSINTERN;   // S */
-
-
-public:
-  /*!  \f${\bf x}_{k \mid k} \f$ valeur estime de l'etat  
-   * \f${\bf x}_{k \mid k} =  {\bf x}_{k \mid k-1} + {\bf W}_k
-   * \left[ {\bf z}_k -  {\bf H x}_{k \mid k-1} \right]\f$
-   */
-  //ml::Vector Xest ;
-
-  /*! \f${\bf x}_{k \mid k-1} \f$ : valeur predite  
-   * de l'etat \f$ {{\bf x}}_{k|k-1}  =  {\bf A}_{k-1} 
-   * {\bf x}_{k-1\mid k-1}\f$.
-   */
-  //ml::Vector Xpre ;
-
-protected:
-  /*! \f${\bf P}_{k \mid k-1} \f$ : matrice de
-   * covariance de l'erreur de prediction.
-   * \f$ {\bf A}_{k-1}  {\bf P}_{k-1 \mid k-1} 
-   * {\bf A}^T_{k-1}   + {\bf Q}_k\f$. 
-   */
-  //ml::Matrix Ppre ;
-
-  /*!  \f${\bf P}_{k \mid k}\f$ matrice de covariance 
-   * de l'erreur d'estimation.
-   * \f${\bf P}_{k \mid k} = \left({\bf I - W}_k {\bf H} \right)  
-   * {\bf P}_{k \mid  k-1}\f$
-   */
-  ml::Matrix Pest ;
-
-  /*! \f${\bf W}_k\f$ : Gain du filtre de Kalman
-   * \f$ {\bf W}_k = {\bf P}_{k \mid k-1} {\bf H}^T 
-   * \left[  {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k \right]^{-1}\f$ 
-   */
-  ml::Matrix W ;
-
-  /*! \f$ \bf I\f$ : matrice identite. */
-  //ml::Matrix Ident;
-
-public:
-  //! matrice d�crivant le modele d'evolution de l'etat
-  //ml::Matrix A ;
-  //! matrice d�crivant le modele d'evolution de la mesure
-  //ml::Matrix H ;
-  //! Variance du bruits sur le modele de mesure
-  //ml::Matrix R ;
-  //! Variance du bruits sur le modele d'etat
-  //ml::Matrix Q ;
-  //! Command: X_+1 = A.X + Bu
-  //ml::Matrix B;
-  //ml::Vector u;
-
-
-public:
-  Kalman( const std::string & name ) ;
-  void  Init( int _size_state,int _size_measure );
-
-  //double Prediction( const ml::Matrix &A ) ;
-  //double Prediction( const ml::Matrix &A,const ml::Vector&u ) ;
-  //double Prediction( void ) ;
-  double Prediction(int n_sensor) ;
-  //double Filtering(ml::Vector &Xmes) ;
-  double Filtering(int n_sensor, ml::Vector *Xmes) ;
-
-  ml::Vector& predict( ml::Vector& Xpre,const int& time );
-  ml::Vector& filter( ml::Vector& Xest,const int& time );
-  ml::Matrix & computeVariancePredicted( ml::Matrix& Ppre,const int& time ); 
-/*   ml::Matrix & computeVarianceUpdated( ml::Matrix& Pest,const int& time ); */
-
-  void reset( void );
-
-  /* --- Initialization --- */
-  void initFilterCteAcceleration( const double &dt, 
-				  const ml::Vector &Z0, 
- 				  const ml::Vector &Z1, 
- 				  const ml::Vector &Z2, 
- 				  const ml::Vector &sigma_noise, 
-				  const ml::Vector &sigma_state ) ; 
-  void initFilterCteVelocity( const double &dt, 
-			      const ml::Vector &Z0, 
-			      const ml::Vector &Z1, 
-			      const ml::Vector &sigma_noise, 
-			      const ml::Vector &sigma_state );
-/*   void initFilterSinger( const double &dt, */
-/* 			 const double &a, */
-/* 			 const ml::Vector &Z0, */
-/* 			 const ml::Vector &Z1, */
-/* 			 const ml::Vector  &sigma_noise, */
-/* 			 const ml::Vector &sigma_state) ; */
-/*   void initFilterCteVelocityRobot( const double& dt,  */
-/* 				   const ml::Vector &Z0,  */
-/* 				   const ml::Vector &Z1,  */
-/* 				   const ml::Vector &sigma_noise,  */
-/* 				   const ml::Vector &sigma_state ); */
-/*   void initFilterCteAccelerationRobot( const double &dt, */
-/* 				       const ml::Vector &Z0, */
-/* 				       const ml::Vector &Z1, */
-/* 				       const ml::Vector &Z2, */
-/* 				       const ml::Vector &sigma_noise, */
-/* 				       const ml::Vector &sigma_state ) ; */
-
-  static ml::Matrix& A_constantAcc( const int& sizeMeasure,
-				    const double& dt,
-				    ml::Matrix & A );
-  
-  static  ml::Matrix& A_constantSpeed( const int& sizeMeasure,
-				       const double& dt,
-				       ml::Matrix & A );
-  
-  static  ml::Matrix& A_constantSpeedRobot( const int& sizeMeasure,
-					    const double& dt,
-					    ml::Matrix & A );
-  
-  /* --- Entity --- */
-  void display( std::ostream& os ) const;
-  void commandLine( const std::string& cmdLine,
-		    std::istringstream& cmdArgs,
-		    std::ostream& os );
-    
-} ;
-
-
-} // namespace sot
-
-
-
-/*!
-  \file Kalman.h
-  \brief  Generic kalman filtering implementation
-*/
-
-#endif

include/sot-core/macros-signal.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      macros-signal.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-/* --- GENERIC MACROS ------------------------------------------------------- */
-/* --- GENERIC MACROS ------------------------------------------------------- */
-/* --- GENERIC MACROS ------------------------------------------------------- */
-
-#define SOT_CALL_SIG(sotName,sotType)          \
-  boost::bind(&Signal<sotType,int>::access, \
-	      &sotName,_2)
-
-/* --- STATIC MACROS -------------------------------------------------------- */
-/* --- STATIC MACROS -------------------------------------------------------- */
-/* --- STATIC MACROS -------------------------------------------------------- */
-
-#define SOT_INIT_SIGNAL_1(sotFunction,             \
-			  sotArg1,sotArg1Type)     \
-boost::bind(&sotFunction,                          \
-	    SOT_CALL_SIG(sotArg1,sotArg1Type),_1), \
-  sotArg1
-
-#define SOT_INIT_SIGNAL_2(sotFunction,             \
-			  sotArg1,sotArg1Type,     \
-			  sotArg2,sotArg2Type)     \
-boost::bind(&sotFunction,                          \
-	    SOT_CALL_SIG(sotArg1,sotArg1Type),     \
-	    SOT_CALL_SIG(sotArg2,sotArg2Type),_1), \
-  sotArg1<<sotArg2
-#define SOT_INIT_SIGNAL_3(sotFunction,             \
-			  sotArg1,sotArg1Type,     \
-			  sotArg2,sotArg2Type,     \
-			  sotArg3,sotArg3Type)     \
-boost::bind(&sotFunction,                          \
-	    SOT_CALL_SIG(sotArg1,sotArg1Type),     \
-	    SOT_CALL_SIG(sotArg2,sotArg2Type),     \
-	    SOT_CALL_SIG(sotArg3,sotArg3Type),_1), \
-  sotArg1<<sotArg2<<sotArg3
-
-#define SOT_INIT_SIGNAL_4(sotFunction,             \
-			  sotArg1,sotArg1Type,     \
-			  sotArg2,sotArg2Type,     \
-			  sotArg3,sotArg3Type,     \
-			  sotArg4,sotArg4Type)     \
-boost::bind(&sotFunction,                          \
-	    SOT_CALL_SIG(sotArg1,sotArg1Type),     \
-	    SOT_CALL_SIG(sotArg2,sotArg2Type),     \
-	    SOT_CALL_SIG(sotArg3,sotArg3Type),     \
-	    SOT_CALL_SIG(sotArg4,sotArg4Type),_1), \
-  sotArg1<<sotArg2<<sotArg3<<sotArg4
-
-#define SOT_INIT_SIGNAL_5(sotFunction,             \
-			  sotArg1,sotArg1Type,     \
-			  sotArg2,sotArg2Type,     \
-			  sotArg3,sotArg3Type,     \
-			  sotArg4,sotArg4Type,     \
-			  sotArg5,sotArg5Type)     \
-boost::bind(&sotFunction,                          \
-	    SOT_CALL_SIG(sotArg1,sotArg1Type),     \
-	    SOT_CALL_SIG(sotArg2,sotArg2Type),     \
-	    SOT_CALL_SIG(sotArg3,sotArg3Type),     \
-	    SOT_CALL_SIG(sotArg4,sotArg4Type),     \
-	    SOT_CALL_SIG(sotArg5,sotArg5Type),_1), \
-  sotArg1<<sotArg2<<sotArg3<<sotArg4<<sotArg5
-
-#define SOT_INIT_SIGNAL_6(sotFunction,             \
-			  sotArg1,sotArg1Type,     \
-			  sotArg2,sotArg2Type,     \
-			  sotArg3,sotArg3Type,     \
-			  sotArg4,sotArg4Type,     \
-			  sotArg5,sotArg5Type,     \
-			  sotArg6,sotArg6Type)     \
-boost::bind(&sotFunction,                          \
-	    SOT_CALL_SIG(sotArg1,sotArg1Type),     \
-	    SOT_CALL_SIG(sotArg2,sotArg2Type),     \
-	    SOT_CALL_SIG(sotArg3,sotArg3Type),     \
-	    SOT_CALL_SIG(sotArg4,sotArg4Type),     \
-	    SOT_CALL_SIG(sotArg5,sotArg5Type),     \
-	    SOT_CALL_SIG(sotArg6,sotArg6Type),_1), \
-  sotArg1<<sotArg2<<sotArg3<<sotArg4<<sotArg5<<sotArg6
-
-#define SOT_INIT_SIGNAL_7(sotFunction,             \
-			  sotArg1,sotArg1Type,     \
-			  sotArg2,sotArg2Type,     \
-			  sotArg3,sotArg3Type,     \
-			  sotArg4,sotArg4Type,     \
-			  sotArg5,sotArg5Type,     \
-			  sotArg6,sotArg6Type,     \
-			  sotArg7,sotArg7Type)     \
-boost::bind(&sotFunction,                          \
-	    SOT_CALL_SIG(sotArg1,sotArg1Type),     \
-	    SOT_CALL_SIG(sotArg2,sotArg2Type),     \
-	    SOT_CALL_SIG(sotArg3,sotArg3Type),     \
-	    SOT_CALL_SIG(sotArg4,sotArg4Type),     \
-	    SOT_CALL_SIG(sotArg5,sotArg5Type),     \
-	    SOT_CALL_SIG(sotArg6,sotArg6Type),     \
-	    SOT_CALL_SIG(sotArg7,sotArg7Type),_1), \
-  sotArg1<<sotArg2<<sotArg3<<sotArg4<<sotArg5<<sotArg6<<sotArg7
-
-/* --- MEMBERS MACROS ------------------------------------------------------- */
-/* --- MEMBERS MACROS ------------------------------------------------------- */
-/* --- MEMBERS MACROS ------------------------------------------------------- */
-
-
-#define SOT_MEMBER_SIGNAL_1(sotFunction,             \
-			    sotArg1,sotArg1Type)    \
-boost::bind(&sotFunction,this,                       \
-	    SOT_CALL_SIG(sotArg1,sotArg1Type),_1),   \
-  sotArg1
-
-#define SOT_MEMBER_SIGNAL_2(sotFunction,             \
-			    sotArg1,sotArg1Type,     \
-			    sotArg2,sotArg2Type)     \
-boost::bind(&sotFunction,this,                       \
-	    SOT_CALL_SIG(sotArg1,sotArg1Type),       \
-	    SOT_CALL_SIG(sotArg2,sotArg2Type),_1),   \
-  sotArg1<<sotArg2
-#define SOT_MEMBER_SIGNAL_5(sotFunction,             \
-			    sotArg1,sotArg1Type,     \
-			    sotArg2,sotArg2Type,     \
-			    sotArg3,sotArg3Type,     \
-			    sotArg4,sotArg4Type,     \
-			    sotArg5,sotArg5Type)     \
-boost::bind(&sotFunction,this,                       \
-	    SOT_CALL_SIG(sotArg1,sotArg1Type),       \
-	    SOT_CALL_SIG(sotArg2,sotArg2Type),       \
-	    SOT_CALL_SIG(sotArg3,sotArg3Type),       \
-	    SOT_CALL_SIG(sotArg4,sotArg4Type),       \
-	    SOT_CALL_SIG(sotArg5,sotArg5Type),_1),   \
-  sotArg1<<sotArg2<<sotArg3<<sotArg4<<sotArg5
-
-#define SOT_MEMBER_SIGNAL_6(sotFunction,             \
-			    sotArg1,sotArg1Type,     \
-			    sotArg2,sotArg2Type,     \
-			    sotArg3,sotArg3Type,     \
-			    sotArg4,sotArg4Type,     \
-			    sotArg5,sotArg5Type,     \
-			    sotArg6,sotArg6Type)     \
-boost::bind(&sotFunction,this,                       \
-	    SOT_CALL_SIG(sotArg1,sotArg1Type),       \
-	    SOT_CALL_SIG(sotArg2,sotArg2Type),       \
-	    SOT_CALL_SIG(sotArg3,sotArg3Type),       \
-	    SOT_CALL_SIG(sotArg4,sotArg4Type),       \
-	    SOT_CALL_SIG(sotArg5,sotArg5Type),       \
-	    SOT_CALL_SIG(sotArg6,sotArg6Type),_1),   \
-  sotArg1<<sotArg2<<sotArg3<<sotArg4<<sotArg5<<sotArg6

include/sot-core/mailbox.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      Mailbox.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-
-#ifndef __SOT_MAILBOX_HH
-#define __SOT_MAILBOX_HH
-
-#ifdef  HAVE_LIBBOOST_THREAD
-
-
-/* --- SOT PLUGIN  --- */
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/all-signals.h>
-
-/* --- BOOST --- */
-#include <boost/thread/mutex.hpp>
-#include <boost/thread/thread.hpp>
-#include <boost/thread/xtime.hpp>
-
-/* --- STD --- */
-#include <time.h>
-#ifndef WIN32
-#include <sys/time.h>
-#else
-// When including Winsock2.h, the MAL must be included first
-//#include <MatrixAbstractLayer/boost.h>
-#include <sot/utils-windows.h>
-#include <Winsock2.h>
-#endif /*WIN32*/
-#include <string>
-
-namespace sot {
-
-namespace dg = dynamicgraph;
-
-template< class Object >
-class Mailbox
-: public dg::Entity
-{
- public: 
-  static const std::string CLASS_NAME;
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- public:
-  struct sotTimestampedObject
-  {
-    Object obj;
-    struct timeval timestamp;
-  };
-
-public:
-  
-  Mailbox( const std::string& name );
-  ~Mailbox( void );
-  
-  void post( const Object& obj );
-  sotTimestampedObject& get( sotTimestampedObject& res,const int& dummy );
-
-  Object& getObject( Object& res,const int& time )
-    { 
-      const sotTimestampedObject & data = SOUT(time);
-      res = data.obj; return res;
-    }
-  struct timeval& getTimestamp( struct timeval& res,const int& time )
-    { 
-      const sotTimestampedObject & data = SOUT(time);
-      res.tv_sec = data.timestamp.tv_sec ; 
-      res.tv_usec = data.timestamp.tv_usec ; 
-      return res;
-    }
-
-  bool hasBeenUpdated( void );
-  
-protected:
-
-  boost::timed_mutex mainObjectMutex;
-  Object mainObject;
-  struct timeval mainTimeStamp;
-  bool update;
-
- public: /* --- SIGNALS --- */
-
-  dg::SignalTimeDependent< struct sotTimestampedObject,int > SOUT;
-  dg::SignalTimeDependent< Object,int > objSOUT;
-  dg::SignalTimeDependent< struct timeval,int > timeSOUT;
-
-};
-
-
-#include <sot-core/mailbox.t.cpp>
-
-} // namespace sot
-
-#endif // #ifdef  HAVE_LIBBOOST_THREAD
-#endif // #ifndef  __SOT_MAILBOX_HH
-
-
-
-
-

include/sot-core/mailbox.t.cpp

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      Mailbox.t.cpp
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-
-#ifndef __SOT_MAILBOX_T_CPP
-#define __SOT_MAILBOX_T_CPP
-
-
-
-/* --- SOT PLUGIN  --- */
-#include <sot-core/mailbox.h>
-
-//#undef VP_TEMPLATE_DEBUG
-//#define VP_TEMPLATE_DEBUG_MODE             0
-#include <sot-core/debug.h>
-
-  
-/* -------------------------------------------------------------------------- */
-/* --- CONSTRUCTION --------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-template< class Object >
-Mailbox<Object>::
-Mailbox( const std::string& name )
-  :Entity(name)
-  ,mainObjectMutex()
-  ,mainObject()
-  ,update(false)
-
-   ,SOUT( boost::bind(&Mailbox::get,this,_1,_2),
-	  sotNOSIGNAL,
-	  "Mailbox("+name+")::output(Object)::out" )
-   ,objSOUT( boost::bind(&Mailbox::getObject,this,_1,_2),
-	     SOUT,
-	     "Mailbox("+name+")::output(Object)::object" )
-   ,timeSOUT( boost::bind(&Mailbox::getTimestamp,this,_1,_2),
-	      SOUT,
-	      "Mailbox("+name+")::output(Object)::timestamp" )
-{
-  signalRegistration( SOUT<<objSOUT<<timeSOUT );
-  SOUT.setDependancyType( TimeDependancy<int>::BOOL_DEPENDANT );
-}
-
-template< class Object >
-Mailbox<Object>::
-~Mailbox( void )
-{
-  boost::timed_mutex::scoped_lock lockMain( mainObjectMutex );
-}
-
-/* -------------------------------------------------------------------------- */
-/* --- ACCESS --------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-template< class Object >
-bool Mailbox<Object>::
-hasBeenUpdated( void )
-{
-  boost::timed_mutex::scoped_try_lock lockMain( this->mainObjectMutex );
-  
-  if( lockMain.locked() )
-    {
-      return update;
-    }
-  else 
-    {
-      return false;
-    }
-}
-
-
-/* -------------------------------------------------------------------------- */
-template< class Object >
-typename Mailbox<Object>::sotTimestampedObject& Mailbox<Object>::
-get( typename Mailbox<Object>::sotTimestampedObject& res,const int& dummy )
-{
-  boost::timed_mutex::scoped_try_lock lockMain( this->mainObjectMutex );
-  
-  if( lockMain.locked() )
-    {
-      res.timestamp.tv_sec=this->mainTimeStamp.tv_sec;
-      res.timestamp.tv_usec=this->mainTimeStamp.tv_usec;
-      
-      update = false;
-      res.obj = this->mainObject;
-    }
-
-  return res;
-}
-
-/* -------------------------------------------------------------------------- */
-template< class Object >
-void Mailbox<Object>::
-post( const Object& value )
-{
-  boost::timed_mutex::scoped_lock lockMain( this->mainObjectMutex );
-  mainObject = value;
-  gettimeofday( &this->mainTimeStamp, NULL );
-  update = true;
-  SOUT.setReady();
-
-  return;
-}
-
-
-#endif // #ifdef __SOT_MAILBOX_T_CPP
-