src/tools/trajectory.cpp

+/*
+ * Copyright 2013,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+#define VP_DEBUG
+#define VP_DEBUG_MODE 45
+#include <iostream>
+#include <sot/core/debug.hh>
+// #ifdef VP_DEBUG
+//  class sotJTE__INIT
+//  {
+//  public:sotJTE__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
+//  };
+//  sotJTE__INIT sotJTE_initiator;
+// #endif //#ifdef VP_DEBUG
+
+#include <sot/core/trajectory.hh>
+
+/************************/
+/* JointTrajectoryPoint */
+/************************/
+
+/**************/
+/* Trajectory */
+/**************/
+namespace dynamicgraph {
+namespace sot {
+
+RulesJointTrajectory::RulesJointTrajectory(Trajectory &aTrajectoryToFill)
+    : TrajectoryToFill_(aTrajectoryToFill),
+      dbg_level(0),
+      float_str_re("[-0-9]+\\.[0-9]*"),
+
+      // Header Regular Expression
+      seq_str_re("([0-9]+)"),
+      timestamp_str_re("(" + float_str_re + "),(" + float_str_re + ")"),
+      frame_id_str_re("[a-zA-z_0-9]*"),
+      header_str_re("\\(" + seq_str_re + "\\,\\(" + timestamp_str_re + "\\),(" +
+                    frame_id_str_re + ")\\)\\,\\("),
+
+      // List of Joint Names
+      joint_name_str_re("([a-zA-Z0-9_]+)"),
+      list_of_jn_str_re(joint_name_str_re + "(\\,|\\))"),
+
+      // Point
+      point_value_str_re("(" + float_str_re + "+)|(?:)"),
+      list_of_pv_str_re(point_value_str_re + "(\\,|\\))"),
+      bg_pt_str_re("\\("),
+      end_pt_str_re("\\)"),
+      comma_pt_str_re("\\,\\("),
+
+      // Liste of points
+      bg_liste_of_pts_str_re("\\,\\("),
+
+      // Reg Exps
+      header_re(header_str_re),
+      list_of_jn_re(list_of_jn_str_re),
+      list_of_pv_re(list_of_pv_str_re),
+      bg_pt_re(bg_pt_str_re),
+      end_pt_re(end_pt_str_re),
+      comma_pt_re(comma_pt_str_re),
+      bg_liste_of_pts_re(bg_liste_of_pts_str_re) {}
+
+bool RulesJointTrajectory::search_exp_sub_string(
+    std::string &text, boost::match_results<std::string::const_iterator> &what,
+    boost::regex &e, std::string &sub_text) {
+  unsigned nb_failures = 0;
+
+  boost::match_flag_type flags = boost::match_extra;
+  if (boost::regex_search(text, what, e, flags)) {
+    if (dbg_level > 5) {
+      std::cout << "** Match found **\n   Sub-Expressions:" << what.size()
+                << std::endl;
+      for (unsigned int i = 0; i < what.size(); ++i)
+        std::cout << "      $" << i << " = \"" << what[i] << "\" "
+                  << what.position(i) << " " << what.length(i) << "\n";
+    }
+    if (what.size() >= 1) {
+      unsigned int all_text = 0;
+      boost::match_results<std::string::const_iterator>::difference_type pos =
+          what.position(all_text);
+      boost::match_results<std::string::const_iterator>::difference_type len =
+          what.length(all_text);
+      sub_text = text.substr(pos + len);
+      return true;
+    }
+  } else {
+    if (dbg_level > 5) std::cout << "** No Match found **\n";
+    sub_text = text;
+    nb_failures++;
+    if (nb_failures > 100) return false;
+  }
+  return false;
+}
+
+void RulesJointTrajectory::parse_header(std::string &trajectory,
+                                        std::string &sub_text1) {
+  std::istringstream is;
+  boost::match_results<std::string::const_iterator> what;
+
+  if (search_exp_sub_string(trajectory, what, header_re, sub_text1)) {
+    is.str(what[1]);
+    is >> TrajectoryToFill_.header_.seq_;
+    is.str(what[2]);
+    is >> TrajectoryToFill_.header_.stamp_.secs_;
+    is.str(what[3]);
+    is >> TrajectoryToFill_.header_.stamp_.nsecs_;
+    TrajectoryToFill_.header_.frame_id_ = what[4];
+
+    if (dbg_level > 5) {
+      std::cout << "seq: " << TrajectoryToFill_.header_.seq_ << std::endl;
+      std::cout << "ts:" << TrajectoryToFill_.header_.stamp_.secs_ << " "
+                << what[2] << " " << TrajectoryToFill_.header_.stamp_.nsecs_
+                << " " << is.str() << std::endl;
+      std::cout << "frame_id:" << TrajectoryToFill_.header_.frame_id_
+                << std::endl;
+
+      std::cout << "sub_text1:" << sub_text1 << std::endl;
+    }
+  }
+}
+
+void RulesJointTrajectory::parse_joint_names(
+    std::string &trajectory, std::string &sub_text1,
+    std::vector<std::string> &joint_names) {
+  std::istringstream is;
+  boost::match_results<std::string::const_iterator> what;
+  bool joint_names_loop = true;
+  do {
+    if (search_exp_sub_string(trajectory, what, list_of_jn_re, sub_text1)) {
+      std::string joint_name;
+      is.str(what[1]);
+      joint_name = what[1];
+      joint_names.push_back(joint_name);
+
+      std::string sep_char;
+      sep_char = what[2];
+
+      if (sep_char == ")") joint_names_loop = false;
+      if (dbg_level > 5) {
+        std::cout << "joint_name:" << joint_name << " " << sep_char
+                  << std::endl;
+        std::cout << "sub_text1:" << sub_text1 << std::endl;
+      }
+    }
+    trajectory = sub_text1;
+
+  } while (joint_names_loop);
+}
+
+bool RulesJointTrajectory::parse_seq(std::string &trajectory,
+                                     std::string &sub_text1,
+                                     std::vector<double> &seq) {
+  boost::match_results<std::string::const_iterator> what;
+  bool joint_seq_loop = true;
+  std::istringstream is;
+  std::string sub_text2 = trajectory;
+  sub_text1 = trajectory;
+  do {
+    if (search_exp_sub_string(sub_text2, what, list_of_pv_re, sub_text1)) {
+      std::string sep_char;
+      if (dbg_level > 5) {
+        std::cout << "size:" << what.size() << std::endl;
+      }
+
+      if (what.size() == 3) {
+        std::string aString(what[1]);
+        if (aString.size() > 0) {
+          is.clear();
+          is.str(aString);
+          double aValue;
+          is >> aValue;
+          if (dbg_level > 5) {
+            std::cout << aString << " | " << aValue << std::endl;
+          }
+
+          seq.push_back(aValue);
+        }
+        sep_char = what[2];
+      } else if (what.size() == 1)
+        sep_char = what[0];
+
+      if (sep_char == ")") joint_seq_loop = false;
+
+    } else {
+      return true;
+    }
+    sub_text2 = sub_text1;
+  } while (joint_seq_loop);
+  return true;
+}
+
+bool RulesJointTrajectory::parse_point(std::string &trajectory,
+                                       std::string &sub_text1) {
+  std::vector<double> position, velocities, acceleration, effort;
+  std::string sub_text2;
+  boost::match_results<std::string::const_iterator> what;
+  JointTrajectoryPoint aJTP;
+
+  if (!search_exp_sub_string(trajectory, what, bg_pt_re, sub_text1))
+    return false;
+  sub_text2 = sub_text1;
+
+  if (!parse_seq(sub_text2, sub_text1, aJTP.positions_)) return false;
+  sub_text2 = sub_text1;
+
+  if (!search_exp_sub_string(sub_text2, what, comma_pt_re, sub_text1))
+    return false;
+  sub_text2 = sub_text1;
+
+  if (!parse_seq(sub_text2, sub_text1, aJTP.velocities_)) return false;
+  sub_text2 = sub_text1;
+
+  if (!search_exp_sub_string(sub_text2, what, comma_pt_re, sub_text1))
+    return false;
+  sub_text2 = sub_text1;
+
+  if (!parse_seq(sub_text2, sub_text1, aJTP.accelerations_)) return false;
+  sub_text2 = sub_text1;
+
+  if (!search_exp_sub_string(sub_text2, what, comma_pt_re, sub_text1))
+    return false;
+  sub_text2 = sub_text1;
+  if (!parse_seq(sub_text2, sub_text1, aJTP.efforts_)) return false;
+
+  TrajectoryToFill_.points_.push_back(aJTP);
+  return true;
+}
+
+bool RulesJointTrajectory::parse_points(std::string &trajectory,
+                                        std::string &sub_text1) {
+  boost::match_results<std::string::const_iterator> what;
+  bool joint_points_loop = true;
+  std::istringstream is;
+
+  if (!search_exp_sub_string(trajectory, what, bg_liste_of_pts_re, sub_text1))
+    return false;
+  std::string sub_text2 = sub_text1;
+
+  do {
+    if (!search_exp_sub_string(sub_text2, what, bg_pt_re, sub_text1))
+      return false;
+    sub_text2 = sub_text1;
+
+    if (!parse_point(sub_text2, sub_text1)) return false;
+    sub_text2 = sub_text1;
+
+    if (!search_exp_sub_string(sub_text2, what, end_pt_re, sub_text1))
+      return false;
+    sub_text2 = sub_text1;
+
+    if (!search_exp_sub_string(sub_text2, what, list_of_pv_re, sub_text1))
+      return false;
+    sub_text2 = sub_text1;
+    std::string sep_char;
+    sep_char = what[1];
+
+    if (sep_char == ")") joint_points_loop = false;
+
+  } while (joint_points_loop);
+
+  return true;
+}
+
+void RulesJointTrajectory::parse_string(std::string &atext) {
+  std::string sub_text1, sub_text2;
+  parse_header(atext, sub_text2);
+  sub_text1 = sub_text2;
+
+  parse_joint_names(sub_text1, sub_text2, TrajectoryToFill_.joint_names_);
+
+  if (dbg_level > 5) {
+    for (std::vector<std::string>::size_type i = 0; i < joint_names.size(); i++)
+      std::cout << joint_names[i] << std::endl;
+  }
+
+  sub_text1 = sub_text2;
+  parse_points(sub_text1, sub_text2);
+}
+
+Trajectory::Trajectory(void) {}
+
+Trajectory::Trajectory(const Trajectory &copy) {
+  header_ = copy.header_;
+  time_from_start_ = copy.time_from_start_;
+  points_ = copy.points_;
+}
+
+Trajectory::~Trajectory(void) {}
+
+int Trajectory::deserialize(std::istringstream &is) {
+  std::string aStr = is.str();
+  RulesJointTrajectory aRJT(*this);
+  aRJT.parse_string(aStr);
+
+  return 0;
+}
+
+void Trajectory::display(std::ostream &os) const {
+  unsigned int index = 0;
+  os << "-- Trajectory --" << std::endl;
+  for (std::vector<std::string>::const_iterator it_joint_name =
+           joint_names_.begin();
+       it_joint_name != joint_names_.end(); it_joint_name++, index++)
+    os << "Joint(" << index << ")=" << *(it_joint_name) << std::endl;
+
+  os << " Number of points: " << points_.size() << std::endl;
+  for (std::vector<JointTrajectoryPoint>::const_iterator it_point =
+           points_.begin();
+       it_point != points_.end(); it_point++) {
+    it_point->display(os);
+  }
+}
+
+}  // namespace sot
+}  // namespace dynamicgraph

