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
-)
-
-# 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/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/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_point6d_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.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/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/matrix-constant.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      matrix-constant.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/all-signals.h>
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-
-
-/* --------------------------------------------------------------------- */
-/* --- MATRIX ---------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-namespace sot {
-namespace dg = dynamicgraph;
-
-class MatrixConstant
-: public dg::Entity
-{
- public: 
-  static const std::string CLASS_NAME;
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
-  int rows,cols;
-  double color;
-
-public:
-  MatrixConstant( const std::string& name )
-    :Entity( name )
-    ,rows(0),cols(0),color(0.)
-    ,SOUT( "sotMatrixConstant("+name+")::output(matrix)::out" )
-    {
-      SOUT.setDependencyType( dg::TimeDependency<int>::BOOL_DEPENDENT );
-      signalRegistration( SOUT );
-    }
-
-  virtual ~MatrixConstant( void ){}
-
-  dg::SignalTimeDependent<ml::Matrix,int> SOUT;
-
-  virtual void commandLine( const std::string& cmdLine,
-			    std::istringstream& cmdArgs, 
-			    std::ostream& os );
-
-};
-    
-} // namespace sot
-
-
-
-
-

include/sot-core/matrix-force.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      matrix-force.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOT_MATRIX_FORCE_H__
-#define __SOT_MATRIX_FORCE_H__
-
-
-/* --- Matrix --- */
-#include <MatrixAbstractLayer/boost.h>
-#include <sot-core/sot-core-api.h>
-namespace ml = maal::boost;
-
-
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-namespace sot {
-
-
-class MatrixHomogeneous;
-class MatrixTwist;
-
-
-class SOT_CORE_EXPORT MatrixForce
-: public ml::Matrix
-{
-
- public: 
-
-  MatrixForce( void ) : ml::Matrix(6,6) { setIdentity(); }
-  ~MatrixForce( void ) { }
-  explicit MatrixForce( const MatrixHomogeneous& M ) 
-    : ml::Matrix(6,6) 
-    { buildFrom(M); }
-
-  MatrixForce& buildFrom( const MatrixHomogeneous& trans );
-
-  MatrixForce& operator=( const ml::Matrix& );
-  MatrixForce&
-    inverse( MatrixForce& invMatrix ) const ;
-  inline MatrixForce inverse( void )  const 
-    { MatrixForce Ainv; return inverse(Ainv); }
-
-  MatrixTwist& transpose( MatrixTwist& Vt ) const;
-  MatrixTwist transpose( void ) const;
-
- };
-
-} // namespace sot
-    
-
-#endif /* #ifndef __SOT_MATRIX_FORCE_H__ */
-
-
-
-
-
-

include/sot-core/matrix-homogeneous.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      matrix-homogeneous.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOT_MATRIX_HOMOGENEOUS_H__
-#define __SOT_MATRIX_HOMOGENEOUS_H__
-
-
-/* --- Matrix --- */
-#include <MatrixAbstractLayer/boost.h>
-#include <sot-core/sot-core-api.h>
-namespace ml = maal::boost;
-
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-namespace sot {
-
-class MatrixRotation;
-
-class SOT_CORE_EXPORT MatrixHomogeneous
-: public ml::Matrix
-{
-
- public: 
-
-  MatrixHomogeneous( void ) : ml::Matrix(4,4) { setIdentity(); }
-  ~MatrixHomogeneous( void ) { }
-
-  MatrixHomogeneous& buildFrom( const MatrixRotation& rot, const ml::Vector& trans );
-  MatrixRotation& extract( MatrixRotation& rot ) const;
-  ml::Vector& extract( ml::Vector& trans ) const;
-
-  MatrixHomogeneous& operator=( const ml::Matrix& );
-
-  MatrixHomogeneous&
-    inverse( MatrixHomogeneous& invMatrix ) const ;
-  inline MatrixHomogeneous inverse( void )  const 
-    { MatrixHomogeneous Ainv; return inverse(Ainv); }
-
-  ml::Vector& multiply( const ml::Vector& v1,ml::Vector& res ) const;
-  inline ml::Vector multiply( const ml::Vector& v1 )  const
-    { ml::Vector res; return multiply(v1,res); }
-  
-  using  ml::Matrix::multiply;
-   
-
- 
-
-
- };
-    
-} // namespace sot
-
-
-#endif /* #ifndef __SOT_MATRIX_HOMOGENEOUS_H__ */
-
-
-
-
-
-

include/sot-core/matrix-rotation.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      matrix-rotation.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOT_MATRIX_ROTATION_H__
-#define __SOT_MATRIX_ROTATION_H__
-
-
-/* --- Matrix --- */
-
-#include <sot-core/sot-core-api.h>
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-namespace sot {
-class VectorUTheta;
-
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-class SOT_CORE_EXPORT MatrixRotation
-: public ml::Matrix
-{
-
- public: 
-
-  MatrixRotation( void ) : ml::Matrix(3,3) { setIdentity(); }
-  ~MatrixRotation( void ) { }
-
-  void fromVector( VectorUTheta& );
-  MatrixRotation& operator= ( VectorUTheta&th ) { fromVector(th); return *this; } 
-};
-    
-} // namespace sot
-#endif /* #ifndef __SOT_MATRIX_ROTATION_H__ */
-
-
-
-
-
-