src/tools/type-name-helper.hh

+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
+ * Nicolas Mansard
+ * Joseph Mirabel
+ *
+ * CNRS/AIST
+ *
+ */
+
+#include <sot/core/matrix-geometry.hh>
+
+namespace dynamicgraph {
+namespace sot {
+template <typename TypeRef>
+struct TypeNameHelper {
+  static inline std::string typeName();
+};
+template <typename TypeRef>
+inline std::string TypeNameHelper<TypeRef>::typeName() {
+  return "unspecified";
+}
+
+#define ADD_KNOWN_TYPE(typeid)                            \
+  template <>                                             \
+  inline std::string TypeNameHelper<typeid>::typeName() { \
+    return #typeid;                                       \
+  }
+
+ADD_KNOWN_TYPE(bool)
+ADD_KNOWN_TYPE(double)
+ADD_KNOWN_TYPE(Vector)
+ADD_KNOWN_TYPE(Matrix)
+ADD_KNOWN_TYPE(MatrixRotation)
+ADD_KNOWN_TYPE(MatrixTwist)
+ADD_KNOWN_TYPE(MatrixHomogeneous)
+ADD_KNOWN_TYPE(VectorQuaternion)
+ADD_KNOWN_TYPE(VectorRollPitchYaw)
+
+#undef ADD_KNOWN_TYPE
+} /* namespace sot */
+} /* namespace dynamicgraph */

src/tools/utils-windows.cpp

-#ifdef WIN32
-#include <sot-core/utils-windows.h>
+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
 
+#ifdef WIN32
 #include < Windows.h >
+#include <sot/core/utils-windows.hh>
 #if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
-#define DELTA_EPOCH_IN_MICROSECS  11644473600000000Ui64
+#define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64
 #else
-#define DELTA_EPOCH_IN_MICROSECS  11644473600000000ULL
+#define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL
 #endif
 
-int gettimeofday(struct timeval *tv, struct timezone *tz)
-{
-	FILETIME ft;
-	unsigned __int64 tmpres = 0;
-	static int tzflag;
-
-	if (NULL != tv)
-	{
-		GetSystemTimeAsFileTime(&ft);
-
-		tmpres |= ft.dwHighDateTime;
-		tmpres <<= 32;
-		tmpres |= ft.dwLowDateTime;
-
-		/*converting file time to unix epoch*/
-		tmpres /= 10;  /*convert into microseconds*/
-		tmpres -= DELTA_EPOCH_IN_MICROSECS; 
-		tv->tv_sec = (long)(tmpres / 1000000UL);
-		tv->tv_usec = (long)(tmpres % 1000000UL);
-	}
-
-	if (NULL != tz)
-	{
-		if (!tzflag)
-		{
-			_tzset();
-			tzflag++;
-		}
-		tz->tz_minuteswest = _timezone / 60;
-		tz->tz_dsttime = _daylight;
-	}
-
-	return 0;
+int gettimeofday(struct timeval *tv, struct timezone *tz) {
+  FILETIME ft;
+  unsigned __int64 tmpres = 0;
+  static int tzflag;
+
+  if (NULL != tv) {
+    GetSystemTimeAsFileTime(&ft);
+
+    tmpres |= ft.dwHighDateTime;
+    tmpres <<= 32;
+    tmpres |= ft.dwLowDateTime;
+
+    /*converting file time to unix epoch*/
+    tmpres /= 10; /*convert into microseconds*/
+    tmpres -= DELTA_EPOCH_IN_MICROSECS;
+    tv->tv_sec = (long)(tmpres / 1000000UL);
+    tv->tv_usec = (long)(tmpres % 1000000UL);
+  }
+
+  if (NULL != tz) {
+    if (!tzflag) {
+      _tzset();
+      tzflag++;
+    }
+    tz->tz_minuteswest = _timezone / 60;
+    tz->tz_dsttime = _daylight;
+  }
+
+  return 0;
 }
 
 #endif /*WIN32*/

src/traces/reader-python.h

+#include <sot/core/reader.hh>
+
+typedef boost::mpl::vector<sotReader> entities_t;

src/traces/reader.cpp

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
  *
- * File:      sotReader.cpp
- * Project:   SOT
- * Author:    Nicolas Mansard
+ * CNRS/AIST
  *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
+ */
 
 /* --------------------------------------------------------------------- */
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
 /* SOT */
-#include <sot-core/reader.h>
-#include <sot-core/debug.h>
+#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/factory.h>
 
 #include <boost/bind.hpp>
+#include <sot/core/debug.hh>
+#include <sot/core/reader.hh>
 #include <sstream>
-#include <dynamic-graph/factory.h>
 
 using namespace dynamicgraph;
-using namespace sot;
+using namespace dynamicgraph::sot;
 using namespace std;
 
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(sotReader,"Reader");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(sotReader, "Reader");
 
 /* --------------------------------------------------------------------- */
 /* --- CLASS ----------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-sotReader::sotReader( const std::string n )
-  :Entity(n)
-  ,selectionSIN( NULL,"Reader("+n+")::input(flag)::selec" )
-   ,vectorSOUT( boost::bind(&sotReader::getNextData,this,_1,_2),
-	      sotNOSIGNAL,
-	      "Reader("+n+")::vector" )
-   ,matrixSOUT( boost::bind(&sotReader::getNextMatrix,this,_1,_2),
-		vectorSOUT,
-		"Reader("+n+")::matrix" )
-   ,dataSet()
-   ,currentData()
-   ,iteratorSet(false)
-  ,nbRows(0),nbCols(0)
-{
-  signalRegistration( selectionSIN<<vectorSOUT<<matrixSOUT );
+sotReader::sotReader(const std::string n)
+    : Entity(n),
+      selectionSIN(NULL, "Reader(" + n + ")::input(flag)::selec"),
+      vectorSOUT(boost::bind(&sotReader::getNextData, this, _1, _2),
+                 sotNOSIGNAL, "Reader(" + n + ")::vector"),
+      matrixSOUT(boost::bind(&sotReader::getNextMatrix, this, _1, _2),
+                 vectorSOUT, "Reader(" + n + ")::matrix"),
+      dataSet(),
+      currentData(),
+      iteratorSet(false),
+      rows(0),
+      cols(0) {
+  signalRegistration(selectionSIN << vectorSOUT << matrixSOUT);
   selectionSIN = true;
   vectorSOUT.setNeedUpdateFromAllChildren(true);
-}  
+
+  initCommands();
+}
 
 /* --------------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-
-void sotReader::
-load( const string& filename )
-{
+void sotReader::load(const string &filename) {
   sotDEBUGIN(15);
 
-  std::ifstream datafile( filename.c_str() );
-  const unsigned int SIZE=1024;
-  char buffer[SIZE]; 
+  std::ifstream datafile(filename.c_str());
+  const unsigned int SIZE = 1024;
+  char buffer[SIZE];
   std::vector<double> newline;
-  while( datafile.good() )
-    {
-      datafile.getline( buffer,SIZE );
-      const unsigned int gcount = datafile.gcount(); 
-      if( gcount>=SIZE ) { /* TODO read error, line to long. */ }
-      std::istringstream iss(buffer);
-      newline.clear();
-      sotDEBUG(25) << "Get line = '" << buffer << "'" << std::endl;
-      while( 1 ) 
-	{
-	  double x; iss>>x;
-	  if(! iss.fail() )newline.push_back(x); else break;
-	  sotDEBUG(45) << "New data = " << x  << std::endl;
-	}
-      if( newline.size()>0 )dataSet.push_back( newline );
+  while (datafile.good()) {
+    datafile.getline(buffer, SIZE);
+    const unsigned int gcount = (unsigned int)(datafile.gcount());
+    if (gcount >= SIZE) { /* TODO read error, line to long. */
     }
+    std::istringstream iss(buffer);
+    newline.clear();
+    sotDEBUG(25) << "Get line = '" << buffer << "'" << std::endl;
+    while (1) {
+      double x;
+      iss >> x;
+      if (!iss.fail())
+        newline.push_back(x);
+      else
+        break;
+      sotDEBUG(45) << "New data = " << x << std::endl;
+    }
+    if (newline.size() > 0) dataSet.push_back(newline);
+  }
 
   sotDEBUGOUT(15);
 }
 