include/sot-core/matrix-twist.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      matrix-twist.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOT_MATRIX_TWIST_H__
-#define __SOT_MATRIX_TWIST_H__
-
-
-/* --- Matrix --- */
-#include <MatrixAbstractLayer/boost.h>
-#include <sot-core/sot-core-api.h>
-
-namespace ml = maal::boost;
-
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-namespace sot {
-
-
-class MatrixHomogeneous;
-class MatrixForce;
-
-class SOT_CORE_EXPORT MatrixTwist
-: public ml::Matrix
-{
-
- public: 
-
-  MatrixTwist( void ) : ml::Matrix(6,6) { setIdentity(); }
-  ~MatrixTwist( void ) { }
-  explicit MatrixTwist( const MatrixHomogeneous& M ) 
-    : ml::Matrix(6,6) 
-    { buildFrom(M); }
-
-  MatrixTwist& buildFrom( const MatrixHomogeneous& trans );
-
-  MatrixTwist& operator=( const ml::Matrix& );
-  MatrixTwist&
-    inverse( MatrixTwist& invMatrix ) const ;
-  inline MatrixTwist inverse( void )  const 
-    { MatrixTwist Ainv; return inverse(Ainv); }
-
-  MatrixForce& transpose( MatrixForce& Vt ) const;
-  MatrixForce transpose( void ) const;
- };
-
-} // namespace sot
-
-#endif /* #ifndef __SOT_MATRIX_TWIST_H__ */
-
-
-
-
-
-

include/sot-core/memory-task-sot.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet Gepetto, LAAS, CNRS, 2009
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      memory-task-sot.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-#ifndef __SOT_MEMORY_TASK_HH
-#define __SOT_MEMORY_TASK_HH
-
-
-#include <sot-core/task-abstract.h>
-#include <sot-core/sot-core-api.h>
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-namespace sot {
-namespace dg = dynamicgraph;
-
-class SOT_CORE_EXPORT MemoryTaskSOT
-: public TaskAbstract::MemoryTaskAbstract, public dg::Entity
-{
- public://   protected:
-  /* Internal memory to reduce the dynamic allocation at task resolution. */
-  ml::MatrixSvd Jt;  //( nJ,mJ );
-  ml::Matrix Jp;
-  ml::Matrix PJp;
-
-  ml::Matrix Jff; //( nJ,FF_SIZE ); // Free-floating part
-  ml::Matrix Jact; //( nJ,mJ );     // Activated part
-  ml::Matrix JK; //(nJ,mJ);
-
-  ml::Matrix U,V;
-  ml::Vector S;
-
- public:
-  /* mJ is the number of actuated joints, nJ the number of feature in the task,
-   * and ffsize the number of unactuated DOF. */
-  MemoryTaskSOT( const std::string & name,const unsigned int nJ=0,
-                    const unsigned int mJ=0,const unsigned int ffsize =0 );
-
-  virtual void initMemory( const unsigned int nJ,
-                           const unsigned int mJ,
-                           const unsigned int ffsize );
-
- public: /* --- ENTITY INHERITANCE --- */
-  static const std::string CLASS_NAME;
-  virtual void display( std::ostream& os ) const;
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- public: /* --- SIGNALS --- */
-  dg::Signal< ml::Matrix,int > jacobianInvSINOUT;
-  dg::Signal< ml::Matrix,int > jacobianConstrainedSINOUT;
-  dg::Signal< ml::Matrix,int > jacobianProjectedSINOUT;
-  dg::Signal< ml::Matrix,int > singularBaseImageSINOUT;
-  dg::Signal< unsigned int,int > rankSINOUT;
-
- public: /* --- PARAMS --- */
-  virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
-			    std::ostream& os );
-};
-
-} //namespace sot
-
-#endif // __SOT_MEMORY_TASK_HH

include/sot-core/op-point-modifier.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      op-point-modifier.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOT_OP_POINT_MODIFIOR_H__
-#define __SOT_OP_POINT_MODIFIOR_H__
-
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/all-signals.h>
-#include <sot-core/debug.h>
-#include <sot-core/matrix-homogeneous.h>
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32) 
-#  if defined (op_point_modifier_EXPORTS)
-#    define SOTOPPOINTMODIFIER_EXPORT __declspec(dllexport)
-#  else  
-#    define SOTOPPOINTMODIFIER_EXPORT __declspec(dllimport)
-#  endif 
-#else
-#  define SOTOPPOINTMODIFIER_EXPORT
-#endif
-
-/* --------------------------------------------------------------------- */
-/* --- VECTOR ---------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-namespace sot {
-namespace dg = dynamicgraph;
-
-class SOTOPPOINTMODIFIER_EXPORT OpPointModifier
-: public dg::Entity
-{
- public:
-  static const std::string CLASS_NAME;
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-  
-  MatrixHomogeneous transformation;
-
- public:
-
-  dg::SignalPtr<ml::Matrix,int> jacobianSIN;
-  dg::SignalPtr<MatrixHomogeneous,int> positionSIN;
-  
-  dg::SignalTimeDependent<ml::Matrix,int> jacobianSOUT;
-  dg::SignalTimeDependent<MatrixHomogeneous,int> positionSOUT;
-
-public:
-  OpPointModifier( const std::string& name );
-
-  virtual ~OpPointModifier( void ){}
-
-  ml::Matrix& computeJacobian( ml::Matrix& res,const int& time );
-  MatrixHomogeneous& computePosition( MatrixHomogeneous& res,const int& time );
-  void setTransformation( const MatrixHomogeneous& tr );
-
-  virtual void commandLine( const std::string& cmdLine,
-			    std::istringstream& cmdArgs, 
-			    std::ostream& os );
-
-};
-
-} // namespace sot
-
-#endif //  __SOT_OP_POINT_MODIFIOR_H__