-void sotReader::clear( void )
-{
+void sotReader::clear(void) {
   sotDEBUGIN(15);
 
   dataSet.clear();
@@ -103,58 +92,54 @@ void sotReader::clear( void )
   sotDEBUGOUT(15);
 }
 
-void sotReader::
-rewind( void )
-{
+void sotReader::rewind(void) {
   sotDEBUGIN(15);
   iteratorSet = false;
   sotDEBUGOUT(15);
 }
 
-ml::Vector& sotReader::
-getNextData( ml::Vector& res, const unsigned int time )
-{
+dynamicgraph::Vector &sotReader::getNextData(dynamicgraph::Vector &res,
+                                             const unsigned int time) {
   sotDEBUGIN(15);
 
-  if(! iteratorSet ) 
-    {
-      sotDEBUG(15) << "Start the list" << std::endl; 
-      currentData = dataSet.begin(); iteratorSet=true;
-    }
-  else if( currentData!=dataSet.end() ){ ++currentData;  }
+  if (!iteratorSet) {
+    sotDEBUG(15) << "Start the list" << std::endl;
+    currentData = dataSet.begin();
+    iteratorSet = true;
+  } else if (currentData != dataSet.end()) {
+    ++currentData;
+  }
 
-  if( currentData==dataSet.end() )
-    {
-      sotDEBUGOUT(15);
-      return res;
-    }
-   
-  const Flags& selection = selectionSIN(time);
-  const std::vector<double> & curr =  *currentData;
+  if (currentData == dataSet.end()) {
+    sotDEBUGOUT(15);
+    return res;
+  }
 
-  unsigned int dim=0;
-  for( unsigned int i=0;i<curr.size();++i ) if( selection(i) ) dim++;
+  const Flags &selection = selectionSIN(time);
+  const std::vector<double> &curr = *currentData;
+
+  unsigned int dim = 0;
+  for (unsigned int i = 0; i < curr.size(); ++i)
+    if (selection(i)) dim++;
 
   res.resize(dim);
-  unsigned int cursor=0;
-  for( unsigned int i=0;i<curr.size();++i ) 
-    if( selection(i) ) res(cursor++)=curr[i];
-  
+  int cursor = 0;
+  for (unsigned int i = 0; i < curr.size(); ++i)
+    if (selection(i)) res(cursor++) = curr[i];
+
   sotDEBUGOUT(15);
   return res;
 }
 
-ml::Matrix& sotReader::
-getNextMatrix( ml::Matrix& res, const unsigned int time )
-{
+dynamicgraph::Matrix &sotReader::getNextMatrix(dynamicgraph::Matrix &res,
+                                               const unsigned int time) {
   sotDEBUGIN(15);
-  const ml::Vector& vect = vectorSOUT(time);
-  if( vect.size()<nbRows*nbCols ) return res;
+  const dynamicgraph::Vector &vect = vectorSOUT(time);
+  if (vect.size() < rows * cols) return res;
 
-  res.resize( nbRows,nbCols );
-  for( unsigned int i=0;i<nbRows;++i )
-    for( unsigned int j=0;j<nbCols;++j )
-      res(i,j)=vect(i*nbCols+j);
+  res.resize(rows, cols);
+  for (int i = 0; i < rows; ++i)
+    for (int j = 0; j < cols; ++j) res(i, j) = vect(i * cols + j);
 
   sotDEBUGOUT(15);
   return res;
@@ -163,56 +148,31 @@ getNextMatrix( ml::Matrix& res, const unsigned int time )
 /* --------------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-
-void sotReader::
-display( std::ostream& os ) const
-{
+void sotReader::display(std::ostream &os) const {
   os << CLASS_NAME << " " << name << endl;
 }
 
-
-std::ostream& operator<< ( std::ostream& os,const sotReader& t )
-{
+std::ostream &operator<<(std::ostream &os, const sotReader &t) {
   t.display(os);
   return os;
 }
 
-void sotReader::
-commandLine( const std::string& cmdLine
-	     ,std::istringstream& cmdArgs
-	     ,std::ostream& os )
-{
-  if( cmdLine=="help" )
-    {
-      os << "Reader: "<<endl
-	 << "  - load" << endl;
-	
-      Entity::commandLine( cmdLine,cmdArgs,os );
-    }
-  else if( cmdLine=="load" )
-    {
-      string filename;
-      cmdArgs>>ws>>filename;
-      load(filename);
-    }
-  else if( cmdLine=="rewind" )
-    {
-      cmdArgs>>ws;
-      rewind( );
-    }
-  else if( cmdLine=="clear" )
-    {
-      cmdArgs>>ws;
-      clear( );
-    }
-  else if( cmdLine=="resize" )
-    {
-      cmdArgs >>ws;
-      if( cmdArgs.good() ) { cmdArgs >> nbRows >>nbCols; }
-      else {  os << "Matrix size = " << nbRows << " x " <<nbCols <<std::endl; }
-    }
-  else { Entity::commandLine( cmdLine,cmdArgs,os ); }
-
-
+/* --- Command line interface
+ * ------------------------------------------------------ */
+void sotReader::initCommands() {
+  namespace dc = ::dynamicgraph::command;
+  addCommand("clear", dc::makeCommandVoid0(*this, &sotReader::clear,
+                                           "Clear the data loaded"));
+  addCommand("rewind",
+             dc::makeCommandVoid0(
+                 *this, &sotReader::rewind,
+                 "Reset the iterator to the beginning of the data set"));
+  addCommand("load",
+             dc::makeCommandVoid1(*this, &sotReader::load, "load file"));
+  addCommand("resize", dc::makeCommandVoid2(*this, &sotReader::resize, " "));
+}
 
+void sotReader::resize(const int &row, const int &col) {
+  rows = row;
+  cols = col;
 }

src/utils/stop-watch.cpp

+/*
+Copyright (c) 2010-2013 Tommaso Urli
+
+Tommaso Urli    tommaso.urli@uniud.it   University of Udine
+
+Permission is hereby granted, free of charge, to any person obtaining
+a copy of this software and associated documentation files (the
+"Software"), to deal in the Software without restriction, including
+without limitation the rights to use, copy, modify, merge, publish,
+distribute, sublicense, and/or sell copies of the Software, and to
+permit persons to whom the Software is furnished to do so, subject to
+the following conditions:
+
+The above copyright notice and this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+*/
+
+#ifndef WIN32
+#include <sys/time.h>
+#else
+#include <Windows.h>
+
+#include <iomanip>
+#endif
+
+#include <iomanip>  // std::setprecision
+
+#include "sot/core/stop-watch.hh"
+
+using std::map;
+using std::ostringstream;
+using std::string;
+
+//#define START_PROFILER(name) getProfiler().start(name)
+//#define STOP_PROFILER(name) getProfiler().stop(name)
+
+Stopwatch &getProfiler() {
+  static Stopwatch s(REAL_TIME);  // alternatives are CPU_TIME and REAL_TIME
+  return s;
+}
+
+Stopwatch::Stopwatch(StopwatchMode _mode) : active(true), mode(_mode) {
+  records_of = new map<string, PerformanceData>();
+}
+
+Stopwatch::~Stopwatch() { delete records_of; }
+
+void Stopwatch::set_mode(StopwatchMode new_mode) { mode = new_mode; }
+
+bool Stopwatch::performance_exists(string perf_name) {
+  return (records_of->find(perf_name) != records_of->end());
+}
+
+long double Stopwatch::take_time() {
+  if (mode == CPU_TIME) {
+    // Use ctime
+    return clock();
+
+  } else if (mode == REAL_TIME) {
+    // Query operating system
+
+#ifdef WIN32
+    /* In case of usage under Windows */
+    FILETIME ft;
+    LARGE_INTEGER intervals;
+
+    // Get the amount of 100 nanoseconds intervals elapsed since January 1, 1601
+    // (UTC)
+    GetSystemTimeAsFileTime(&ft);
+    intervals.LowPart = ft.dwLowDateTime;
+    intervals.HighPart = ft.dwHighDateTime;
+
+    long double measure = intervals.QuadPart;
+    measure -= 116444736000000000.0;  // Convert to UNIX epoch time
+    measure /= 10000000.0;            // Convert to seconds
+
+    return measure;
+#else
+    /* Linux, MacOS, ... */
+    struct timeval tv;
+    gettimeofday(&tv, NULL);
+
+    long double measure = tv.tv_usec;
+    measure /= 1000000.0;  // Convert to seconds
+    measure += tv.tv_sec;  // Add seconds part
+
+    return measure;
+#endif
+
+  } else {
+    // If mode == NONE, clock has not been initialized, then throw exception
+    throw StopwatchException("Clock not initialized to a time taking mode!");
+  }
+}
+
+void Stopwatch::start(string perf_name) {
+  if (!active) return;
+
+  // Just works if not already present
+  records_of->insert(make_pair(perf_name, PerformanceData()));
+
+  PerformanceData &perf_info = records_of->find(perf_name)->second;
+
+  // Take ctime
+  perf_info.clock_start = take_time();
+
+  // If this is a new start (i.e. not a restart)
+  //  if (!perf_info.paused)
+  //    perf_info.last_time = 0;
+
+  perf_info.paused = false;
+}
+
+void Stopwatch::stop(string perf_name) {
+  if (!active) return;
+
+  long double clock_end = take_time();
+
+  // Try to recover performance data
+  if (!performance_exists(perf_name))
+    throw StopwatchException("Performance not initialized.");
+
+  PerformanceData &perf_info = records_of->find(perf_name)->second;
+
+  // check whether the performance has been reset
+  if (perf_info.clock_start == 0) return;
+
+  perf_info.stops++;
+  long double lapse = clock_end - perf_info.clock_start;
+
+  if (mode == CPU_TIME) lapse /= (double)CLOCKS_PER_SEC;
+
+  // Update last time
+  perf_info.last_time = lapse;
+
+  // Update min/max time
+  if (lapse >= perf_info.max_time) perf_info.max_time = lapse;
+  if (lapse <= perf_info.min_time || perf_info.min_time == 0)
+    perf_info.min_time = lapse;
+
+  // Update total time
+  perf_info.total_time += lapse;
+}
+
+void Stopwatch::pause(string perf_name) {
+  if (!active) return;
+
+  long double clock_end = clock();
+
+  // Try to recover performance data
+  if (!performance_exists(perf_name))
+    throw StopwatchException("Performance not initialized.");
+
+  PerformanceData &perf_info = records_of->find(perf_name)->second;
+
+  // check whether the performance has been reset
+  if (perf_info.clock_start == 0) return;
+
+  long double lapse = clock_end - perf_info.clock_start;
+
+  // Update total time
+  perf_info.last_time += lapse;
+  perf_info.total_time += lapse;
+}
+
+void Stopwatch::reset_all() {
+  if (!active) return;
+
+  map<string, PerformanceData>::iterator it;
+
+  for (it = records_of->begin(); it != records_of->end(); ++it) {
+    reset(it->first);
+  }
+}
+
+void Stopwatch::report_all(int precision, std::ostream &output) {
+  if (!active) return;
+
+  output << "\n*** PROFILING RESULTS [ms] (min - avg - max - lastTime - "
+            "nSamples - totalTime) ***\n";
+  map<string, PerformanceData>::iterator it;
+  for (it = records_of->begin(); it != records_of->end(); ++it) {
+    report(it->first, precision, output);
+  }
+}
+
+void Stopwatch::reset(string perf_name) {
+  if (!active) return;
+
+  // Try to recover performance data
+  if (!performance_exists(perf_name))
+    throw StopwatchException("Performance not initialized.");
+
+  PerformanceData &perf_info = records_of->find(perf_name)->second;
+
+  perf_info.clock_start = 0;
+  perf_info.total_time = 0;
+  perf_info.min_time = 0;
+  perf_info.max_time = 0;
+  perf_info.last_time = 0;
+  perf_info.paused = false;
+  perf_info.stops = 0;
+}
+
+void Stopwatch::turn_on() {
+  std::cout << "Stopwatch active." << std::endl;
+  active = true;
+}
+
+void Stopwatch::turn_off() {
+  std::cout << "Stopwatch inactive." << std::endl;
+  active = false;
+}
+
+void Stopwatch::report(string perf_name, int precision, std::ostream &output) {
+  if (!active) return;
+
+  // Try to recover performance data
+  if (!performance_exists(perf_name))
+    throw StopwatchException("Performance not initialized.");
+
+  PerformanceData &perf_info = records_of->find(perf_name)->second;
+
+  string pad = "";
+  for (size_t i = perf_name.length(); i < STOP_WATCH_MAX_NAME_LENGTH; i++)
+    pad.append(" ");
+
+  output << perf_name << pad;
+  output << std::fixed << std::setprecision(precision)
+         << (perf_info.min_time * 1e3) << "\t";
+  output << std::fixed << std::setprecision(precision)
+         << (perf_info.total_time * 1e3 / (long double)perf_info.stops) << "\t";
+  output << std::fixed << std::setprecision(precision)
+         << (perf_info.max_time * 1e3) << "\t";
+  output << std::fixed << std::setprecision(precision)
+         << (perf_info.last_time * 1e3) << "\t";
+  output << std::fixed << std::setprecision(precision) << perf_info.stops
+         << std::endl;
+  output << std::fixed << std::setprecision(precision)
+         << perf_info.total_time * 1e3 << std::endl;
+}
+
+long double Stopwatch::get_time_so_far(string perf_name) {
+  // Try to recover performance data
+  if (!performance_exists(perf_name))
+    throw StopwatchException("Performance not initialized.");
+
+  long double lapse =
+      (take_time() - (records_of->find(perf_name)->second).clock_start);
+
+  if (mode == CPU_TIME) lapse /= (double)CLOCKS_PER_SEC;
+
+  return lapse;
+}
+
+long double Stopwatch::get_total_time(string perf_name) {
+  // Try to recover performance data
+  if (!performance_exists(perf_name))
+    throw StopwatchException("Performance not initialized.");
+
+  PerformanceData &perf_info = records_of->find(perf_name)->second;
+
+  return perf_info.total_time;
+}
+
+long double Stopwatch::get_average_time(string perf_name) {
+  // Try to recover performance data
+  if (!performance_exists(perf_name))
+    throw StopwatchException("Performance not initialized.");
+
+  PerformanceData &perf_info = records_of->find(perf_name)->second;
+
+  return (perf_info.total_time / (long double)perf_info.stops);
+}
+
+long double Stopwatch::get_min_time(string perf_name) {
+  // Try to recover performance data
+  if (!performance_exists(perf_name))
+    throw StopwatchException("Performance not initialized.");
+
+  PerformanceData &perf_info = records_of->find(perf_name)->second;
+
+  return perf_info.min_time;
+}
+
+long double Stopwatch::get_max_time(string perf_name) {
+  // Try to recover performance data
+  if (!performance_exists(perf_name))
+    throw StopwatchException("Performance not initialized.");
+
+  PerformanceData &perf_info = records_of->find(perf_name)->second;
+
+  return perf_info.max_time;
+}
+
+long double Stopwatch::get_last_time(string perf_name) {
+  // Try to recover performance data
+  if (!performance_exists(perf_name))
+    throw StopwatchException("Performance not initialized.");
+
+  PerformanceData &perf_info = records_of->find(perf_name)->second;
+
+  return perf_info.last_time;
+}

tests/CMakeLists.txt

+# Copyright 2010, 2020, François Bleibel, Olivier Stasse, Guilhem Saurel, JRL,
+# CNRS/AIST, LAAS-CNRS
+
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/test-paths.h.in
+               ${CMAKE_CURRENT_BINARY_DIR}/test-paths.h)
+
+# Make Boost.Test generates the main function in test cases.
+add_definitions(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
+
+# Definition of a macro that corresponds to the suffix for a dynamic lib
+add_definitions(-DTESTS_DYNLIBSUFFIX="${CMAKE_SHARED_LIBRARY_SUFFIX}")
+
+# The sot-core plugin libraries that tests have to be linked against the name of
+# the variable for test EXECUTABLE_NAME is TEST_${EXECUTABLE_NAME}_LIBS
+set(TEST_tsot_LIBS task feature-visual-point gain-adaptive)
+
+set(TEST_test_traces_EXT_LIBS dynamic-graph::tracer)
+
+set(TEST_test_gain_LIBS gain-adaptive feature-visual-point)
+
+set(TEST_test_task_LIBS gain-adaptive feature-visual-point task)
+
+set(TEST_test_feature_point6d_LIBS gain-adaptive feature-point6d task)
+
+set(TEST_test_feature_generic_LIBS gain-adaptive feature-generic task
+                                   feature-pose)
+
+set(TEST_test_mailbox_LIBS mailbox-vector)
+
+set(TEST_test_control_pd_LIBS control-pd)
+
+set(TEST_test_control_admittance_LIBS admittance-control-op-point)
+
+set(TEST_test_feature_generic_EXT_LIBS pinocchio::pinocchio)
+
+set(TEST_test_device_EXT_LIBS pinocchio::pinocchio)
+
+set(TEST_test_filter_differentiator_LIBS filter-differentiator)
+
+set(TEST_test_madgwick_ahrs_LIBS madgwickahrs)
+
+set(tests
+    dummy
+    control/test_control_pd
+    control/test_control_admittance
+    features/test_feature_point6d
+    features/test_feature_generic
+    filters/test_filter_differentiator
+    filters/test_madgwick_ahrs
+    signal/test_signal
+    signal/test_depend
+    signal/test_ptr
+    signal/test_dep
+    signal/test_ptrcast
+    sot/tsot
+    traces/files
+    traces/test_traces
+    task/test_flags
+    task/test_gain
+    task/test_multi_bound
+    task/test_task
+    tools/test_boost
+    tools/test_device
+    tools/test_sot_loader
+    tools/test_mailbox
+    tools/test_matrix
+    tools/test_robot_utils
+    math/matrix-twist
+    math/matrix-homogeneous
+    matrix/test_operator)
+
+# TODO
+if(WIN32)
+  list(REMOVE_ITEM tests tools/test_mailbox)
+endif(WIN32)
+
+if(UNIX)
+  add_library(pluginabstract SHARED tools/plugin.cc)
+
+  add_executable(test_abstract_interface tools/test_abstract_interface.cpp)
+  target_link_libraries(
+    test_abstract_interface PRIVATE Boost::program_options pluginabstract
+                                    ${CMAKE_DL_LIBS})
+
+  add_library(dummy_sot_external_interface SHARED
+              tools/dummy-sot-external-interface.cc)
+endif(UNIX)
+
+foreach(path ${tests})
+  get_filename_component(test ${path} NAME)
+  add_unit_test(${test} ${path}.cpp)
+
+  target_link_libraries(
+    ${test} PRIVATE ${PROJECT_NAME} Boost::unit_test_framework
+                    ${TEST_${test}_LIBS} ${TEST_${test}_EXT_LIBS})
+
+  # add some preprocessor variable
+  target_compile_definitions(
+    ${test}
+    PUBLIC
+      LIB_PLUGIN_ABSTRACT_PATH="${CMAKE_BINARY_DIR}/tests/libdummy_sot_external_interface.so"
+      TEST_GOOD_PYTHON_SCRIPT="${PROJECT_SOURCE_DIR}/tests/tools/good_python_script.py"
+      TEST_BAD_PYTHON_SCRIPT="${PROJECT_SOURCE_DIR}/tests/tools/bad_python_script"
+  )
+
+  if(UNIX)
+    target_link_libraries(${test} PRIVATE ${CMAKE_DL_LIBS})
+  endif(UNIX)
+endforeach(path ${tests})
+
+add_subdirectory(python)

tests/control/test_control_admittance.cpp

+/*
+ * Copyright 2019,
+ * Noëlie Ramuzat,
+ *
+ *
+ */
+
+#include <iostream>
+#include <sot/core/debug.hh>
+
+#ifndef WIN32
+#include <unistd.h>
+#endif
+
+using namespace std;
+
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/factory.h>
+
+#include <sot/core/admittance-control-op-point.hh>
+#include <sstream>
+
+using namespace dynamicgraph;
+using namespace dynamicgraph::sot;
+
+#define BOOST_TEST_MODULE debug - control - admittance
+
+#include <boost/test/output_test_stream.hpp>
+#include <boost/test/unit_test.hpp>
+
+BOOST_AUTO_TEST_CASE(control_admittance) {
+  sot::core::AdmittanceControlOpPoint *aControlAdm =
+      new sot::core::AdmittanceControlOpPoint("acontrol_admittance");
+
+  std::istringstream Kp("[6](10.0,10.0,10.0,10.0,10.0,10.0)");
+  std::istringstream Kd("[6](0.0,0.0,0.0,0.0,0.0,0.0)");
+  aControlAdm->m_KpSIN.set(Kp);
+  aControlAdm->m_KdSIN.set(Kd);
+  std::istringstream dqSaturation("[6](10.0,10.0,10.0,10.0,10.0,10.0)");
+  aControlAdm->m_dqSaturationSIN.set(dqSaturation);
+  std::istringstream w_forceDes("[6](100.0,0.0,0.0,0.0,0.0,0.0)");
+  aControlAdm->m_w_forceDesSIN.set(w_forceDes);
+  std::istringstream force("[6](10.0,0.0,10.0,0.0,0.0,0.0)");
+  aControlAdm->m_forceSIN.set(force);
+  MatrixHomogeneous opPose;
+  opPose.translation() << 0.3, 0.0, 0.0;
+  opPose.linear() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
+  aControlAdm->m_opPoseSIN = opPose;
+  MatrixHomogeneous sensorPose;
+  sensorPose.translation() << 0.3, 0.0, 0.0;
+  sensorPose.linear() << 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0;
+  aControlAdm->m_sensorPoseSIN = sensorPose;
+  aControlAdm->init(0.001);
+
+  aControlAdm->m_dqSOUT.recompute(0);
+  {
+    dynamicgraph::Vector expected(6);
+    expected << 1.1, 0.0, -0.109, 0.0, 0.03, 0.0;
+    BOOST_CHECK(aControlAdm->m_dqSOUT(0).isApprox(expected));
+  }
+}

tests/control/test_control_pd.cpp

+/*
+ * Copyright 2019,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+
+#include <iostream>
+#include <sot/core/debug.hh>
+
+#ifndef WIN32
+#include <unistd.h>
+#endif
+
+using namespace std;
+
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/factory.h>
+
+#include <sot/core/control-pd.hh>
+#include <sstream>
+
+using namespace dynamicgraph;
+using namespace dynamicgraph::sot;
+
+#define BOOST_TEST_MODULE debug - control - pd
+
+#include <boost/test/output_test_stream.hpp>
+#include <boost/test/unit_test.hpp>
+
+BOOST_AUTO_TEST_CASE(control_pd) {
+  sot::ControlPD *aControlPD = new ControlPD("acontrol_pd");
+  aControlPD->init(0.001);
+  std::istringstream Kpiss("[5](10.0,20.0,30.0,40.0,50.0)");
+  std::istringstream Kdiss("[5](0.10,0.20,0.30,0.40,0.50)");
+  aControlPD->KpSIN.set(Kpiss);
+  aControlPD->KdSIN.set(Kdiss);
+  std::istringstream posiss("[5](1.0,1.0,1.0,1.0,1.0)");
+  aControlPD->positionSIN.set(posiss);
+  std::istringstream dposiss("[5](3.0,3.1,3.2,3.3,3.4)");
+  aControlPD->desiredpositionSIN.set(dposiss);
+  std::istringstream veliss("[5](0.0,0.0,0.0,0.0,0.0)");
+  aControlPD->velocitySIN.set(veliss);
+  std::istringstream dveliss("[5](1.5,1.4,1.3,1.2,1.1)");
+  aControlPD->desiredvelocitySIN.set(dveliss);
+
+  aControlPD->controlSOUT.recompute(0);
+  aControlPD->positionErrorSOUT.recompute(0);
+  aControlPD->velocityErrorSOUT.recompute(0);
+  {
+    boost::test_tools::output_test_stream output;
+    aControlPD->controlSOUT.get(output);
+    BOOST_CHECK(output.is_equal("20.15 42.28 66.39 92.48 120.55"));
+  }
+  {
+    boost::test_tools::output_test_stream output;
+    aControlPD->positionErrorSOUT.get(output);
+    BOOST_CHECK(output.is_equal("2 2.1 2.2 2.3 2.4"));
+  }
+  {
+    boost::test_tools::output_test_stream output;
+    aControlPD->velocityErrorSOUT.get(output);
+    BOOST_CHECK(output.is_equal("1.5 1.4 1.3 1.2 1.1"));
+  }
+}

tests/dummy.cpp

+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+
+#include <iostream>
+#include <sot/core/debug.hh>
+
+int main(int, char **) {
+  dynamicgraph::sot::sotDEBUGFLOW.openFile();
+  dynamicgraph::sot::sotDEBUGFLOW.trace("test test test");
+
+  std::cout << "It works!" << std::endl;
+  return 0;
+}

tests/factory/test_factory.cpp

+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/linear-algebra.h>
+
+#include <iostream>
+#include <sot/core/debug.hh>
+#include <sot/core/exception-feature.hh>
+#include <sot/core/factory.hh>
+#include <sot/core/feature-visual-point.hh>
+#include <string>
+
+#include "../test-paths.h"
+using namespace std;
+using namespace dynamicgraph::sot;
+using namespace dg;
+
+#ifdef WIN32
+#include <Windows.h>
+#else
+#include <dlfcn.h>
+#endif
+
+#ifdef WIN32
+typedef HMODULE sotPluginKey;
+#else
+typedef void *sotPluginKey;
+#endif
+
+class TestFeature : public FeatureAbstract {
+ public:
+  TestFeature(void) : FeatureAbstract("") {}
+  virtual ~TestFeature(void) {}
+  virtual unsigned int &getDimension(unsigned int &res, int /*time*/) {
+    return res;
+  }
+
+  virtual dg::Vector &computeError(dg::Vector &res, int /*time*/) {
+    return res;
+  }
+  virtual dg::Matrix &computeJacobian(dg::Matrix &res, int /*time*/) {
+    return res;
+  }
+  virtual dg::Vector &computeActivation(dg::Vector &res, int /*time*/) {
+    return res;
+  }
+};
+
+int main() {
+  sotDEBUG(0) << "# In {" << endl;
+  //   Entity test("");
+  //   ExceptionFeature t2(ExceptionFeature::BAD_INIT);
+  //   ExceptionSignal t4(ExceptionSignal::COPY_NOT_INITIALIZED);
+  //   Flags t3;
+  //   TestFeature t5;
+
+#ifdef WIN32
+  sotPluginKey dlib = LoadLibrary(PLUGIN_LIB_INSTALL_PATH
+                                  "/feature-visual-point" TESTS_DYNLIBSUFFIX);
+#else
+  sotPluginKey dlib =
+      dlopen(PLUGIN_LIB_INSTALL_PATH "/feature-visual-point" TESTS_DYNLIBSUFFIX,
+             RTLD_NOW);
+#endif
+
+  if (NULL == dlib) {
+    cerr << " Error dl" << endl;
+#ifndef WIN32
+    cerr << dlerror() << endl;
+#else
+    // Retrieve the system error message for the last-error code
+    LPTSTR pszMessage;
+    DWORD dwLastError = GetLastError();
+    FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM |
+                      FORMAT_MESSAGE_IGNORE_INSERTS,
+                  NULL, dwLastError, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
+                  (LPTSTR)&pszMessage, 0, NULL);
+
+    cerr << pszMessage << endl;
+    LocalFree(pszMessage);
+#endif
+
+    exit(1);
+  }
+
+#ifdef WIN32
+  dlib =
+      LoadLibrary(PLUGIN_LIB_INSTALL_PATH "/gain-adaptive" TESTS_DYNLIBSUFFIX);
+#else
+  dlib = dlopen(PLUGIN_LIB_INSTALL_PATH "/gain-adaptive" TESTS_DYNLIBSUFFIX,
+                RTLD_NOW);
+#endif
+
+  if (NULL == dlib) {
+    cerr << " Error dl" << endl;
+#ifndef WIN32
+    cerr << dlerror() << endl;
+#else
+    // Retrieve the system error message for the last-error code
+    LPTSTR pszMessage;
+    DWORD dwLastError = GetLastError();
+    FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM |
+                      FORMAT_MESSAGE_IGNORE_INSERTS,
+                  NULL, dwLastError, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
+                  (LPTSTR)&pszMessage, 0, NULL);
+
+    cerr << pszMessage << endl;
+    LocalFree(pszMessage);
+#endif
+    exit(1);
+  }
+
+  Entity *gain =
+      FactoryStorage::getInstance()->newEntity("GainAdaptive", "Gain");
+  FeatureAbstract *point =
+      sotFactory.newFeature("FeatureVisualPoint", "DynamicTest.");
+
+  try {
+    gain->display(cout);
+    cout << endl;
+    cout << gain->getClassName();
+    cout << endl;
+
+    point->display(cout);
+    cout << endl;
+    cout << point->getClassName();
+    cout << endl;
+  } catch (ExceptionSignal e) {
+    cout << "Exception caught! " << e << endl;
+  }
+
+  sotDEBUG(0) << "# Out }" << endl;
+}