diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8e6c681b1cba6d7377d16169ab1a7111e46fe91b..fb5bf237ac1774fd7cef9b60822f6949b0986c25 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -58,6 +58,7 @@ SET(BOOST_COMPONENTS thread filesystem program_options unit_test_framework syste
 
 OPTION (BUILD_ROS_PACKAGES "Build ros packages" ON)
 OPTION (BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
+OPTION (INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python binding" OFF)
 OPTION (BUILD_TEST "Build tests" ON)
 
 # name of the python module
@@ -89,8 +90,6 @@ SET(LIBRARY_NAME ${SOTTALOSBALANCE_LIB_NAME})
 
 SET(${LIBRARY_NAME}_HEADERS
   include/sot/talos_balance/utils/commands-helper.hh
-  include/sot/talos_balance/utils/stop-watch.hh
-  include/sot/talos_balance/utils/causal-filter.hh
   include/sot/talos_balance/utils/statistics.hh
   include/sot/talos_balance/math/fwd.hh
   include/sot/talos_balance/robot/fwd.hh
@@ -106,9 +105,7 @@ SET(${LIBRARY_NAME}_HEADERS
 #  PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
 
 SET(${LIBRARY_NAME}_SOURCES ${${LIBRARY_NAME}_HEADERS}
-    src/utils/causal-filter.cpp
     src/utils/statistics.cpp
-    src/utils/stop-watch.cpp
     src/robot/robot-wrapper.cpp
     src/sdk_qualisys/Network.cpp
     src/sdk_qualisys/RTPacket.cpp
@@ -146,7 +143,9 @@ ENDIF(UNIX AND NOT APPLE)
 
 TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES})
 
-INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
+IF (NOT INSTALL_PYTHON_INTERFACE_ONLY)
+  INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
+ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
 
 IF(BUILD_PYTHON_INTERFACE)
   INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/__init__.py
@@ -178,6 +177,8 @@ IF(BUILD_PYTHON_INTERFACE)
                   python/${SOTTALOSBALANCE_PYNAME}/test/appli_dcmComZmpControl.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmZmpControl.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/appli_dcmZmpControl.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmZmpControl_file.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/appli_dcmZmpControl_file.py		  
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_comAdmittance.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_single_joint.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/appli_admittance_single_joint.py
@@ -245,8 +246,10 @@ MACRO(DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE MODULENAME SUBMODULENAME LIBRARYNAME TA
    )
   CMAKE_POLICY(POP)
 
-  TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD} "-Wl,--no-as-needed")
-  TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD} ${LIBRARYNAME} ${PYTHON_LIBRARY})
+  TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD}
+    "-Wl,--no-as-needed")
+  TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD}
+    ${LIBRARYNAME} ${PYTHON_LIBRARY})
 
   INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
 
@@ -282,7 +285,8 @@ MACRO(DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE MODULENAME SUBMODULENAME LIBRARYNAME TA
 ENDMACRO(DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE)
 
 MACRO(SOT_TALOS_BALANCE_PYTHON_MODULE SUBMODULENAME LIBRARYNAME TARGETNAME)
-  DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE("${SOTTALOSBALANCE_PYNAME}" "${SUBMODULENAME}" "${LIBRARYNAME}" "${TARGETNAME}")
+  DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE("${SOTTALOSBALANCE_PYNAME}"
+    "${SUBMODULENAME}" "${LIBRARYNAME}" "${TARGETNAME}")
 ENDMACRO(SOT_TALOS_BALANCE_PYTHON_MODULE)
 
 ADD_SUBDIRECTORY(src)
diff --git a/include/sot/talos_balance/filter-differentiator.hh b/include/sot/talos_balance/filter-differentiator.hh
deleted file mode 100644
index d713ba74cbee145fd02439a1f9db4533dc1e9758..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/filter-differentiator.hh
+++ /dev/null
@@ -1,118 +0,0 @@
-/*
- * Copyright 2017-, Rohan Budhirja, LAAS-CNRS
- *
- * This file is part of sot-torque-control.
- * sot-torque-control is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-torque-control is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.  You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __sot_torque_control_FilterDifferentiator_H__
-#define __sot_torque_control_FilterDifferentiator_H__
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-#  if defined (low_pass_filter_EXPORTS)
-#    define SOTFILTERDIFFERENTIATOR_EXPORT __declspec(dllexport)
-#  else
-#    define SOTFILTERDIFFERENTIATOR_EXPORT __declspec(dllimport)
-#  endif
-#else
-#  define SOTFILTERDIFFERENTIATOR_EXPORT
-#endif
-
-//#define VP_DEBUG 1        /// enable debug output
-//#define VP_DEBUG_MODE 20
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* HELPER */
-#include <dynamic-graph/signal-helper.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
-#include <sot/talos_balance/utils/causal-filter.hh>
-
-namespace dynamicgraph {
-  namespace sot {
-    namespace talos_balance {
-
-      /**
-        * This Entity takes as inputs a signal and applies a low pass filter
-        and computes finite difference derivative.
-        */
-      class SOTFILTERDIFFERENTIATOR_EXPORT FilterDifferentiator
-          :public ::dynamicgraph::Entity
-      {
-        DYNAMIC_GRAPH_ENTITY_DECL();
-
-      public:  /* --- SIGNALS --- */
-        DECLARE_SIGNAL_IN(x,                 dynamicgraph::Vector);
-        DECLARE_SIGNAL_OUT(x_filtered,       dynamicgraph::Vector);
-        DECLARE_SIGNAL_OUT(dx,               dynamicgraph::Vector);
-        DECLARE_SIGNAL_OUT(ddx,              dynamicgraph::Vector);
-
-        /// The following inner signals are used because this entity has some output signals
-        /// whose related quantities are computed at the same time by the same algorithm
-        /// To avoid the risk of recomputing the same things twice, we create an inner signal that groups together
-        /// all the quantities that are computed together. Then the single output signals will depend
-        /// on this inner signal, which is the one triggering the computations.
-        /// Inner signals are not exposed, so that nobody can access them.
-
-        /// This signal contains the estimated positions, velocities and accelerations.
-        DECLARE_SIGNAL_INNER(x_dx_ddx,              dynamicgraph::Vector);
-        
-      protected:
-      
-        double m_dt;      /// sampling timestep of the input signal
-        int m_x_size;
-
-        /// polynomial-fitting filters
-        CausalFilter* m_filter;
-
-      public:
-        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-        /** --- CONSTRUCTOR ---- */
-        FilterDifferentiator( const std::string & name );
-
-        /** Initialize the FilterDifferentiator.
-         * @param timestep Period (in seconds) after which the sensors' data are updated.
-         * @param sigSize  Size of the input signal.
-         * @param delay    Delay (in seconds) introduced by the estimation.
-         *                 This should be a multiple of timestep.
-         * @note The estimationDelay is half of the length of the window used for the
-         * polynomial fitting. The larger the delay, the smoother the estimations.
-         */
-        void init(const double &timestep,
-                  const int& xSize,
-                  const Eigen::VectorXd& filter_numerator,
-                  const Eigen::VectorXd& filter_denominator);
-
-        void switch_filter(const Eigen::VectorXd& filter_numerator,
-                           const Eigen::VectorXd& filter_denominator);
-
-
-      protected:
-
-      public: /* --- ENTITY INHERITANCE --- */
-        virtual void display( std::ostream& os ) const;
-
-      }; // class FilterDifferentiator
-
-    } // namespace talos_balance
-  } // namespace sot
-} // namespace dynamicgraph
-
-
-
-#endif // #ifndef __sot_torque_control_FilterDifferentiator_H__
diff --git a/include/sot/talos_balance/imu_offset_compensation.hh b/include/sot/talos_balance/imu_offset_compensation.hh
deleted file mode 100644
index d5854bb888c2189b36511c911d17917171ece49b..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/imu_offset_compensation.hh
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- * Copyright 2017, Andrea Del Prete, LAAS-CNRS
- *
- * This file is part of sot-torque-control.
- * sot-torque-control is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-torque-control is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.  You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __sot_torque_control_imu_offset_compensation_H__
-#define __sot_torque_control_imu_offset_compensation_H__
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-#  if defined (imu_offset_compensation_EXPORTS)
-#    define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllexport)
-#  else
-#    define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllimport)
-#  endif
-#else
-#  define SOTIMUOFFSETCOMPENSATION_EXPORT
-#endif
-
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#include <dynamic-graph/signal-helper.h>
-#include <sot/core/matrix-geometry.hh>
-#include <map>
-#include "boost/assign.hpp"
-
-namespace dynamicgraph {
-  namespace sot {
-    namespace talos_balance {
-
-      /* --------------------------------------------------------------------- */
-      /* --- CLASS ----------------------------------------------------------- */
-      /* --------------------------------------------------------------------- */
-
-      class SOTIMUOFFSETCOMPENSATION_EXPORT ImuOffsetCompensation
-          :public::dynamicgraph::Entity
-      {
-        typedef ImuOffsetCompensation EntityClassName;
-        DYNAMIC_GRAPH_ENTITY_DECL();
-        typedef Eigen::Vector3d Vector3;
-
-      public:
-        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-        /* --- CONSTRUCTOR ---- */
-        ImuOffsetCompensation( const std::string & name );
-
-        /* --- COMMANDS --- */
-        void init(const double& dt);
-        void update_offset(const double & duration);
-        void setGyroDCBlockerParameter(const double & alpha);
-        /* --- SIGNALS --- */
-        DECLARE_SIGNAL_IN(accelerometer_in,          dynamicgraph::Vector);  /// raw accelerometer data
-        DECLARE_SIGNAL_IN(gyrometer_in,              dynamicgraph::Vector);  /// raw gyrometer data
-        DECLARE_SIGNAL_OUT(accelerometer_out,        dynamicgraph::Vector);  /// compensated accelerometer data
-        DECLARE_SIGNAL_OUT(gyrometer_out,            dynamicgraph::Vector);  /// compensated gyrometer data
-
-      protected:
-        /* --- ENTITY INHERITANCE --- */
-        virtual void display( std::ostream& os ) const;
-
-        /* --- METHODS --- */
-        void update_offset_impl(int iter);
-
-      protected:
-        bool            m_initSucceeded;      /// true if the entity has been successfully initialized
-        double          m_dt;                 /// sampling time in seconds
-        int             m_update_cycles_left; /// number of update cycles left
-        int             m_update_cycles;      /// total number of update cycles to perform
-        double          m_a_gyro_DC_blocker;  /// filter parameter to remove DC from gyro online (should be close to <1.0 and equal to 1.0 for disabling)
-        Vector3         m_gyro_offset;        /// gyrometer offset
-        Vector3         m_acc_offset;         /// accelerometer offset
-
-        Vector3         m_gyro_sum;           /// tmp variable to store the sum of the gyro measurements during update phase
-        Vector3         m_acc_sum;            /// tmp variable to store the sum of the acc measurements during update phase
-
-      }; // class ImuOffsetCompensation
-    }    // namespace talos_balance
-  }      // namespace sot
-}        // namespace dynamicgraph
-
-#endif // #ifndef __sot_torque_control_imu_offset_compensation_H__
diff --git a/include/sot/talos_balance/madgwickahrs.hh b/include/sot/talos_balance/madgwickahrs.hh
deleted file mode 100644
index 6f6eb388c1f2fbd8131a0c225d51a4667d10b05c..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/madgwickahrs.hh
+++ /dev/null
@@ -1,108 +0,0 @@
-//=====================================================================================================
-//
-// Implementation of Madgwick's IMU and AHRS algorithms.
-// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
-//
-// Date       Author        Notes
-// 29/09/2011 SOH Madgwick  Initial release
-// 02/10/2011 SOH Madgwick  Optimised for reduced CPU load
-// 11/05/2017 T Flayols     Make it a dynamic graph entity
-// 26/03/2019 G Buondonno   Converted to double
-//
-//=====================================================================================================
-
-/*
- * Copyright 2017, Thomas Flayols, LAAS-CNRS
- *
- * This file is part of sot-torque-control.
- * sot-torque-control is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-torque-control is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.  You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __sot_torque_control_madgwickahrs_H__
-#define __sot_torque_control_madgwickahrs_H__
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-#  if defined (madgwickahrs_EXPORTS)
-#    define SOTMADGWICKAHRS_EXPORT __declspec(dllexport)
-#  else
-#    define SOTMADGWICKAHRS_EXPORT __declspec(dllimport)
-#  endif
-#else
-#  define SOTMADGWICKAHRS_EXPORT
-#endif
-
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#include <dynamic-graph/signal-helper.h>
-#include <sot/core/matrix-geometry.hh>
-#include <map>
-#include "boost/assign.hpp"
-
-#define betaDef 0.01 // 2 * proportional g
-
-namespace dynamicgraph {
-  namespace sot {
-    namespace talos_balance {
-
-      /* --------------------------------------------------------------------- */
-      /* --- CLASS ----------------------------------------------------------- */
-      /* --------------------------------------------------------------------- */
-
-      class SOTMADGWICKAHRS_EXPORT MadgwickAHRS
-          :public::dynamicgraph::Entity
-      {
-        typedef MadgwickAHRS EntityClassName;
-        DYNAMIC_GRAPH_ENTITY_DECL();
-
-      public:
-        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-        /* --- CONSTRUCTOR ---- */
-        MadgwickAHRS( const std::string & name );
-
-        void init(const double& dt);
-        void set_beta(const double & beta);
-
-        /* --- SIGNALS --- */
-        DECLARE_SIGNAL_IN(accelerometer,              dynamicgraph::Vector);  /// ax ay az in m.s-2
-        DECLARE_SIGNAL_IN(gyroscope,                  dynamicgraph::Vector);  /// gx gy gz in rad.s-1
-        DECLARE_SIGNAL_OUT(imu_quat,                  dynamicgraph::Vector);  /// Estimated orientation of IMU as a quaternion
-
-      protected:
-        /* --- COMMANDS --- */
-        /* --- ENTITY INHERITANCE --- */
-        virtual void display( std::ostream& os ) const;
-
-        /* --- METHODS --- */
-        double invSqrt(double x);
-        void madgwickAHRSupdateIMU(double gx, double gy, double gz, double ax, double ay, double az) ;
-        //void madgwickAHRSupdate(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz);
-
-      protected:
-        bool     m_initSucceeded;        /// true if the entity has been successfully initialized
-        double   m_beta;                 /// 2 * proportional gain (Kp)
-        double   m_q0, m_q1, m_q2, m_q3; /// quaternion of sensor frame
-        double   m_sampleFreq;           /// sample frequency in Hz
-
-      }; // class MadgwickAHRS
-    }    // namespace talos_balance
-  }      // namespace sot
-}        // namespace dynamicgraph
-
-#endif // #ifndef __sot_torque_control_madgwickahrs_H__
diff --git a/include/sot/talos_balance/parameter-server.hh b/include/sot/talos_balance/parameter-server.hh
deleted file mode 100644
index 196064d3ea52df765e876af9bb94a53556f0f667..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/parameter-server.hh
+++ /dev/null
@@ -1,126 +0,0 @@
-/*
- * Copyright 2015, Andrea Del Prete, LAAS-CNRS
- *
- * This file is part of sot-torque-control.
- * sot-torque-control is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-torque-control is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.  You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __sot_torque_control_parameter_server_H__
-#define __sot_torque_control_parameter_server_H__
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-#  if defined (__sot_torque_parameter_server_H__)
-#    define SOTParameterServer_EXPORT __declspec(dllexport)
-#  else
-#    define SOTParameterServer_EXPORT __declspec(dllimport)
-#  endif
-#else
-#  define SOTParameterServer_EXPORT
-#endif
-
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#include <dynamic-graph/signal-helper.h>
-#include <sot/core/matrix-geometry.hh>
-#include <sot/core/robot-utils.hh>
-#include <map>
-#include "boost/assign.hpp"
-
-
-#include <pinocchio/multibody/model.hpp>
-#include <pinocchio/parsers/urdf.hpp>
-#include <sot/talos_balance/robot/robot-wrapper.hh>
-
-namespace dynamicgraph {
-  namespace sot {
-    namespace talos_balance {
-
-      /* --------------------------------------------------------------------- */
-      /* --- CLASS ----------------------------------------------------------- */
-      /* --------------------------------------------------------------------- */
-
-/// Number of time step to transition from one ctrl mode to another
-#define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
-
-
-      class SOTParameterServer_EXPORT ParameterServer
-        :public::dynamicgraph::Entity
-      {
-        typedef ParameterServer EntityClassName;
-        DYNAMIC_GRAPH_ENTITY_DECL();
-
-      public:
-        /* --- CONSTRUCTOR ---- */
-        ParameterServer( const std::string & name);
-
-        /// Initialize
-        /// @param dt: control interval
-        /// @param urdfFile: path to the URDF model of the robot
-        void init(const double & dt,
-                  const std::string & urdfFile,
-                  const std::string & robotRef);
-
-        /* --- SIGNALS --- */
-
-
-        /* --- COMMANDS --- */
-
-        /// Commands related to joint name and joint id
-        void setNameToId(const std::string& jointName, const unsigned int & jointId);
-        void setJointLimitsFromId(const unsigned int &jointId, const double &lq, const double &uq);
-
-        /// Command related to ForceUtil
-        void setForceLimitsFromId(const unsigned int & jointId, const dynamicgraph::Vector &lq, const dynamicgraph::Vector &uq);
-        void setForceNameToForceId(const std::string& forceName, const unsigned int & forceId);
-
-        /// Commands related to FootUtil
-        void setRightFootSoleXYZ(const dynamicgraph::Vector &);
-        void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
-        void setFootFrameName(const std::string &, const std::string &);
-        void setImuJointName(const std::string &);
-        void displayRobotUtil();
-        /// Set the mapping between urdf and sot.
-        void setJoints(const dynamicgraph::Vector &);
-
-        /* --- ENTITY INHERITANCE --- */
-        virtual void display( std::ostream& os ) const;
-
-      protected:
-        RobotUtilShrPtr                   m_robot_util;
-        dynamicgraph::sot::talos_balance::robots::RobotWrapper *  m_robot;
-        bool    m_initSucceeded;    /// true if the entity has been successfully initialized
-        double  m_dt;               /// control loop time period
-        bool    m_emergency_stop_triggered;  /// true if an emergency condition as been triggered either by an other entity, or by control limit violation
-        bool    m_is_first_iter;    /// true at the first iteration, false otherwise
-        int     m_iter;
-        double  m_sleep_time;       /// time to sleep at every iteration (to slow down simulation)
-
-        bool convertJointNameToJointId(const std::string& name, unsigned int& id);
-        bool isJointInRange(unsigned int id, double q);
-        void updateJointCtrlModesOutputSignal();
-
-      }; // class ParameterServer
-
-    }    // namespace talos_balance
-  }      // namespace sot
-}        // namespace dynamicgraph
-
-
-
-#endif // #ifndef __sot_torque_control_control_manager_H__
diff --git a/include/sot/talos_balance/admittance-controller.hh b/include/sot/talos_balance/simple-admittance-controller.hh
similarity index 87%
rename from include/sot/talos_balance/admittance-controller.hh
rename to include/sot/talos_balance/simple-admittance-controller.hh
index 9c9ddfba2050a78cfc4883f1b4865f143e302f94..7b5d908c197590db18441bb906eee7b26be25147 100644
--- a/include/sot/talos_balance/admittance-controller.hh
+++ b/include/sot/talos_balance/simple-admittance-controller.hh
@@ -23,12 +23,12 @@
 
 #if defined (WIN32)
 #  if defined (admittance_controller_EXPORTS)
-#    define ADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
+#    define SIMPLEADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
 #  else
-#    define ADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
+#    define SIMPLEADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
 #  endif
 #else
-#  define ADMITTANCECONTROLLER_EXPORT
+#  define SIMPLEADMITTANCECONTROLLER_EXPORT
 #endif
 
 
@@ -48,21 +48,21 @@ namespace dynamicgraph {
       /* --- CLASS ----------------------------------------------------------- */
       /* --------------------------------------------------------------------- */
 
-      class ADMITTANCECONTROLLER_EXPORT AdmittanceController
-          : public ::dynamicgraph::Entity
+      class SIMPLEADMITTANCECONTROLLER_EXPORT SimpleAdmittanceController
+	: public ::dynamicgraph::Entity
       {
-      DYNAMIC_GRAPH_ENTITY_DECL();
+	DYNAMIC_GRAPH_ENTITY_DECL();
 
       public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-        /* --- CONSTRUCTOR ---- */
-        AdmittanceController( const std::string & name );
-
+	  
+	  /* --- CONSTRUCTOR ---- */
+	  SimpleAdmittanceController( const std::string & name );
+	
         void init(const double & dt, const unsigned & n);
-
+	
         void setPosition(const dynamicgraph::Vector & position);
-
+	
         /* --- SIGNALS --- */
         DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
         DECLARE_SIGNAL_IN(state, dynamicgraph::Vector);
diff --git a/include/sot/talos_balance/base-estimator.hh b/include/sot/talos_balance/talos-base-estimator.hh
similarity index 97%
rename from include/sot/talos_balance/base-estimator.hh
rename to include/sot/talos_balance/talos-base-estimator.hh
index 1d72989c24f41fe6b9eb1db823e90b41f75dac73..e4decdd6db84ae2de8b78b99557076a2b89025d4 100644
--- a/include/sot/talos_balance/base-estimator.hh
+++ b/include/sot/talos_balance/talos-base-estimator.hh
@@ -23,12 +23,12 @@
 
 #if defined (WIN32)
 #  if defined (base_estimator_EXPORTS)
-#    define SOTBASEESTIMATOR_EXPORT __declspec(dllexport)
+#    define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllexport)
 #  else
-#    define SOTBASEESTIMATOR_EXPORT __declspec(dllimport)
+#    define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllimport)
 #  endif
 #else
-#  define SOTBASEESTIMATOR_EXPORT
+#  define TALOS_BASE_ESTIMATOR_EXPORT
 #endif
 
 
@@ -82,10 +82,10 @@ namespace dynamicgraph {
       /** Avoids singularity while taking the mean of euler angles**/
       double wEulerMean(double a1, double a2, double w1, double w2);
 
-      class SOTBASEESTIMATOR_EXPORT BaseEstimator
+      class TALOS_BASE_ESTIMATOR_EXPORT TalosBaseEstimator
           :public::dynamicgraph::Entity
       {
-        typedef BaseEstimator EntityClassName;
+        typedef TalosBaseEstimator EntityClassName;
         typedef pinocchio::SE3 SE3;
         typedef Eigen::Vector2d Vector2;
         typedef Eigen::Vector3d Vector3;
@@ -101,7 +101,7 @@ namespace dynamicgraph {
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
         /* --- CONSTRUCTOR ---- */
-        BaseEstimator( const std::string & name );
+        TalosBaseEstimator( const std::string & name );
 
         void init(const double & dt, const std::string& urdfFile);
 
@@ -249,7 +249,7 @@ namespace dynamicgraph {
         Vector3 m_last_DCvel;
         Vector3 m_last_DCacc;
 
-      }; // class BaseEstimator
+      }; // class TalosBaseEstimator
 
     }    // namespace talos_balance
   }      // namespace sot
diff --git a/include/sot/talos_balance/control-manager.hh b/include/sot/talos_balance/talos-control-manager.hh
similarity index 94%
rename from include/sot/talos_balance/control-manager.hh
rename to include/sot/talos_balance/talos-control-manager.hh
index 74e619db5ee0aa08b715ae78d168a5e03caf2c6b..697bb7c873d29511837fc565cd950bdd0dcd6840 100644
--- a/include/sot/talos_balance/control-manager.hh
+++ b/include/sot/talos_balance/talos-control-manager.hh
@@ -23,12 +23,12 @@
 
 #if defined (WIN32)
 #  if defined (__sot_torque_control_control_manager_H__)
-#    define SOTCONTROLMANAGER_EXPORT __declspec(dllexport)
+#    define TALOS_CONTROL_MANAGER_EXPORT __declspec(dllexport)
 #  else
-#    define SOTCONTROLMANAGER_EXPORT __declspec(dllimport)
+#    define TALOS_CONTROL_MANAGER_EXPORT __declspec(dllimport)
 #  endif
 #else
-#  define SOTCONTROLMANAGER_EXPORT
+#  define TALOS_CONTROL_MANAGER_EXPORT
 #endif
 
 
@@ -70,16 +70,16 @@ namespace dynamicgraph {
         return os;
       }
 
-      class SOTCONTROLMANAGER_EXPORT ControlManager
+      class TALOS_CONTROL_MANAGER_EXPORT TalosControlManager
         :public::dynamicgraph::Entity
       {
         typedef Eigen::VectorXd::Index Index;
-        typedef ControlManager EntityClassName;
+        typedef TalosControlManager EntityClassName;
         DYNAMIC_GRAPH_ENTITY_DECL();
 
       public:
         /* --- CONSTRUCTOR ---- */
-        ControlManager( const std::string & name);
+        TalosControlManager( const std::string & name);
 
         /// Initialize
         /// @param dt: control interval
@@ -142,7 +142,7 @@ namespace dynamicgraph {
         //bool isJointInRange(unsigned int id, double q);
         void updateJointCtrlModesOutputSignal();
 
-      }; // class ControlManager
+      }; // class TalosControlManager
 
     }    // namespace talos_balance
   }      // namespace sot
diff --git a/include/sot/talos_balance/utils/causal-filter.hh b/include/sot/talos_balance/utils/causal-filter.hh
deleted file mode 100644
index d740ea2600c7be26f3d15ace6cd2bf60cb54bd85..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/utils/causal-filter.hh
+++ /dev/null
@@ -1,54 +0,0 @@
-/*
- * Copyright 2017-, Rohan Budhirja, LAAS-CNRS
- *
- * This file is part of sot-torque-control.
- * sot-torque-control is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-torque-control is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.  You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-#include <Eigen/Core>
-
-class CausalFilter
-{
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  
-  /** --- CONSTRUCTOR ---- */
-  CausalFilter(const double &timestep,
-               const int& xSize,
-               const Eigen::VectorXd& filter_numerator,
-               const Eigen::VectorXd& filter_denominator);
-  
-  void get_x_dx_ddx(const Eigen::VectorXd& base_x,
-                Eigen::VectorXd& x_output_dx_ddx);
-  
-  void switch_filter(const Eigen::VectorXd& filter_numerator,
-                     const Eigen::VectorXd& filter_denominator);
-  
-private:
-  double m_dt;      /// sampling timestep of the input signal
-  int m_x_size;
-  Eigen::Index m_filter_order_m;
-  Eigen::Index m_filter_order_n;
-
-  Eigen::VectorXd m_filter_numerator;
-  Eigen::VectorXd m_filter_denominator;
-  bool first_sample;
-  int pt_numerator;
-  int pt_denominator;
-  Eigen::MatrixXd input_buffer;
-  Eigen::MatrixXd output_buffer;
-}; // class CausalFilter
diff --git a/include/sot/talos_balance/utils/stop-watch.hh b/include/sot/talos_balance/utils/stop-watch.hh
deleted file mode 100644
index 940932b0e74891bcd16c04192a402d05755e963a..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/utils/stop-watch.hh
+++ /dev/null
@@ -1,272 +0,0 @@
-/*
-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 __sot_talos_balance_stopwatch_H__
-#define __sot_talos_balance_stopwatch_H__
-
-#include <iostream>
-#include <map>
-#include <ctime>
-#include <sstream>
-
-#ifndef WIN32
-/* The classes below are exported */
-#pragma GCC visibility push(default)
-#endif
-
-// Generic stopwatch exception class
-struct StopwatchException
-{
-public:
-  StopwatchException(std::string error) : error(error) { }
-  std::string error;
-};
-
-
-enum StopwatchMode
-{
-  NONE      = 0,  // Clock is not initialized
-  CPU_TIME  = 1,  // Clock calculates time ranges using ctime and CLOCKS_PER_SEC
-  REAL_TIME = 2   // Clock calculates time by asking the operating system how
-  // much real time passed
-};
-
-#define STOP_WATCH_MAX_NAME_LENGTH 80
-
-/**
-    @brief A class representing a stopwatch.
-    
-    @code
-    Stopwatch swatch();
-    @endcode
-    
-    The Stopwatch class can be used to measure execution time of code,
-    algorithms, etc., // TODO: he Stopwatch can be initialized in two
-    time-taking modes, CPU time and real time:
-
-    @code
-    swatch.set_mode(REAL_TIME);
-    @endcode
-    
-    CPU time is the time spent by the processor on a certain piece of code,
-    while real time is the real amount of time taken by a certain piece of
-    code to execute (i.e. in general if you are doing hard work such as
-    image or video editing on a different process the measured time will
-    probably increase).
-    
-    How does it work? Basically, one wraps the code to be measured with the
-    following method calls:
-
-    @code
-    swatch.start("My astounding algorithm");
-    // Hic est code
-    swatch.stop("My astounding algorithm");
-    @endcode
-    
-    A string representing the code ID is provided so that nested portions of
-    code can be profiled separately:
-
-    @code
-    swatch.start("My astounding algorithm");
-    
-    swatch.start("My astounding algorithm - Super smart init");
-    // Initialization
-    swatch.stop("My astounding algorithm - Super smart init");
-    
-    swatch.start("My astounding algorithm - Main loop");
-    // Loop
-    swatch.stop("My astounding algorithm - Main loop");
-    
-    swatch.stop("My astounding algorithm");
-    @endcode
-    
-    Note: ID strings can be whatever you like, in the previous example I have
-    used "My astounding algorithm - *" only to enforce the fact that the
-    measured code portions are part of My astounding algorithm, but there's no
-    connection between the three measurements.
-
-    If the code for a certain task is scattered through different files or
-    portions of the same file one can use the start-pause-stop method:
-
-    @code
-    swatch.start("Setup");
-    // First part of setup
-    swatch.pause("Setup");
-    
-    swatch.start("Main logic");
-    // Main logic
-    swatch.stop("Main logic");
-    
-    swatch.start("Setup");
-    // Cleanup (part of the setup)
-    swatch.stop("Setup");
-    @endcode
-    
-    Finally, to report the results of the measurements just run:
-    
-    @code
-    swatch.report("Code ID");
-    @endcode
-    
-    Thou can also provide an additional std::ostream& parameter to report() to
-    redirect the logging on a different output. Also, you can use the
-    get_total/min/max/average_time() methods to get the individual numeric data,
-    without all the details of the logging. You can also extend Stopwatch to
-    implement your own logging syntax.
-
-    To report all the measurements:
-    
-    @code
-    swatch.report_all();
-    @endcode
-    
-    Same as above, you can redirect the output by providing a std::ostream&
-    parameter.
-
-*/
-class Stopwatch {
-public:
-
-  /** Constructor */
-  Stopwatch(StopwatchMode _mode=NONE);
-
-  /** Destructor */
-  ~Stopwatch();
-
-  /** Tells if a performance with a certain ID exists */
-  bool performance_exists(std::string perf_name);
-
-  /** Initialize stopwatch to use a certain time taking mode */
-  void set_mode(StopwatchMode mode);
-
-  /** Start the stopwatch related to a certain piece of code */
-  void start(std::string perf_name);
-
-  /** Stops the stopwatch related to a certain piece of code */
-  void stop(std::string perf_name);
-
-  /** Stops the stopwatch related to a certain piece of code */
-  void pause(std::string perf_name);
-
-  /** Reset a certain performance record */
-  void reset(std::string perf_name);
-
-  /** Resets all the performance records */
-  void reset_all();
-
-  /** Dump the data of a certain performance record */
-  void report(std::string perf_name, int precision=2,
-              std::ostream& output = std::cout);
-
-  /** Dump the data of all the performance records */
-  void report_all(int precision=2, std::ostream& output = std::cout);
-
-  /** Returns total execution time of a certain performance */
-  long double get_total_time(std::string perf_name);
-
-  /** Returns average execution time of a certain performance */
-  long double get_average_time(std::string perf_name);
-
-  /** Returns minimum execution time of a certain performance */
-  long double get_min_time(std::string perf_name);
-
-  /** Returns maximum execution time of a certain performance */
-  long double get_max_time(std::string perf_name);
-
-  /** Return last measurement of a certain performance */
-  long double get_last_time(std::string perf_name);
-
-  /** Return the time since the start of the last measurement of a given
-      performance. */
-  long double get_time_so_far(std::string perf_name);
-
-  /** Turn off clock, all the Stopwatch::* methods return without doing
-        anything after this method is called. */
-  void turn_off();
-
-  /** Turn on clock, restore clock operativity after a turn_off(). */
-  void turn_on();
-
-  /** Take time, depends on mode */
-  long double take_time();
-
-protected:
-
-  /** Struct to hold the performance data */
-  struct PerformanceData {
-
-    PerformanceData() :
-      clock_start(0),
-      total_time(0),
-      min_time(0),
-      max_time(0),
-      last_time(0),
-      paused(false),
-      stops(0) {
-    }
-
-    /** Start time */
-    long double clock_start;
-
-    /** Cumulative total time */
-    long double total_time;
-
-    /** Minimum time */
-    long double min_time;
-
-    /** Maximum time */
-    long double max_time;
-
-    /** Last time */
-    long double last_time;
-
-    /** Tells if this performance has been paused, only for internal use */
-    bool paused;
-
-    /** How many cycles have been this stopwatch executed? */
-    int stops;
-  };
-
-  /** Flag to hold the clock's status */
-  bool active;
-
-  /** Time taking mode */
-  StopwatchMode mode;
-
-  /** Pointer to the dynamic structure which holds the collection of performance
-      data */
-  std::map<std::string, PerformanceData >* records_of;
-
-};
-
-Stopwatch& getProfiler();
-
-#ifndef WIN32
-#pragma GCC visibility pop
-#endif
-
-#endif
diff --git a/python/sot_talos_balance/create_entities_utils.py b/python/sot_talos_balance/create_entities_utils.py
index c609840f209fbdb6fdf1ca9f0d952209fe7220ba..8919fa17c78bc18184d66564f380549ec5fdf9e8 100644
--- a/python/sot_talos_balance/create_entities_utils.py
+++ b/python/sot_talos_balance/create_entities_utils.py
@@ -1,11 +1,10 @@
-from sot_talos_balance.control_manager import ControlManager
+from sot_talos_balance.talos_control_manager import TalosControlManager
 from sot_talos_balance.example import Example
-from sot_talos_balance.parameter_server import ParameterServer
+from dynamic_graph.sot.core.parameter_server import ParameterServer
 from dynamic_graph.tracer_real_time import TracerRealTime
 from time import sleep
-from sot_talos_balance.base_estimator import BaseEstimator
-from sot_talos_balance.madgwickahrs import MadgwickAHRS
-from sot_talos_balance.imu_offset_compensation import ImuOffsetCompensation
+from sot_talos_balance.talos_base_estimator import TalosBaseEstimator
+from dynamic_graph.sot.core.madgwickahrs import MadgwickAHRS
 from sot_talos_balance.dcm_estimator import DcmEstimator
 from sot_talos_balance.ft_calibration import FtCalibration
 from sot_talos_balance.ft_wrist_calibration import FtWristCalibration
@@ -25,7 +24,7 @@ from sot_talos_balance.nd_trajectory_generator import NdTrajectoryGenerator
 from sot_talos_balance.simple_pid import SimplePID
 from sot_talos_balance.simple_pidd import SimplePIDD
 from sot_talos_balance.joint_position_controller import JointPositionController
-from sot_talos_balance.admittance_controller import AdmittanceController
+from sot_talos_balance.simple_admittance_controller import SimpleAdmittanceController
 from sot_talos_balance.admittance_controller_end_effector import AdmittanceControllerEndEffector
 from sot_talos_balance.dummy_dcm_estimator import DummyDcmEstimator
 from sot_talos_balance.com_admittance_controller import ComAdmittanceController
@@ -200,12 +199,6 @@ def create_joint_admittance_controller(joint, Kp, dt, robot, filter=False):
     return controller
 
 
-def create_imu_offset_compensation(robot, dt):
-    imu_offset_compensation = ImuOffsetCompensation('imu_offset_comp')
-    imu_offset_compensation.init(dt)
-    plug(robot.device.accelerometer, imu_offset_compensation.accelerometer_in)
-    plug(robot.device.gyrometer,   imu_offset_compensation.gyrometer_in)
-    return imu_offset_compensation
 
 
 def create_device_filters(robot, dt):
@@ -232,10 +225,7 @@ def create_device_filters(robot, dt):
     plug(robot.device.ptorque,                            filters.torque_filter.x)
     plug(robot.vselec.sout,                               filters.vel_filter.x)
 
-    # switch following lines if willing to use imu offset compensation
-    #~ plug(robot.imu_offset_compensation.accelerometer_out, filters.acc_filter.x);
     plug(robot.device.accelerometer, filters.acc_filter.x)
-    #~ plug(robot.imu_offset_compensation.gyrometer_out,     filters.gyro_filter.x);
     plug(robot.device.gyrometer,     filters.gyro_filter.x)
 
     return filters
@@ -249,7 +239,7 @@ def create_be_filters(robot, dt):
 
 
 def create_ctrl_manager(conf, dt, robot_name='robot'):
-    ctrl_manager = ControlManager("ctrl_man")
+    ctrl_manager = TalosControlManager("ctrl_man")
     ctrl_manager.init(dt, robot_name)
     ctrl_manager.u_max.value = conf.NJ*(conf.CTRL_MAX,)
     # Init should be called before addCtrlMode
@@ -258,7 +248,7 @@ def create_ctrl_manager(conf, dt, robot_name='robot'):
 
 
 def create_base_estimator(robot, dt, conf, robot_name="robot"):
-    base_estimator = BaseEstimator('base_estimator')
+    base_estimator = TalosBaseEstimator('base_estimator')
     base_estimator.init(dt, robot_name)
     # device.state, device.joint_angles or device.motor_angles ?
     plug(robot.device.joint_angles,                      base_estimator.joint_positions)
diff --git a/python/sot_talos_balance/utils/filter_utils.py b/python/sot_talos_balance/utils/filter_utils.py
index f32387442d4bd5544744c19d6150c56f750b7bdb..e295b4013b217daeaa0a3a5cfcc390995ae817dd 100644
--- a/python/sot_talos_balance/utils/filter_utils.py
+++ b/python/sot_talos_balance/utils/filter_utils.py
@@ -1,4 +1,4 @@
-from sot_talos_balance.filter_differentiator import FilterDifferentiator
+from dynamic_graph.sot.core.filter_differentiator import FilterDifferentiator
 import numpy as np
 
 def create_chebi1_checby2_series_filter(name, dt, size):
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 8a76c01f186488754ef520a37f1d79bde693c11e..0c71ee73813899f3c5b84a01a3374c1c70d0340d 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -52,16 +52,12 @@ SET(plugins
     dcm-controller
     dummy-dcm-estimator
     com-admittance-controller
-    admittance-controller
+    simple-admittance-controller
     admittance-controller-end-effector
     joint-position-controller
     nd-trajectory-generator
-    base-estimator
-    filter-differentiator
-    madgwickahrs
-    control-manager
-    imu_offset_compensation
-    parameter-server
+    talos-base-estimator
+    talos-control-manager
     dcm-estimator
     qualisys-client
     ft-calibration
@@ -98,7 +94,7 @@ FOREACH(plugin ${plugins})
 
   ADD_DEPENDENCIES(${LIBRARY_NAME} ${SOTTALOSBALANCE_LIB_NAME})
 
-  
+
   IF(ADDITIONAL_${LIBRARY_NAME}_LIBS)
     ADD_DEPENDENCIES(${LIBRARY_NAME} ${ADDITIONAL_${LIBRARY_NAME}_LIBS})
     TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${ADDITIONAL_${LIBRARY_NAME}_LIBS})
@@ -128,8 +124,10 @@ FOREACH(plugin ${plugins})
       )
   ENDIF(BUILD_PYTHON_INTERFACE)
   # Install plugins
-  INSTALL(TARGETS ${LIBRARY_NAME}
-    DESTINATION ${CMAKE_INSTALL_LIBDIR}/plugin)
+  IF (NOT INSTALL_PYTHON_INTERFACE_ONLY)
+    INSTALL(TARGETS ${LIBRARY_NAME}
+      DESTINATION ${CMAKE_INSTALL_LIBDIR}/plugin)
+  ENDIF (NOT INSTALL_PYTHON_INTERFACE_ONLY)
 ENDFOREACH(plugin)
 
 # Bindings Python
diff --git a/src/admittance-controller-end-effector.cpp b/src/admittance-controller-end-effector.cpp
index 4a8c42a66e1d9ecc049612a6630333cadccb2f16..572608e59fb9452fdf381b2f0b88601d92af7e3d 100644
--- a/src/admittance-controller-end-effector.cpp
+++ b/src/admittance-controller-end-effector.cpp
@@ -12,7 +12,7 @@
 #include <sot/core/debug.hh>
 #include <dynamic-graph/factory.h>
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 namespace dynamicgraph
 {
diff --git a/src/com-admittance-controller.cpp b/src/com-admittance-controller.cpp
index d335eab12624568268598c740e9fae4d67bf6c65..48b33506a743b6929f0256ea3a0fc673f121c597 100644
--- a/src/com-admittance-controller.cpp
+++ b/src/com-admittance-controller.cpp
@@ -21,7 +21,7 @@
 #include <dynamic-graph/command-bind.h>
 
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 namespace dynamicgraph
 {
diff --git a/src/dcm-com-controller.cpp b/src/dcm-com-controller.cpp
index bb56c47f8cc24c2574e4829b07e985ab7cb803cd..08841b415054bffebc66a048b245e98a9112a510 100644
--- a/src/dcm-com-controller.cpp
+++ b/src/dcm-com-controller.cpp
@@ -21,7 +21,7 @@
 #include <dynamic-graph/command-bind.h>
 
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 namespace dynamicgraph
 {
diff --git a/src/dcm-controller.cpp b/src/dcm-controller.cpp
index 7ab50976168e044acb75c28223dc4ba92d83c866..94b9eb49aec7f31325be4da3a11ee4ae2d3851ba 100644
--- a/src/dcm-controller.cpp
+++ b/src/dcm-controller.cpp
@@ -21,7 +21,7 @@
 #include <dynamic-graph/command-bind.h>
 
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 namespace dynamicgraph
 {
diff --git a/src/dcm-estimator.cpp b/src/dcm-estimator.cpp
index 4821d1cd49514d0f7a3c4fde9a75ec2fd9448578..010136a0215588afbedc5dc5b2d455b934eafb36 100644
--- a/src/dcm-estimator.cpp
+++ b/src/dcm-estimator.cpp
@@ -11,7 +11,7 @@
 #include <sot/core/debug.hh>
 #include <dynamic-graph/all-commands.h>
 #include <dynamic-graph/factory.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
 #include "pinocchio/algorithm/frames.hpp"
 #include "pinocchio/algorithm/center-of-mass.hpp"
 
diff --git a/src/distribute-wrench.cpp b/src/distribute-wrench.cpp
index cfbb8b23fc77ec1140cae091a131214281999ec8..62afb896cc09c3c9806a19c7021a330a490268df 100644
--- a/src/distribute-wrench.cpp
+++ b/src/distribute-wrench.cpp
@@ -23,7 +23,7 @@
 #include <dynamic-graph/command-bind.h>
 
 #include "sot/talos_balance/utils/commands-helper.hh"
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include "sot/core/stop-watch.hh"
 
 #include <pinocchio/parsers/urdf.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
diff --git a/src/dummy-dcm-estimator.cpp b/src/dummy-dcm-estimator.cpp
index 8bc2110211a9dbbe303c000c2cb4fbf494ae19b9..aaedb0734f7abacb3d7ec73bea9948c9c56e2f9e 100644
--- a/src/dummy-dcm-estimator.cpp
+++ b/src/dummy-dcm-estimator.cpp
@@ -21,7 +21,7 @@
 #include <dynamic-graph/command-bind.h>
 
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 namespace dynamicgraph
 {
diff --git a/src/dummy-walking-pattern-generator.cpp b/src/dummy-walking-pattern-generator.cpp
index 1e2c72a917a7cc6ee8aed90d553db2d7ddca4559..34a8681ac2d5039e6c6ff438eaa82489113f824a 100644
--- a/src/dummy-walking-pattern-generator.cpp
+++ b/src/dummy-walking-pattern-generator.cpp
@@ -21,7 +21,7 @@
 #include <dynamic-graph/command-bind.h>
 
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 namespace dynamicgraph
 {
diff --git a/src/euler-to-quat.cpp b/src/euler-to-quat.cpp
index 5d5361f16fc1651156898395486562813fe9caa4..54918809203a20472a28002a58654c5180973c04 100644
--- a/src/euler-to-quat.cpp
+++ b/src/euler-to-quat.cpp
@@ -19,7 +19,7 @@
 #include <sot/core/debug.hh>
 #include <dynamic-graph/factory.h>
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 #include <Eigen/Core>
 
diff --git a/src/example.cpp b/src/example.cpp
index 94b380f0a29930238d3b8373044ed4e4b8483284..708a1a86c26441fc89d7d5b1d2bfe6d6ebe5ea8a 100644
--- a/src/example.cpp
+++ b/src/example.cpp
@@ -19,7 +19,7 @@
 #include <sot/core/debug.hh>
 #include <dynamic-graph/factory.h>
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 
 namespace dynamicgraph
diff --git a/src/filter-differentiator.cpp b/src/filter-differentiator.cpp
deleted file mode 100644
index c98891538863b30d94b5b3f270b9c3b504f67e3b..0000000000000000000000000000000000000000
--- a/src/filter-differentiator.cpp
+++ /dev/null
@@ -1,189 +0,0 @@
-/*
- * Copyright 2017-, Rohan Budhiraja LAAS-CNRS
- *
- * This file is part of sot-torque-control.
- * sot-torque-control is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-torque-control is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.  You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#define LOGFILE "/tmp/fd_log.dat"
-
-#define LOG(x) { std::ofstream LogFile; \
-    LogFile.open(LOGFILE,std::ofstream::app); \
-    LogFile << x << std::endl; \
-    LogFile.close();}
-
-
-#include <sot/talos_balance/filter-differentiator.hh>
-#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
-#include <dynamic-graph/all-commands.h>
-//#include <sot/torque_control/motor-model.hh>
-#include <Eigen/Dense>
-
-namespace dynamicgraph
-{
-  namespace sot
-  {
-    namespace talos_balance
-    {
-
-#define ALL_INPUT_SIGNALS m_xSIN
-
-#define ALL_OUTPUT_SIGNALS  m_x_filteredSOUT << m_dxSOUT << m_ddxSOUT
-
-      namespace dynamicgraph = ::dynamicgraph;
-      using namespace dynamicgraph;
-      using namespace dynamicgraph::command;
-      using namespace Eigen;
-
-      /// Define EntityClassName here rather than in the header file
-      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
-      typedef FilterDifferentiator EntityClassName;
-
-      /* --- DG FACTORY ------------------------------------------------------- */
-      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FilterDifferentiator,"FilterDifferentiator");
-
-      /* --- CONSTRUCTION ----------------------------------------------------- */
-      /* --- CONSTRUCTION ----------------------------------------------------- */
-      /* --- CONSTRUCTION ----------------------------------------------------- */
-      FilterDifferentiator::
-      FilterDifferentiator( const std::string & name )
-        : Entity(name),
-          CONSTRUCT_SIGNAL_IN(x,                dynamicgraph::Vector)
-        ,CONSTRUCT_SIGNAL_OUT(x_filtered,       dynamicgraph::Vector, m_x_dx_ddxSINNER)
-        ,CONSTRUCT_SIGNAL_OUT(dx,               dynamicgraph::Vector, m_x_dx_ddxSINNER)
-        ,CONSTRUCT_SIGNAL_OUT(ddx,               dynamicgraph::Vector, m_x_dx_ddxSINNER)
-        ,CONSTRUCT_SIGNAL_INNER(x_dx_ddx,       dynamicgraph::Vector, m_xSIN)
-      {
-        Entity::signalRegistration( ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
-
-        /* Commands. */
-        addCommand("getTimestep",
-                   makeDirectGetter(*this,&m_dt,
-                                    docDirectGetter("Control timestep [s ]","double")));
-        addCommand("getSize",
-                   makeDirectGetter(*this,&m_x_size,
-                                    docDirectGetter("Size of the x signal","int")));
-        addCommand("init", makeCommandVoid4(*this, &FilterDifferentiator::init,
-                              docCommandVoid4("Initialize the filter.",
-                                              "Control timestep [s].",
-                                              "Size of the input signal x",
-                                              "Numerator of the filter",
-                                              "Denominator of the filter")));
-        addCommand("switch_filter",
-                   makeCommandVoid2(*this, &FilterDifferentiator::switch_filter,
-                                    docCommandVoid2("Switch Filter.",
-                                                    "Numerator of the filter",
-                                                    "Denominator of the filter")));
-
-      }
-
-
-      /* --- COMMANDS ---------------------------------------------------------- */
-      /* --- COMMANDS ---------------------------------------------------------- */
-      /* --- COMMANDS ---------------------------------------------------------- */
-      void FilterDifferentiator::init(const double &timestep,
-                                      const int& xSize,
-                                      const Eigen::VectorXd& filter_numerator,
-                                      const Eigen::VectorXd& filter_denominator)
-      {
-        m_x_size = xSize;
-        m_dt = timestep;
-        m_filter = new CausalFilter(timestep, xSize,
-                                    filter_numerator, filter_denominator);
-
-        LOG("Filtering started with "<<
-            "Numerator "<< filter_numerator<<std::endl<<
-            "Denominator"<<filter_denominator<<std::endl);
-        return;
-      }
-
-      void FilterDifferentiator::switch_filter(const Eigen::VectorXd& filter_numerator,
-                                               const Eigen::VectorXd& filter_denominator)
-      {
-        LOG("Filter switched with "<<
-            "Numerator "<< filter_numerator<<std::endl<<
-            "Denominator"<<filter_denominator<<std::endl<<
-            "at time"<<m_xSIN.getTime());
-        m_filter->switch_filter(filter_numerator, filter_denominator);
-      }
-      
-
-      /* --- SIGNALS ---------------------------------------------------------- */
-      /* --- SIGNALS ---------------------------------------------------------- */
-      /* --- SIGNALS ---------------------------------------------------------- */
-
-      DEFINE_SIGNAL_INNER_FUNCTION(x_dx_ddx, dynamicgraph::Vector)
-      {
-        sotDEBUG(15)<<"Compute x_dx inner signal "<<iter<<std::endl;
-        if(s.size()!=3*m_x_size)
-          s.resize(3*m_x_size);
-        // read encoders
-        const dynamicgraph::Vector& base_x = m_xSIN(iter);
-        assert(base_x.size() == m_x_size);
-        m_filter->get_x_dx_ddx(base_x, s);
-        return s;
-      }
-
-
-      /// ************************************************************************* ///
-      /// The following signals depend only on other inner signals, so they
-      /// just need to copy the interested part of the inner signal they depend on.
-      /// ************************************************************************* ///
-
-
-      DEFINE_SIGNAL_OUT_FUNCTION(x_filtered, dynamicgraph::Vector)
-      {
-        sotDEBUG(15)<<"Compute x_filtered output signal "<<iter<<std::endl;
-
-        const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
-        if(s.size()!=m_x_size)
-          s.resize(m_x_size);
-        s = x_dx_ddx.head(m_x_size);
-        return s;
-      }
-
-      DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector)
-      {
-        sotDEBUG(15)<<"Compute dx output signal "<<iter<<std::endl;
-
-        const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
-        if(s.size()!=m_x_size)
-          s.resize(m_x_size);
-        s = x_dx_ddx.segment(m_x_size, m_x_size);
-        return s;
-      }
-
-      DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector)
-      {
-        sotDEBUG(15)<<"Compute ddx output signal "<<iter<<std::endl;
-
-        const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
-        if(s.size()!=m_x_size)
-          s.resize(m_x_size);
-        s = x_dx_ddx.tail(m_x_size);
-        return s;
-      }
-
-      void FilterDifferentiator::display( std::ostream& os ) const
-      {
-        os << "FilterDifferentiator "<<getName()<<":\n";
-        try
-        {
-          getProfiler().report_all(3, os);
-        }
-        catch (ExceptionSignal e) {}
-      }
-
-    } // namespace talos_balance
-  } // namespace sot
-} // namespace dynamicgraph
diff --git a/src/ft-calibration.cpp b/src/ft-calibration.cpp
index 5ca83ac24c381c261951836c8ed3d648c6577ee3..0bac338b93393f436a511dada752fb12a2663e30 100644
--- a/src/ft-calibration.cpp
+++ b/src/ft-calibration.cpp
@@ -12,7 +12,7 @@
 
 
 #include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
 #include <sot/talos_balance/utils/statistics.hh>
 
 #define CALIB_ITER_TIME 1000 //Iteration needed for sampling and averaging the FT sensors while calibrating
diff --git a/src/ft-wrist-calibration.cpp b/src/ft-wrist-calibration.cpp
index 7245754a11ef91d4c53f15cc7cc43f0759496a4f..1fee19802bccc3799100281ef5b9efddfeb15ea9 100644
--- a/src/ft-wrist-calibration.cpp
+++ b/src/ft-wrist-calibration.cpp
@@ -13,7 +13,7 @@
 
 
 #include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
 #include <sot/talos_balance/utils/statistics.hh>
 
 #define CALIB_ITER_TIME 1000 //Iteration needed for sampling and averaging the FT sensors while calibrating
diff --git a/src/imu_offset_compensation.cpp b/src/imu_offset_compensation.cpp
deleted file mode 100644
index fe1f799d42682268cfac1a24040ad2d9566859ea..0000000000000000000000000000000000000000
--- a/src/imu_offset_compensation.cpp
+++ /dev/null
@@ -1,235 +0,0 @@
-#include <sot/talos_balance/imu_offset_compensation.hh>
-#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
-
-#include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
-
-
-namespace dynamicgraph
-{
-  namespace sot
-  {
-    namespace talos_balance
-    {
-      namespace dg = ::dynamicgraph;
-      using namespace dg;
-      using namespace dg::command;
-      using namespace std;
-
-#define CALIBRATION_FILE_NAME "/opt/imu_calib.txt"
-
-#define PROFILE_IMU_OFFSET_COMPENSATION_COMPUTATION          "ImuOffsetCompensation computation"
-
-#define INPUT_SIGNALS     m_accelerometer_inSIN  << m_gyrometer_inSIN
-#define OUTPUT_SIGNALS    m_accelerometer_outSOUT << m_gyrometer_outSOUT
-
-      /// Define EntityClassName here rather than in the header file
-      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
-      typedef ImuOffsetCompensation EntityClassName;
-
-      /* --- DG FACTORY ---------------------------------------------------- */
-      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ImuOffsetCompensation,
-                                         "ImuOffsetCompensation");
-
-      /* ------------------------------------------------------------------- */
-      /* --- CONSTRUCTION -------------------------------------------------- */
-      /* ------------------------------------------------------------------- */
-      ImuOffsetCompensation::ImuOffsetCompensation(const std::string& name)
-        : Entity(name)
-        ,CONSTRUCT_SIGNAL_IN( accelerometer_in, dynamicgraph::Vector)
-        ,CONSTRUCT_SIGNAL_IN( gyrometer_in,     dynamicgraph::Vector)
-        ,CONSTRUCT_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector, m_accelerometer_inSIN)
-        ,CONSTRUCT_SIGNAL_OUT(gyrometer_out,     dynamicgraph::Vector, m_gyrometer_inSIN)
-        ,m_initSucceeded(false)
-        ,m_dt(0.001)
-        ,m_update_cycles_left(0)
-        ,m_update_cycles(0)
-        ,m_a_gyro_DC_blocker(1.0)
-      {
-        Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
-
-        m_gyro_offset.setZero();
-        m_acc_offset.setZero();
-        m_gyro_sum.setZero();
-        m_acc_sum.setZero();
-
-        /* Commands. */
-        addCommand("init",
-                   makeCommandVoid1(*this, &ImuOffsetCompensation::init,
-                                    docCommandVoid1("Initialize the entity.",
-                                                    "Timestep in seconds (double)")));
-        addCommand("update_offset",
-                   makeCommandVoid1(*this, &ImuOffsetCompensation::update_offset,
-                                    docCommandVoid1("Update the IMU offsets.",
-                                                    "Duration of the update phase in seconds (double)")));
-        addCommand("setGyroDCBlockerParameter",
-                   makeCommandVoid1(*this, &ImuOffsetCompensation::setGyroDCBlockerParameter,
-                                    docCommandVoid1("Set DC Blocker filter parameter.",
-                                                    "alpha (double)")));
-
-
-      }
-
-      /* ------------------------------------------------------------------- */
-      /* --- COMMANDS ------------------------------------------------------ */
-      /* ------------------------------------------------------------------- */
-
-      void ImuOffsetCompensation::init(const double& dt)
-      {
-        if(dt<=0.0)
-          return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
-        m_dt = dt;
-        m_initSucceeded = true;
-
-        // try to read IMU calibration data from file
-        std::ifstream infile;
-        infile.open(CALIBRATION_FILE_NAME, std::ios::in);
-        if(!infile.is_open())
-          return SEND_MSG("Error trying to read calibration results from file "+toString(CALIBRATION_FILE_NAME), MSG_TYPE_ERROR);
-
-        double z=0;
-        int i=0;
-        while(infile>>z)
-        {
-          m_gyro_offset(i) = z;
-          i++;
-          if(i==3)
-            break;
-        }
-        if(i!=3)
-        {
-          m_gyro_offset.setZero();
-          return SEND_MSG("Error trying to read gyro offset from file "+toString(CALIBRATION_FILE_NAME)+". Not enough values: "+toString(i), MSG_TYPE_ERROR);
-        }
-
-        i=0;
-        while(infile>>z)
-        {
-          m_acc_offset(i) = z;
-          i++;
-          if(i==3)
-            break;
-        }
-        if(i!=3)
-        {
-          m_gyro_offset.setZero();
-          m_acc_offset.setZero();
-          return SEND_MSG("Error trying to read acc offset from file "+toString(CALIBRATION_FILE_NAME)+". Not enough values: "+toString(i), MSG_TYPE_ERROR);
-        }
-
-        SEND_MSG("Offset read finished:\n* acc offset: "+toString(m_acc_offset.transpose())+
-                 "\n* gyro offset: "+toString(m_gyro_offset.transpose()), MSG_TYPE_INFO);
-      }
-
-      void ImuOffsetCompensation::setGyroDCBlockerParameter(const double & alpha)
-      {
-        if(alpha>1.0 || alpha<=0.0)
-          return SEND_MSG("GyroDCBlockerParameter must be > 0 and <= 1", MSG_TYPE_ERROR);
-        m_a_gyro_DC_blocker = alpha;
-      }
-
-      void ImuOffsetCompensation::update_offset(const double& duration)
-      {
-        if(duration<m_dt)
-          return SEND_MSG("Duration must be greater than the time step", MSG_TYPE_ERROR);
-        m_update_cycles = int(duration/m_dt);
-        m_update_cycles_left = m_update_cycles;
-      }
-
-      void ImuOffsetCompensation::update_offset_impl(int iter)
-      {
-        const dynamicgraph::Vector& accelerometer = m_accelerometer_inSIN(iter);
-        const dynamicgraph::Vector& gyrometer = m_gyrometer_inSIN(iter);
-        m_acc_sum  += accelerometer;
-        m_gyro_sum += gyrometer;
-
-        m_update_cycles_left--;
-        if(m_update_cycles_left==0)
-        {
-          Vector3 g, new_acc_offset, new_gyro_offset;
-          g<<0.0, 0.0, 9.81;
-          new_acc_offset  = (m_acc_sum /m_update_cycles) - g;
-          new_gyro_offset = m_gyro_sum/m_update_cycles;
-          m_acc_sum.setZero();
-          m_gyro_sum.setZero();
-          SEND_MSG("Offset computation finished:"+
-                   ("\n* old acc offset: "+toString(m_acc_offset.transpose()))+
-                   "\n* new acc offset: "+toString(new_acc_offset.transpose())+
-                   "\n* old gyro offset: "+toString(m_gyro_offset.transpose())+
-                   "\n* new gyro offset: "+toString(new_gyro_offset.transpose()), MSG_TYPE_INFO);
-          m_acc_offset = new_acc_offset;
-          m_gyro_offset = new_gyro_offset;
-
-          // save to text file
-          ofstream aof(CALIBRATION_FILE_NAME);
-          if(!aof.is_open())
-            return SEND_MSG("Error trying to save calibration results on file "+toString(CALIBRATION_FILE_NAME), MSG_TYPE_ERROR);
-
-          for(unsigned long int i=0;i<3;i++)
-            aof << m_gyro_offset[i] << " " ;
-          aof << std::endl;
-          for(unsigned long int i=0;i<3;i++)
-            aof << m_acc_offset[i] << " " ;
-          aof << std::endl;
-          aof.close();
-          SEND_MSG("IMU calibration data saved to file "+toString(CALIBRATION_FILE_NAME), MSG_TYPE_INFO);
-        }
-      }
-
-      /* ------------------------------------------------------------------- */
-      /* --- SIGNALS ------------------------------------------------------- */
-      /* ------------------------------------------------------------------- */
-
-      DEFINE_SIGNAL_OUT_FUNCTION(accelerometer_out, dynamicgraph::Vector)
-      {
-        if(!m_initSucceeded)
-        {
-          SEND_WARNING_STREAM_MSG("Cannot compute signal accelerometer before initialization!");
-          return s;
-        }
-
-        if(m_update_cycles_left>0)
-          update_offset_impl(iter);
-
-        const dynamicgraph::Vector& accelerometer = m_accelerometer_inSIN(iter);
-        if(s.size()!=3)
-          s.resize(3);
-        s = accelerometer - m_acc_offset;
-        return s;
-      }
-
-      DEFINE_SIGNAL_OUT_FUNCTION(gyrometer_out, dynamicgraph::Vector)
-      {
-        if(!m_initSucceeded)
-        {
-          SEND_WARNING_STREAM_MSG("Cannot compute signal gyrometer before initialization!");
-          return s;
-        }
-        const dynamicgraph::Vector& gyrometer = m_gyrometer_inSIN(iter);
-        if(s.size()!=3)
-          s.resize(3);
-        //estimate bias online with the assumption that average angular velocity should be zero.
-        if (m_a_gyro_DC_blocker !=1.0)
-            m_gyro_offset = m_gyro_offset*m_a_gyro_DC_blocker + (1.-m_a_gyro_DC_blocker)*gyrometer;
-        s = gyrometer - m_gyro_offset;
-        return s;
-      }
-
-
-      /* ------------------------------------------------------------------- */
-      /* --- ENTITY -------------------------------------------------------- */
-      /* ------------------------------------------------------------------- */
-
-      void ImuOffsetCompensation::display(std::ostream& os) const
-      {
-        os << "ImuOffsetCompensation "<<getName();
-        try
-        {
-          getProfiler().report_all(3, os);
-        }
-        catch (ExceptionSignal e) {}
-      }
-    } // namespace talos_balance
-  } // namespace sot
-} // namespace dynamicgraph
diff --git a/src/joint-position-controller.cpp b/src/joint-position-controller.cpp
index 72c0e6b788b75883f26b3bd58cbb78f51b3d578a..2a81e0be3e2fdbaa7f384f53aeac8a42d6e39d3d 100644
--- a/src/joint-position-controller.cpp
+++ b/src/joint-position-controller.cpp
@@ -21,7 +21,7 @@
 #include <dynamic-graph/command-bind.h>
 
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 namespace dynamicgraph
 {
diff --git a/src/madgwickahrs.cpp b/src/madgwickahrs.cpp
deleted file mode 100644
index 2b9f00b5f0ec7ceb44401f54ad56b8cfed6aa0f8..0000000000000000000000000000000000000000
--- a/src/madgwickahrs.cpp
+++ /dev/null
@@ -1,245 +0,0 @@
-//=====================================================================================================
-//
-// Implementation of Madgwick's IMU and AHRS algorithms.
-// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
-//
-// Date       Author       Notes
-// 29/09/2011 SOH Madgwick Initial release
-// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
-// 11/05/2017   T Flayols  Make it a dynamic-graph entity
-//
-//=====================================================================================================
-
-
-
-#include <sot/talos_balance/madgwickahrs.hh>
-#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
-
-#include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
-
-
-namespace dynamicgraph
-{
-  namespace sot
-  {
-    namespace talos_balance
-    {
-      namespace dg = ::dynamicgraph;
-      using namespace dg;
-      using namespace dg::command;
-      using namespace std;
-
-      typedef Vector6d Vector6;
-
-#define PROFILE_MADGWICKAHRS_COMPUTATION          "MadgwickAHRS computation"
-
-#define INPUT_SIGNALS     m_accelerometerSIN << m_gyroscopeSIN
-#define OUTPUT_SIGNALS    m_imu_quatSOUT
-
-      /// Define EntityClassName here rather than in the header file
-      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
-      typedef MadgwickAHRS EntityClassName;
-
-      /* --- DG FACTORY ---------------------------------------------------- */
-      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MadgwickAHRS,
-                                         "MadgwickAHRS");
-
-      /* ------------------------------------------------------------------- */
-      /* --- CONSTRUCTION -------------------------------------------------- */
-      /* ------------------------------------------------------------------- */
-      MadgwickAHRS::MadgwickAHRS(const std::string& name)
-        : Entity(name)
-        ,CONSTRUCT_SIGNAL_IN( accelerometer,            dynamicgraph::Vector)
-        ,CONSTRUCT_SIGNAL_IN( gyroscope,                dynamicgraph::Vector)
-        ,CONSTRUCT_SIGNAL_OUT(imu_quat,                 dynamicgraph::Vector, m_gyroscopeSIN <<
-                                                                              m_accelerometerSIN)
-        ,m_initSucceeded(false)
-        ,m_beta(betaDef)
-        ,m_q0(1.0)
-        ,m_q1(0.0)
-        ,m_q2(0.0)
-        ,m_q3(0.0)
-        ,m_sampleFreq(512.0)
-      {
-        Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
-
-        /* Commands. */
-        addCommand("init",
-                   makeCommandVoid1(*this, &MadgwickAHRS::init,
-                                    docCommandVoid1("Initialize the entity.",
-                                                    "Timestep in seconds (double)")));
-        addCommand("getBeta",
-                   makeDirectGetter(*this, &m_beta,
-                                    docDirectGetter("Beta parameter", "double")));
-        addCommand("setBeta",
-                   makeCommandVoid1(*this, &MadgwickAHRS::set_beta,
-                                    docCommandVoid1("Set the filter parameter beta", "double")));
-      }
-
-      void MadgwickAHRS::init(const double& dt)
-      {
-        if(dt<=0.0)
-          return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
-        m_sampleFreq=1.0/dt;
-        m_initSucceeded = true;
-      }
-
-      void MadgwickAHRS::set_beta(const double& beta)
-      {
-        if(beta<0.0 || beta>1.0)
-          return SEND_MSG("Beta must be in [0,1]", MSG_TYPE_ERROR);
-        m_beta = beta;
-      }
-
-      /* ------------------------------------------------------------------- */
-      /* --- SIGNALS ------------------------------------------------------- */
-      /* ------------------------------------------------------------------- */
-
-      DEFINE_SIGNAL_OUT_FUNCTION(imu_quat, dynamicgraph::Vector)
-      {
-        if(!m_initSucceeded)
-        {
-          SEND_WARNING_STREAM_MSG("Cannot compute signal imu_quat before initialization!");
-          return s;
-        }
-        const dynamicgraph::Vector& accelerometer = m_accelerometerSIN(iter);
-        const dynamicgraph::Vector& gyroscope = m_gyroscopeSIN(iter);
-
-        getProfiler().start(PROFILE_MADGWICKAHRS_COMPUTATION);
-        {
-          // Update state with new measurment
-          madgwickAHRSupdateIMU(     gyroscope(0),     gyroscope(1),     gyroscope(2),
-                                     accelerometer(0), accelerometer(1), accelerometer(2));
-          if(s.size()!=4)
-            s.resize(4);
-          s(0) = m_q0;
-          s(1) = m_q1;
-          s(2) = m_q2;
-          s(3) = m_q3;
-        }
-        getProfiler().stop(PROFILE_MADGWICKAHRS_COMPUTATION);
-
-        return s;
-      }
-
-
-      /* --- COMMANDS ---------------------------------------------------------- */
-
-      /* ------------------------------------------------------------------- */
-      // ************************ PROTECTED MEMBER METHODS ********************
-      /* ------------------------------------------------------------------- */
-
-      // Fast inverse square-root
-      // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
-      double MadgwickAHRS::invSqrt(double x)
-      {
-        /*
-          float halfx = 0.5f * x;
-          float y = x;
-          long i = *(long*)&y;
-          i = 0x5f3759df - (i>>1);
-          y = *(float*)&i;
-          y = y * (1.5f - (halfx * y * y));
-          return y;*/
-        return (1.0/sqrt(x)); //we're not in the 70's
-      }
-
-      // IMU algorithm update
-      void MadgwickAHRS::madgwickAHRSupdateIMU(double gx, double gy, double gz, double ax, double ay, double az)
-      {
-        double recipNorm;
-        double s0, s1, s2, s3;
-        double qDot1, qDot2, qDot3, qDot4;
-        double _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
-
-        // Rate of change of quaternion from gyroscope
-        qDot1 = 0.5 * (-m_q1 * gx - m_q2 * gy - m_q3 * gz);
-        qDot2 = 0.5 * ( m_q0 * gx + m_q2 * gz - m_q3 * gy);
-        qDot3 = 0.5 * ( m_q0 * gy - m_q1 * gz + m_q3 * gx);
-        qDot4 = 0.5 * ( m_q0 * gz + m_q1 * gy - m_q2 * gx);
-
-        // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-        if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0)))
-        {
-          // Normalise accelerometer measurement
-          recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-          ax *= recipNorm;
-          ay *= recipNorm;
-          az *= recipNorm;
-
-          // Auxiliary variables to avoid repeated arithmetic
-          _2q0 = 2.0 * m_q0;
-          _2q1 = 2.0 * m_q1;
-          _2q2 = 2.0 * m_q2;
-          _2q3 = 2.0 * m_q3;
-          _4q0 = 4.0 * m_q0;
-          _4q1 = 4.0 * m_q1;
-          _4q2 = 4.0 * m_q2;
-          _8q1 = 8.0 * m_q1;
-          _8q2 = 8.0 * m_q2;
-          q0q0 = m_q0 * m_q0;
-          q1q1 = m_q1 * m_q1;
-          q2q2 = m_q2 * m_q2;
-          q3q3 = m_q3 * m_q3;
-
-          // Gradient decent algorithm corrective step
-          s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
-          s1 = _4q1 * q3q3 - _2q3 * ax + 4.0 * q0q0 * m_q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
-          s2 = 4.0 * q0q0 * m_q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
-          s3 = 4.0 * q1q1 * m_q3 - _2q1 * ax + 4.0 * q2q2 * m_q3 - _2q2 * ay;
-          if(!((s0 == 0.0) && (s1 == 0.0) && (s2 == 0.0) && (s3 == 0.0)))
-          {
-            recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
-            s0 *= recipNorm;
-            s1 *= recipNorm;
-            s2 *= recipNorm;
-            s3 *= recipNorm;
-
-            // Apply feedback step
-            qDot1 -= m_beta * s0;
-            qDot2 -= m_beta * s1;
-            qDot3 -= m_beta * s2;
-            qDot4 -= m_beta * s3;
-          }
-        }
-
-        // Integrate rate of change of quaternion to yield quaternion
-        m_q0 += qDot1 * (1.0 / m_sampleFreq);
-        m_q1 += qDot2 * (1.0 / m_sampleFreq);
-        m_q2 += qDot3 * (1.0 / m_sampleFreq);
-        m_q3 += qDot4 * (1.0 / m_sampleFreq);
-
-        // Normalise quaternion
-        recipNorm = invSqrt(m_q0 * m_q0 + m_q1 * m_q1 + m_q2 * m_q2 + m_q3 * m_q3);
-        m_q0 *= recipNorm;
-        m_q1 *= recipNorm;
-        m_q2 *= recipNorm;
-        m_q3 *= recipNorm;
-      }
-
-
-      /* ------------------------------------------------------------------- */
-      /* --- ENTITY -------------------------------------------------------- */
-      /* ------------------------------------------------------------------- */
-
-      void MadgwickAHRS::display(std::ostream& os) const
-      {
-        os << "MadgwickAHRS "<<getName();
-        try
-        {
-          getProfiler().report_all(3, os);
-        }
-        catch (ExceptionSignal e) {}
-      }
-    } // namespace talos_balance
-  } // namespace sot
-} // namespace dynamicgraph
-
-
-
-
-
-
-
diff --git a/src/nd-trajectory-generator.cpp b/src/nd-trajectory-generator.cpp
index 18c09557cc5396ee27cb216146642f2eda82e918..557cf9fc737057e4f7743c55b8889acfc8f5a590 100644
--- a/src/nd-trajectory-generator.cpp
+++ b/src/nd-trajectory-generator.cpp
@@ -21,7 +21,7 @@
 #include <dynamic-graph/factory.h>
 
 #include <sot/talos_balance/utils/commands-helper.hh>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
 
 namespace dynamicgraph
 {
diff --git a/src/parameter-server.cpp b/src/parameter-server.cpp
deleted file mode 100644
index 1f882af8a8cef768afdf4897a9f4b785f87716f4..0000000000000000000000000000000000000000
--- a/src/parameter-server.cpp
+++ /dev/null
@@ -1,352 +0,0 @@
-/*
- * Copyright 2014, Andrea Del Prete, LAAS-CNRS
- *
- * This file is part of sot-torque-control.
- * sot-torque-control is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-torque-control is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.  You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
- */
-#include <sot/talos_balance/robot/robot-wrapper.hh>
-
-#include <sot/talos_balance/parameter-server.hh>
-#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
-
-
-#include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
-#include <sot/talos_balance/utils/statistics.hh>
-
-
-
-using namespace sot::talos_balance;
-
-namespace dynamicgraph
-{
-  namespace sot
-  {
-    namespace talos_balance
-    {
-      namespace dynamicgraph = ::dynamicgraph;
-      using namespace dynamicgraph;
-      using namespace dynamicgraph::command;
-      using namespace std;
-      using namespace dg::sot::talos_balance;
-
-
-//Size to be aligned                          "-------------------------------------------------------"
-#define PROFILE_PWM_DESIRED_COMPUTATION       "Control manager                                        "
-#define PROFILE_DYNAMIC_GRAPH_PERIOD          "Control period                                         "
-
-#define INPUT_SIGNALS  
-#define OUTPUT_SIGNALS 
-
-      /// Define EntityClassName here rather than in the header file
-      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
-      typedef ParameterServer EntityClassName;
-
-      /* --- DG FACTORY ---------------------------------------------------- */
-      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ParameterServer,
-                                         "ParameterServer");
-
-      /* ------------------------------------------------------------------- */
-      /* --- CONSTRUCTION -------------------------------------------------- */
-      /* ------------------------------------------------------------------- */
-      ParameterServer::
-      ParameterServer(const std::string& name)
-        : Entity(name)
-        ,m_robot_util(RefVoidRobotUtil())
-        ,m_initSucceeded(false)
-        ,m_emergency_stop_triggered(false)
-        ,m_is_first_iter(true)
-        ,m_iter(0)
-        ,m_sleep_time(0.0)
-      {
-
-        //~ Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS);
-
-        /* Commands. */
-        addCommand("init",
-                   makeCommandVoid3(*this, &ParameterServer::init,
-                                    docCommandVoid3("Initialize the entity.",
-                                                    "Time period in seconds (double)",
-                                                    "URDF file path (string)",
-                                                    "Robot reference (string)")));
- 
-        addCommand("setNameToId",
-                   makeCommandVoid2(*this,&ParameterServer::setNameToId,
-                                    docCommandVoid2("Set map for a name to an Id",
-                                                    "(string) joint name",
-                                                    "(int) joint id")));
-
-        addCommand("setForceNameToForceId",
-                   makeCommandVoid2(*this,&ParameterServer::setForceNameToForceId,
-                                    docCommandVoid2("Set map from a force sensor name to a force sensor Id",
-                                                    "(string) force sensor name",
-                                                    "(int) force sensor id")));
-
-
-        addCommand("setJointLimitsFromId",
-                   makeCommandVoid3(*this,&ParameterServer::setJointLimitsFromId,
-                                    docCommandVoid3("Set the joint limits for a given joint ID",
-                                                    "(int) joint id",
-                                                    "(double) lower limit",
-                                                    "(double) uppper limit")));
-
-        addCommand("setForceLimitsFromId",
-                   makeCommandVoid3(*this,&ParameterServer::setForceLimitsFromId,
-                                    docCommandVoid3("Set the force limits for a given force sensor ID",
-                                                    "(int) force sensor id",
-                                                    "(double) lower limit",
-                                                    "(double) uppper limit")));
-
-        addCommand("setJointsUrdfToSot",
-                   makeCommandVoid1(*this, &ParameterServer::setJoints,
-                                    docCommandVoid1("Map Joints From URDF to SoT.",
-                                                    "Vector of integer for mapping")));
-
-        addCommand("setRightFootSoleXYZ",
-                   makeCommandVoid1(*this, &ParameterServer::setRightFootSoleXYZ,
-                                    docCommandVoid1("Set the right foot sole 3d position.",
-                                                    "Vector of 3 doubles")));
-        addCommand("setRightFootForceSensorXYZ",
-                   makeCommandVoid1(*this, &ParameterServer::setRightFootForceSensorXYZ,
-                                    docCommandVoid1("Set the right foot sensor 3d position.",
-                                                    "Vector of 3 doubles")));
-
-        addCommand("setFootFrameName",
-                   makeCommandVoid2(*this, &ParameterServer::setFootFrameName,
-                                    docCommandVoid2("Set the Frame Name for the Foot Name.",
-                                                    "(string) Foot name",
-                                                    "(string) Frame name")));
-        addCommand("setImuJointName",
-                   makeCommandVoid1(*this, &ParameterServer::setImuJointName,
-                                    docCommandVoid1("Set the Joint Name wich IMU is attached to.",
-                                                    "(string) Joint name")));
-        addCommand("displayRobotUtil",
-                   makeCommandVoid0(*this, &ParameterServer::displayRobotUtil,
-                                    docCommandVoid0("Display the current robot util data set.")));
-
-      }
-
-      void ParameterServer::init(const double & dt,
-                                const std::string & urdfFile,
-                                const std::string &robotRef)
-      {
-        if(dt<=0.0)
-          return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
-        m_dt = dt;
-        m_emergency_stop_triggered = false; 
-        m_initSucceeded = true;
-        vector<string> package_dirs;
-        m_robot = new robots::RobotWrapper(urdfFile, package_dirs, pinocchio::JointModelFreeFlyer());
-
-        std::string localName(robotRef);
-        if (!isNameInRobotUtil(localName))
-        {
-            m_robot_util = createRobotUtil(localName);
-        }
-        else
-        {
-            m_robot_util = getRobotUtil(localName);
-        }
-
-        m_robot_util->m_urdf_filename = urdfFile;
-
-        addCommand("getJointsUrdfToSot",
-                   makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
-                                    docDirectSetter("Display map Joints From URDF to SoT.",
-                                                    "Vector of integer for mapping")));
-
-        m_robot_util->m_nbJoints = m_robot->nv()-6;
-        //~ m_jointCtrlModes_current.resize(m_robot_util->m_nbJoints);
-        //~ m_jointCtrlModes_previous.resize(m_robot_util->m_nbJoints);
-        //~ m_jointCtrlModesCountDown.resize(m_robot_util->m_nbJoints,0);
-      }
-
-
-      /* ------------------------------------------------------------------- */
-      /* --- SIGNALS ------------------------------------------------------- */
-      /* ------------------------------------------------------------------- */
-
-  
-
-
-      /* --- COMMANDS ---------------------------------------------------------- */
-
-      void ParameterServer::setNameToId(const std::string &jointName,
-                                        const unsigned int & jointId)
-      {
-        if(!m_initSucceeded)
-        {
-          SEND_WARNING_STREAM_MSG("Cannot set joint name from joint id  before initialization!");
-          return;
-        }
-        m_robot_util->set_name_to_id(jointName,jointId);
-      }
-
-      void ParameterServer::setJointLimitsFromId(const unsigned int & jointId,
-                                                 const double & lq,
-                                                 const double & uq)
-      {
-        if(!m_initSucceeded)
-        {
-          SEND_WARNING_STREAM_MSG("Cannot set joints limits from joint id  before initialization!");
-          return;
-        }
-
-        m_robot_util->set_joint_limits_for_id((Index)jointId,lq,uq);
-      }
-
-      void ParameterServer::setForceLimitsFromId(const unsigned int & jointId,
-                                                 const dynamicgraph::Vector &lq,
-                                                 const dynamicgraph::Vector &uq)
-      {
-        if(!m_initSucceeded)
-        {
-          SEND_WARNING_STREAM_MSG("Cannot set force limits from force id  before initialization!");
-          return;
-        }
-
-        m_robot_util->m_force_util.set_force_id_to_limits((Index)jointId,lq,uq);
-      }
-
-      void ParameterServer::setForceNameToForceId(const std::string &forceName,
-                                                  const unsigned int & forceId)
-      {
-        if(!m_initSucceeded)
-        {
-          SEND_WARNING_STREAM_MSG("Cannot set force sensor name from force sensor id  before initialization!");
-          return;
-        }
-
-        m_robot_util->m_force_util.set_name_to_force_id(forceName,forceId);
-      }
-
-      void ParameterServer::setJoints(const dg::Vector & urdf_to_sot)
-      {
-        if(!m_initSucceeded)
-        {
-          SEND_WARNING_STREAM_MSG("Cannot set mapping to sot before initialization!");
-          return;
-        }
-        m_robot_util->set_urdf_to_sot(urdf_to_sot);
-      }
-
-      void ParameterServer::setRightFootSoleXYZ( const dynamicgraph::Vector &xyz)
-      {
-        if(!m_initSucceeded)
-        {
-          SEND_WARNING_STREAM_MSG("Cannot set right foot sole XYZ before initialization!");
-          return;
-        }
-
-        m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
-      }
-
-      void ParameterServer::setRightFootForceSensorXYZ(const dynamicgraph::Vector &xyz)
-      {
-        if(!m_initSucceeded)
-        {
-          SEND_WARNING_STREAM_MSG("Cannot set right foot force sensor XYZ before initialization!");
-          return;
-        }
-
-        m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ = xyz;
-      }
-
-      void ParameterServer::setFootFrameName(const std::string &FootName,
-                                             const std::string &FrameName)
-      {
-        if(!m_initSucceeded)
-        {
-          SEND_WARNING_STREAM_MSG("Cannot set foot frame name!");
-          return;
-        }
-        if (FootName=="Left")
-          m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName;
-        else if (FootName=="Right")
-          m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName;
-        else 
-          SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
-      }
-
-      void ParameterServer::setImuJointName(const std::string &JointName)
-      {
-        if(!m_initSucceeded)
-        {
-          SEND_WARNING_STREAM_MSG("Cannot set IMU joint name!");
-          return;
-        }
-        m_robot_util->m_imu_joint_name = JointName;
-      }
-
-      void ParameterServer::displayRobotUtil()
-      {
-        m_robot_util->display(std::cout);
-      }
-
-      /* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
-
-      bool ParameterServer::convertJointNameToJointId(const std::string& name, unsigned int& id)
-      {
-        // Check if the joint name exists
-        int jid = int(m_robot_util->get_id_from_name(name)); // cast needed due to bug in robot-utils
-        if (jid<0)
-        {
-          SEND_MSG("The specified joint name does not exist: "+name, MSG_TYPE_ERROR);
-          std::stringstream ss;
-          for(pinocchio::Model::JointIndex it=0; it< m_robot_util->m_nbJoints;it++)
-            ss<< m_robot_util->get_name_from_id(it) <<", ";
-          SEND_MSG("Possible joint names are: "+ss.str(), MSG_TYPE_INFO);
-          return false;
-        }
-        id = (unsigned int )jid;
-        return true;
-      }
-
-      bool ParameterServer::isJointInRange(unsigned int id, double q)
-      {
-        const JointLimits & JL = m_robot_util->get_joint_limits_from_id((Index)id);
-
-        double jl= JL.lower;
-        if(q<jl)
-        {
-          SEND_MSG("Desired joint angle "+toString(q)+" is smaller than lower limit: "+toString(jl),MSG_TYPE_ERROR);
-          return false;
-        }
-        double ju = JL.upper;
-        if(q>ju)
-        {
-          SEND_MSG("Desired joint angle "+toString(q)+" is larger than upper limit: "+toString(ju),MSG_TYPE_ERROR);
-          return false;
-        }
-        return true;
-      }
-
-
-      /* ------------------------------------------------------------------- */
-      /* --- ENTITY -------------------------------------------------------- */
-      /* ------------------------------------------------------------------- */
-
-
-      void ParameterServer::display(std::ostream& os) const
-      {
-        os << "ParameterServer "<<getName();
-        try
-        {
-          getProfiler().report_all(3, os);
-        }
-        catch (ExceptionSignal e) {}
-      }
-      
-    } // namespace talos_balance
-  } // namespace sot
-} // namespace dynamicgraph
diff --git a/src/pose-quaternion-to-matrix-homo.cpp b/src/pose-quaternion-to-matrix-homo.cpp
index e1f871331d9ec6301a0b3e047844e20697474ccf..ac98432daa98ac4fbb41cddcc7d4871f7ec2d63f 100644
--- a/src/pose-quaternion-to-matrix-homo.cpp
+++ b/src/pose-quaternion-to-matrix-homo.cpp
@@ -19,7 +19,7 @@
 #include <sot/core/debug.hh>
 #include <dynamic-graph/factory.h>
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 #include <Eigen/Core>
 
diff --git a/src/qualisys-client.cpp b/src/qualisys-client.cpp
index f61d99dd36ccdc803c2241af83c31b93b0fb5171..aace23d71b19cd4aaecb3fec851c232aedbb8371 100644
--- a/src/qualisys-client.cpp
+++ b/src/qualisys-client.cpp
@@ -11,7 +11,7 @@
 #include <dynamic-graph/factory.h>
 
 #include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
 #include <sot/talos_balance/utils/statistics.hh>
 
 // #include "RTProtocol.h"
diff --git a/src/quat-to-euler.cpp b/src/quat-to-euler.cpp
index b4cb65985c8c1c467305a19f1671114291333472..a7a15183e647cdeba63b2513191ed0e6b4a0dcc4 100644
--- a/src/quat-to-euler.cpp
+++ b/src/quat-to-euler.cpp
@@ -19,7 +19,7 @@
 #include <sot/core/debug.hh>
 #include <dynamic-graph/factory.h>
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 #include <Eigen/Core>
 
diff --git a/src/admittance-controller.cpp b/src/simple-admittance-controller.cpp
similarity index 77%
rename from src/admittance-controller.cpp
rename to src/simple-admittance-controller.cpp
index 32cd2b96930941d3769423672dfd429445e7f294..8375b064659154a137c97c0e71246f20e48af0b3 100644
--- a/src/admittance-controller.cpp
+++ b/src/simple-admittance-controller.cpp
@@ -14,14 +14,14 @@
  * with sot-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "sot/talos_balance/admittance-controller.hh"
+#include "sot/talos_balance/simple-admittance-controller.hh"
 
 #include <sot/core/debug.hh>
 #include <dynamic-graph/factory.h>
 #include <dynamic-graph/command-bind.h>
 
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 namespace dynamicgraph
 {
@@ -35,9 +35,9 @@ namespace dynamicgraph
 
 //Size to be aligned                                   "-------------------------------------------------------"
 
-#define PROFILE_ADMITTANCECONTROLLER_QREF_COMPUTATION  "AdmittanceController: qRef computation                 "
+#define PROFILE_SIMPLE_ADMITTANCECONTROLLER_QREF_COMPUTATION  "SimpleAdmittanceController: qRef computation                 "
 
-#define PROFILE_ADMITTANCECONTROLLER_DQREF_COMPUTATION "AdmittanceController: dqRef computation                "
+#define PROFILE_SIMPLE_ADMITTANCECONTROLLER_DQREF_COMPUTATION "SimpleAdmittanceController: dqRef computation                "
 
 #define INPUT_SIGNALS     m_KpSIN << m_stateSIN << m_tauSIN << m_tauDesSIN
 
@@ -45,16 +45,16 @@ namespace dynamicgraph
 
       /// Define EntityClassName here rather than in the header file
       /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
-      typedef AdmittanceController EntityClassName;
+      typedef SimpleAdmittanceController EntityClassName;
 
       /* --- DG FACTORY ---------------------------------------------------- */
-      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController,
-                                         "AdmittanceController");
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SimpleAdmittanceController,
+                                         "SimpleAdmittanceController");
 
       /* ------------------------------------------------------------------- */
       /* --- CONSTRUCTION -------------------------------------------------- */
       /* ------------------------------------------------------------------- */
-      AdmittanceController::AdmittanceController(const std::string& name)
+      SimpleAdmittanceController::SimpleAdmittanceController(const std::string& name)
           : Entity(name)
           , CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector)
           , CONSTRUCT_SIGNAL_IN(state, dynamicgraph::Vector)
@@ -67,13 +67,13 @@ namespace dynamicgraph
         Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
 
         /* Commands. */
-        addCommand("init", makeCommandVoid2(*this, &AdmittanceController::init, docCommandVoid2("Initialize the entity.","time step","Number of elements")));
-        addCommand("setPosition", makeCommandVoid1(*this, &AdmittanceController::setPosition, docCommandVoid1("Set initial reference position.","Initial position")));
+        addCommand("init", makeCommandVoid2(*this, &SimpleAdmittanceController::init, docCommandVoid2("Initialize the entity.","time step","Number of elements")));
+        addCommand("setPosition", makeCommandVoid1(*this, &SimpleAdmittanceController::setPosition, docCommandVoid1("Set initial reference position.","Initial position")));
         addCommand("useExternalState", makeDirectSetter(*this,&m_useState, docDirectSetter("use external state","bool")));
         addCommand("isUsingExternalState", makeDirectGetter(*this,&m_useState, docDirectGetter("use external state","bool")));
       }
 
-      void AdmittanceController::init(const double & dt, const unsigned & n)
+      void SimpleAdmittanceController::init(const double & dt, const unsigned & n)
       {
         if(n<1)
           return SEND_MSG("n must be at least 1", MSG_TYPE_ERROR);
@@ -90,7 +90,7 @@ namespace dynamicgraph
         m_initSucceeded = true;
       }
 
-      void AdmittanceController::setPosition(const dynamicgraph::Vector & position)
+      void SimpleAdmittanceController::setPosition(const dynamicgraph::Vector & position)
 
       {
         m_q = position;
@@ -108,7 +108,7 @@ namespace dynamicgraph
           return s;
         }
 
-        getProfiler().start(PROFILE_ADMITTANCECONTROLLER_DQREF_COMPUTATION);
+        getProfiler().start(PROFILE_SIMPLE_ADMITTANCECONTROLLER_DQREF_COMPUTATION);
 
         const Vector & tauDes = m_tauDesSIN(iter);
         const Vector & tau = m_tauSIN(iter);
@@ -122,7 +122,7 @@ namespace dynamicgraph
 
         s = dqRef;
 
-        getProfiler().stop(PROFILE_ADMITTANCECONTROLLER_DQREF_COMPUTATION);
+        getProfiler().stop(PROFILE_SIMPLE_ADMITTANCECONTROLLER_DQREF_COMPUTATION);
 
         return s;
       }
@@ -135,7 +135,7 @@ namespace dynamicgraph
           return s;
         }
 
-        getProfiler().start(PROFILE_ADMITTANCECONTROLLER_QREF_COMPUTATION);
+        getProfiler().start(PROFILE_SIMPLE_ADMITTANCECONTROLLER_QREF_COMPUTATION);
 
         const Vector & dqRef = m_dqRefSOUT(iter);
 
@@ -157,7 +157,7 @@ namespace dynamicgraph
 
         s = m_q;
 
-        getProfiler().stop(PROFILE_ADMITTANCECONTROLLER_QREF_COMPUTATION);
+        getProfiler().stop(PROFILE_SIMPLE_ADMITTANCECONTROLLER_QREF_COMPUTATION);
 
         return s;
       }
@@ -169,9 +169,9 @@ namespace dynamicgraph
       /* --- ENTITY -------------------------------------------------------- */
       /* ------------------------------------------------------------------- */
 
-      void AdmittanceController::display(std::ostream& os) const
+      void SimpleAdmittanceController::display(std::ostream& os) const
       {
-        os << "AdmittanceController " << getName();
+        os << "SimpleAdmittanceController " << getName();
         try
         {
           getProfiler().report_all(3, os);
diff --git a/src/simple-reference-frame.cpp b/src/simple-reference-frame.cpp
index fb3e68b49dabdeff9833d9e999c887970f42e296..9c4055c21050fd088fc05318a1f7222425e100c7 100644
--- a/src/simple-reference-frame.cpp
+++ b/src/simple-reference-frame.cpp
@@ -21,7 +21,7 @@
 #include <dynamic-graph/command-bind.h>
 
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 namespace dynamicgraph
 {
diff --git a/src/simple-zmp-estimator.cpp b/src/simple-zmp-estimator.cpp
index 5a697f71d339679928834f57f2eca4e3b381acdf..fe2803b260affd34ed76fac08005732095fa111a 100644
--- a/src/simple-zmp-estimator.cpp
+++ b/src/simple-zmp-estimator.cpp
@@ -21,7 +21,7 @@
 #include <dynamic-graph/command-bind.h>
 
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 namespace dynamicgraph
 {
diff --git a/src/state-transformation.cpp b/src/state-transformation.cpp
index a5a1cea22537eb5bad9168c4529ff40b0af1ee3a..f21b110109140567faaaec699d52474890d4fa5a 100644
--- a/src/state-transformation.cpp
+++ b/src/state-transformation.cpp
@@ -21,7 +21,7 @@
 #include <dynamic-graph/command-bind.h>
 
 #include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
 
 namespace dynamicgraph
 {
diff --git a/src/base-estimator.cpp b/src/talos-base-estimator.cpp
similarity index 91%
rename from src/base-estimator.cpp
rename to src/talos-base-estimator.cpp
index 41701c662860a89e1fe38e7bb27962ce2ba0ef5c..0651047cd91796d806e3d63376754dba79608606 100644
--- a/src/base-estimator.cpp
+++ b/src/talos-base-estimator.cpp
@@ -14,11 +14,11 @@
  * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <sot/talos_balance/base-estimator.hh>
+#include <sot/talos_balance/talos-base-estimator.hh>
 #include <sot/core/debug.hh>
 #include <dynamic-graph/factory.h>
 #include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
 #include "pinocchio/algorithm/frames.hpp"
 
 namespace dynamicgraph
@@ -34,7 +34,10 @@ namespace dynamicgraph
       using namespace pinocchio;
       using boost::math::normal; // typedef provides default type is double.
 
-      void se3Interp(const pinocchio::SE3 & s1, const pinocchio::SE3 & s2, const double alpha, pinocchio::SE3 & s12)
+      void se3Interp(const pinocchio::SE3 & s1,
+		     const pinocchio::SE3 & s2,
+		     const double alpha,
+		     pinocchio::SE3 & s12)
       {
         const Eigen::Vector3d t_( s1.translation() * alpha+
                                   s2.translation() * (1-alpha));
@@ -45,7 +48,8 @@ namespace dynamicgraph
         s12 =  pinocchio::SE3(pinocchio::exp3(w),t_);
       }
 
-      void rpyToMatrix(double roll, double pitch, double yaw, Eigen::Matrix3d & R)
+      void rpyToMatrix(double roll,
+		       double pitch, double yaw, Eigen::Matrix3d & R)
       {
         Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
         Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
@@ -75,7 +79,9 @@ namespace dynamicgraph
         }
       }
 
-      void quanternionMult(const Eigen::Vector4d & q1, const Eigen::Vector4d & q2,  Eigen::Vector4d & q12)
+      void quanternionMult(const Eigen::Vector4d & q1,
+			   const Eigen::Vector4d & q2,
+			   Eigen::Vector4d & q12)
       {
         q12(0) = q2(0)*q1(0)-q2(1)*q1(1)-q2(2)*q1(2)-q2(3)*q1(3);
         q12(1) = q2(0)*q1(1)+q2(1)*q1(0)-q2(2)*q1(3)+q2(3)*q1(2);
@@ -83,7 +89,10 @@ namespace dynamicgraph
         q12(3) = q2(0)*q1(3)-q2(1)*q1(2)+q2(2)*q1(1)+q2(3)*q1(0);
       }
 
-      void pointRotationByQuaternion(const Eigen::Vector3d & point,const Eigen::Vector4d & quat, Eigen::Vector3d & rotatedPoint)
+      void pointRotationByQuaternion
+      (const Eigen::Vector3d & point,
+       const Eigen::Vector4d & quat,
+       Eigen::Vector3d & rotatedPoint)
       {
         const Eigen::Vector4d p4(0.0, point(0),point(1),point(2));
         const Eigen::Vector4d quat_conj(quat(0),-quat(1),-quat(2),-quat(3));
@@ -111,9 +120,14 @@ namespace dynamicgraph
       {
         double wMean = (a1*w1+ a2*w2)/(w1+w2);
         if ( a1-a2 >= EIGEN_PI )
-            return (EIGEN_PI*(w1-w2)/(w2+w1) - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w1-w2)/(w2+w1) : EIGEN_PI + wMean - EIGEN_PI*(w1-w2)/(w2+w1); 
+	  return (EIGEN_PI*(w1-w2)/(w2+w1) - wMean)
+	    < 0 ? -EIGEN_PI +
+	      wMean - EIGEN_PI*(w1-w2)/(w2+w1) : EIGEN_PI +
+	      wMean - EIGEN_PI*(w1-w2)/(w2+w1); 
         else if ( a1-a2 < -EIGEN_PI )
-            return (EIGEN_PI*(w2-w1)/(w1+w2) - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2) : EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2); 
+	  return (EIGEN_PI*(w2-w1)/(w1+w2) - wMean)
+	    < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2) :
+	      EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2); 
         return wMean;
       }
 
@@ -129,7 +143,8 @@ namespace dynamicgraph
       //     pa  = a1; //positive angle
       //     paw = w1; //positive angle weight 
       //     double piFac = (paw-naw)/(naw+paw);
-      //     return (EIGEN_PI*piFac - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*piFac : EIGEN_PI + wMean - EIGEN_PI*piFac;        
+      //     return (EIGEN_PI*piFac - wMean) < 0 ? -EIGEN_PI +
+      //       wMean - EIGEN_PI*piFac : EIGEN_PI + wMean - EIGEN_PI*piFac;
       //   }
       //   else if ( a1-a2 < -EIGEN_PI )
       //   {
@@ -138,7 +153,8 @@ namespace dynamicgraph
       //     pa  = a2; //positive angle
       //     paw = w2; //positive angle weight
       //     double piFac = (paw-naw)/(naw+paw);
-      //     return (EIGEN_PI*piFac - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*piFac : EIGEN_PI + wMean - EIGEN_PI*piFac; 
+      //     return (EIGEN_PI*piFac - wMean) < 0 ? -EIGEN_PI + wMean -
+      //    EIGEN_PI*piFac : EIGEN_PI + wMean - EIGEN_PI*piFac; 
       //   }
       //   return wMean;
       // }
@@ -157,17 +173,17 @@ namespace dynamicgraph
 
       /// Define EntityClassName here rather than in the header file
       /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
-      typedef BaseEstimator EntityClassName;
+      typedef TalosBaseEstimator EntityClassName;
 
       /* --- DG FACTORY ---------------------------------------------------- */
-      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(BaseEstimator,
-                                         "BaseEstimator");
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TalosBaseEstimator,
+                                         "TalosBaseEstimator");
 
       /* ------------------------------------------------------------------- */
       /* --- CONSTRUCTION -------------------------------------------------- */
       /* ------------------------------------------------------------------- */
-      BaseEstimator::
-      BaseEstimator(const std::string& name)
+      TalosBaseEstimator::
+      TalosBaseEstimator(const std::string& name)
         : Entity(name)
         ,CONSTRUCT_SIGNAL_IN( joint_positions,            dynamicgraph::Vector)
         ,CONSTRUCT_SIGNAL_IN( joint_velocities,           dynamicgraph::Vector)
@@ -238,90 +254,90 @@ namespace dynamicgraph
 
         /* Commands. */
         addCommand("init",
-                   makeCommandVoid2(*this, &BaseEstimator::init,
+                   makeCommandVoid2(*this, &TalosBaseEstimator::init,
                                     docCommandVoid2("Initialize the entity.",
                                                     "time step (double)",
                                                     "URDF file path (string)")));
 
 
         addCommand("set_fz_stable_windows_size",
-                           makeCommandVoid1(*this, &BaseEstimator::set_fz_stable_windows_size,
+                           makeCommandVoid1(*this, &TalosBaseEstimator::set_fz_stable_windows_size,
                                             docCommandVoid1("Set the windows size used to detect that feet is stable",
                                                             "int")));
         addCommand("set_alpha_w_filter",
-                   makeCommandVoid1(*this, &BaseEstimator::set_alpha_w_filter,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_alpha_w_filter,
                                     docCommandVoid1("Set the filter parameter to filter weights",
                                                     "double")));
         addCommand("set_alpha_DC_acc",
-                   makeCommandVoid1(*this, &BaseEstimator::set_alpha_DC_acc,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_alpha_DC_acc,
                                     docCommandVoid1("Set the filter parameter for removing DC from accelerometer data",
                                                     "double")));
         addCommand("set_alpha_DC_vel",
-                   makeCommandVoid1(*this, &BaseEstimator::set_alpha_DC_vel,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_alpha_DC_vel,
                                     docCommandVoid1("Set the filter parameter for removing DC from velocity integrated from acceleration",
                                                     "double")));
         addCommand("reset_foot_positions",
-                   makeCommandVoid0(*this, &BaseEstimator::reset_foot_positions,
+                   makeCommandVoid0(*this, &TalosBaseEstimator::reset_foot_positions,
                                     docCommandVoid0("Reset the position of the feet.")));
         addCommand("get_imu_weight",
                    makeDirectGetter(*this,&m_w_imu,
                                     docDirectGetter("Weight of imu-based orientation in sensor fusion",
                                                     "double")));
         addCommand("set_imu_weight",
-                   makeCommandVoid1(*this, &BaseEstimator::set_imu_weight,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_imu_weight,
                                     docCommandVoid1("Set the weight of imu-based orientation in sensor fusion",
                                                     "double")));
         addCommand("set_zmp_std_dev_right_foot",
-                   makeCommandVoid1(*this, &BaseEstimator::set_zmp_std_dev_right_foot,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_zmp_std_dev_right_foot,
                                     docCommandVoid1("Set the standard deviation of the ZMP measurement errors for the right foot",
                                                     "double")));
         addCommand("set_zmp_std_dev_left_foot",
-                   makeCommandVoid1(*this, &BaseEstimator::set_zmp_std_dev_left_foot,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_zmp_std_dev_left_foot,
                                     docCommandVoid1("Set the standard deviation of the ZMP measurement errors for the left foot",
                                                     "double")));
         addCommand("set_normal_force_std_dev_right_foot",
-                   makeCommandVoid1(*this, &BaseEstimator::set_normal_force_std_dev_right_foot,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_normal_force_std_dev_right_foot,
                                     docCommandVoid1("Set the standard deviation of the normal force measurement errors for the right foot",
                                                     "double")));
         addCommand("set_normal_force_std_dev_left_foot",
-                   makeCommandVoid1(*this, &BaseEstimator::set_normal_force_std_dev_left_foot,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_normal_force_std_dev_left_foot,
                                     docCommandVoid1("Set the standard deviation of the normal force measurement errors for the left foot",
                                                     "double")));
         addCommand("set_stiffness_right_foot",
-                   makeCommandVoid1(*this, &BaseEstimator::set_stiffness_right_foot,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_stiffness_right_foot,
                                     docCommandVoid1("Set the 6d stiffness of the spring at the right foot",
                                                     "vector")));
         addCommand("set_stiffness_left_foot",
-                   makeCommandVoid1(*this, &BaseEstimator::set_stiffness_left_foot,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_stiffness_left_foot,
                                     docCommandVoid1("Set the 6d stiffness of the spring at the left foot",
                                                     "vector")));
         addCommand("set_right_foot_sizes",
-                   makeCommandVoid1(*this, &BaseEstimator::set_right_foot_sizes,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_right_foot_sizes,
                                     docCommandVoid1("Set the size of the right foot (pos x, neg x, pos y, neg y)",
                                                     "4d vector")));
         addCommand("set_left_foot_sizes",
-                   makeCommandVoid1(*this, &BaseEstimator::set_left_foot_sizes,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_left_foot_sizes,
                                     docCommandVoid1("Set the size of the left foot (pos x, neg x, pos y, neg y)",
                                                     "4d vector")));
         addCommand("set_zmp_margin_right_foot",
-                   makeCommandVoid1(*this, &BaseEstimator::set_zmp_margin_right_foot,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_zmp_margin_right_foot,
                                     docCommandVoid1("Set the ZMP margin for the right foot",
                                                     "double")));
         addCommand("set_zmp_margin_left_foot",
-                   makeCommandVoid1(*this, &BaseEstimator::set_zmp_margin_left_foot,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_zmp_margin_left_foot,
                                     docCommandVoid1("Set the ZMP margin for the left foot",
                                                     "double")));
         addCommand("set_normal_force_margin_right_foot",
-                   makeCommandVoid1(*this, &BaseEstimator::set_normal_force_margin_right_foot,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_normal_force_margin_right_foot,
                                     docCommandVoid1("Set the normal force margin for the right foot",
                                                     "double")));
         addCommand("set_normal_force_margin_left_foot",
-                   makeCommandVoid1(*this, &BaseEstimator::set_normal_force_margin_left_foot,
+                   makeCommandVoid1(*this, &TalosBaseEstimator::set_normal_force_margin_left_foot,
                                     docCommandVoid1("Set the normal force margin for the left foot",
                                                     "double")));
       }
 
-      void BaseEstimator::init(const double & dt, const std::string& robotRef)
+      void TalosBaseEstimator::init(const double & dt, const std::string& robotRef)
       {
         m_dt = dt;
         try
@@ -388,14 +404,14 @@ namespace dynamicgraph
         m_initSucceeded = true;
       }
 
-      void BaseEstimator::set_fz_stable_windows_size(const int& ws)
+      void TalosBaseEstimator::set_fz_stable_windows_size(const int& ws)
       {
         if(ws<0.0)
           return SEND_MSG("windows_size should be a positive integer", MSG_TYPE_ERROR);
         m_fz_stable_windows_size = ws;
       }
 
-      void BaseEstimator::set_alpha_w_filter(const double& a)
+      void TalosBaseEstimator::set_alpha_w_filter(const double& a)
       {
         if(a<0.0 || a>1.0)
           return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
@@ -403,14 +419,14 @@ namespace dynamicgraph
       }
 
 
-      void BaseEstimator::set_alpha_DC_acc(const double& a)
+      void TalosBaseEstimator::set_alpha_DC_acc(const double& a)
       {
         if(a<0.0 || a>1.0)
           return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
         m_alpha_DC_acc = a;
       }
 
-      void BaseEstimator::set_alpha_DC_vel(const double& a)
+      void TalosBaseEstimator::set_alpha_DC_vel(const double& a)
       {
         if(a<0.0 || a>1.0)
           return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
@@ -418,95 +434,95 @@ namespace dynamicgraph
       }
 
 
-      void BaseEstimator::reset_foot_positions()
+      void TalosBaseEstimator::reset_foot_positions()
       {
         m_reset_foot_pos = true;
       }
 
-      void BaseEstimator::set_imu_weight(const double& w)
+      void TalosBaseEstimator::set_imu_weight(const double& w)
       {
         if(w<0.0)
           return SEND_MSG("Imu weight must be nonnegative", MSG_TYPE_ERROR);
         m_w_imu = w;
       }
 
-      void BaseEstimator::set_stiffness_right_foot(const dynamicgraph::Vector & k)
+      void TalosBaseEstimator::set_stiffness_right_foot(const dynamicgraph::Vector & k)
       {
         if(k.size()!=6)
           return SEND_MSG("Stiffness vector should have size 6, not "+toString(k.size()), MSG_TYPE_ERROR);
         m_K_rf = k;
       }
 
-      void BaseEstimator::set_stiffness_left_foot(const dynamicgraph::Vector & k)
+      void TalosBaseEstimator::set_stiffness_left_foot(const dynamicgraph::Vector & k)
       {
         if(k.size()!=6)
           return SEND_MSG("Stiffness vector should have size 6, not "+toString(k.size()), MSG_TYPE_ERROR);
         m_K_lf = k;
       }
 
-      void BaseEstimator::set_zmp_std_dev_right_foot(const double & std_dev)
+      void TalosBaseEstimator::set_zmp_std_dev_right_foot(const double & std_dev)
       {
         if(std_dev<=0.0)
           return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
         m_zmp_std_dev_rf = std_dev;
       }
 
-      void BaseEstimator::set_zmp_std_dev_left_foot(const double & std_dev)
+      void TalosBaseEstimator::set_zmp_std_dev_left_foot(const double & std_dev)
       {
         if(std_dev<=0.0)
           return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
         m_zmp_std_dev_lf = std_dev;
       }
 
-      void BaseEstimator::set_normal_force_std_dev_right_foot(const double & std_dev)
+      void TalosBaseEstimator::set_normal_force_std_dev_right_foot(const double & std_dev)
       {
         if(std_dev<=0.0)
           return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
         m_fz_std_dev_rf = std_dev;
       }
 
-      void BaseEstimator::set_normal_force_std_dev_left_foot(const double & std_dev)
+      void TalosBaseEstimator::set_normal_force_std_dev_left_foot(const double & std_dev)
       {
         if(std_dev<=0.0)
           return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
         m_fz_std_dev_lf = std_dev;
       }
 
-      void BaseEstimator::set_right_foot_sizes(const dynamicgraph::Vector & s)
+      void TalosBaseEstimator::set_right_foot_sizes(const dynamicgraph::Vector & s)
       {
         if(s.size()!=4)
           return SEND_MSG("Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
         m_right_foot_sizes = s;
       }
 
-      void BaseEstimator::set_left_foot_sizes(const dynamicgraph::Vector & s)
+      void TalosBaseEstimator::set_left_foot_sizes(const dynamicgraph::Vector & s)
       {
         if(s.size()!=4)
           return SEND_MSG("Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
         m_left_foot_sizes = s;
       }
 
-      void BaseEstimator::set_zmp_margin_right_foot(const double & margin)
+      void TalosBaseEstimator::set_zmp_margin_right_foot(const double & margin)
       {
         m_zmp_margin_rf = margin;
       }
 
-      void BaseEstimator::set_zmp_margin_left_foot(const double & margin)
+      void TalosBaseEstimator::set_zmp_margin_left_foot(const double & margin)
       {
         m_zmp_margin_lf = margin;
       }
 
-      void BaseEstimator::set_normal_force_margin_right_foot(const double & margin)
+      void TalosBaseEstimator::set_normal_force_margin_right_foot(const double & margin)
       {
         m_fz_margin_rf = margin;
       }
 
-      void BaseEstimator::set_normal_force_margin_left_foot(const double & margin)
+      void TalosBaseEstimator::set_normal_force_margin_left_foot(const double & margin)
       {
         m_fz_margin_lf = margin;
       }
 
-      void BaseEstimator::compute_zmp(const Vector6 & w, Vector2 & zmp)
+      void TalosBaseEstimator::compute_zmp(const Vector6 & w, Vector2 & zmp)
       {
         pinocchio::Force f(w);
         f = m_sole_M_ftSens.act(f);
@@ -516,7 +532,7 @@ namespace dynamicgraph
         zmp[1] =  f.angular()[0] / f.linear()[2];
       }
 
-      double BaseEstimator::compute_zmp_weight(const Vector2 & zmp, const Vector4 & foot_sizes,
+      double TalosBaseEstimator::compute_zmp_weight(const Vector2 & zmp, const Vector4 & foot_sizes,
                                                double std_dev, double margin)
       {
         double fs0=foot_sizes[0] - margin;
@@ -534,14 +550,14 @@ namespace dynamicgraph
         return cdx*cdy;
       }
 
-      double BaseEstimator::compute_force_weight(double fz, double std_dev, double margin)
+      double TalosBaseEstimator::compute_force_weight(double fz, double std_dev, double margin)
       {
         if (fz<margin)
           return 0.0;
         return (cdf(m_normal, (fz-margin)/std_dev)-0.5)*2.0;
       }
 
-      void BaseEstimator::reset_foot_positions_impl(const Vector6 & ftlf, const Vector6 & ftrf)
+      void TalosBaseEstimator::reset_foot_positions_impl(const Vector6 & ftlf, const Vector6 & ftrf)
       {
         // compute the base position wrt each foot
         SE3 dummy, dummy1, lfMff, rfMff;
@@ -595,7 +611,7 @@ namespace dynamicgraph
         m_reset_foot_pos = false;
       }
 
-      void BaseEstimator::kinematics_estimation(const Vector6 & ft, const Vector6 & K,
+      void TalosBaseEstimator::kinematics_estimation(const Vector6 & ft, const Vector6 & K,
                                                 const SE3 & oMfs, const pinocchio::FrameIndex foot_id,
                                                 SE3 & oMff, SE3 & oMfa, SE3 & fsMff)
       {
@@ -1293,9 +1309,9 @@ namespace dynamicgraph
       /* --- ENTITY -------------------------------------------------------- */
       /* ------------------------------------------------------------------- */
 
-      void BaseEstimator::display(std::ostream& os) const
+      void TalosBaseEstimator::display(std::ostream& os) const
       {
-        os << "BaseEstimator "<<getName();
+        os << "TalosBaseEstimator "<<getName();
         try
         {
           getProfiler().report_all(3, os);
diff --git a/src/control-manager.cpp b/src/talos-control-manager.cpp
similarity index 88%
rename from src/control-manager.cpp
rename to src/talos-control-manager.cpp
index 883bebf4dcb474630a1499b084c1929eabb7b7d0..d0f38a5e58720822bf8e73b1fe3a5038ba270ab3 100644
--- a/src/control-manager.cpp
+++ b/src/talos-control-manager.cpp
@@ -14,12 +14,12 @@
  * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <sot/talos_balance/control-manager.hh>
+#include <sot/talos_balance/talos-control-manager.hh>
 #include <sot/core/debug.hh>
 #include <dynamic-graph/factory.h>
 
 #include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
 #include <sot/talos_balance/utils/statistics.hh>
 
 
@@ -47,17 +47,17 @@ namespace dynamicgraph
 
       /// Define EntityClassName here rather than in the header file
       /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
-      typedef ControlManager EntityClassName;
+      typedef TalosControlManager EntityClassName;
 
       /* --- DG FACTORY ---------------------------------------------------- */
-      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControlManager,
-                                         "ControlManager");
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TalosControlManager,
+                                         "TalosControlManager");
 
       /* ------------------------------------------------------------------- */
       /* --- CONSTRUCTION -------------------------------------------------- */
       /* ------------------------------------------------------------------- */
-      ControlManager::
-      ControlManager(const std::string& name)
+      TalosControlManager::
+      TalosControlManager(const std::string& name)
         : Entity(name)
         ,CONSTRUCT_SIGNAL_IN(u_max,        dynamicgraph::Vector)
         ,CONSTRUCT_SIGNAL_OUT(u,           dynamicgraph::Vector, m_u_maxSIN)
@@ -73,43 +73,43 @@ namespace dynamicgraph
 
         /* Commands. */
         addCommand("init",
-                   makeCommandVoid2(*this, &ControlManager::init,
+                   makeCommandVoid2(*this, &TalosControlManager::init,
                                     docCommandVoid2("Initialize the entity.",
                                                     "Time period in seconds (double)",
                                                     "Robot reference (string)")));
         addCommand("addCtrlMode",
-                   makeCommandVoid1(*this, &ControlManager::addCtrlMode,
+                   makeCommandVoid1(*this, &TalosControlManager::addCtrlMode,
                                     docCommandVoid1("Create an input signal with name 'ctrl_x' where x is the specified name.",
                                                     "Name (string)")));
 
         addCommand("ctrlModes",
-                   makeCommandVoid0(*this, &ControlManager::ctrlModes,
+                   makeCommandVoid0(*this, &TalosControlManager::ctrlModes,
                                     docCommandVoid0("Get a list of all the available control modes.")));
 
         addCommand("setCtrlMode",
-                   makeCommandVoid2(*this, &ControlManager::setCtrlMode,
+                   makeCommandVoid2(*this, &TalosControlManager::setCtrlMode,
                                     docCommandVoid2("Set the control mode for a joint.",
                                                     "(string) joint name",
                                                     "(string) control mode")));
 
         addCommand("getCtrlMode",
-                   makeCommandVoid1(*this, &ControlManager::getCtrlMode,
+                   makeCommandVoid1(*this, &TalosControlManager::getCtrlMode,
                                     docCommandVoid1("Get the control mode of a joint.",
                                                     "(string) joint name")));
 
         addCommand("resetProfiler",
-                   makeCommandVoid0(*this, &ControlManager::resetProfiler,
+                   makeCommandVoid0(*this, &TalosControlManager::resetProfiler,
                                     docCommandVoid0("Reset the statistics computed by the profiler (print this entity to see them).")));
 
         addCommand("addEmergencyStopSIN",
-                   makeCommandVoid1(*this, &ControlManager::addEmergencyStopSIN,
+                   makeCommandVoid1(*this, &TalosControlManager::addEmergencyStopSIN,
                                     docCommandVoid1("Add emergency signal input from another entity that can stop the control if necessary.",
                                                     "(string) signal name : 'emergencyStop_' + name")));
 
         addCommand("isEmergencyStopTriggered", makeDirectGetter(*this,&m_emergency_stop_triggered, docDirectGetter("Check whether emergency stop is triggered","bool")));
       }
 
-      void ControlManager::init(const double & dt,
+      void TalosControlManager::init(const double & dt,
                                 const std::string &robotRef)
       {
         if(dt<=0.0)
@@ -129,7 +129,7 @@ namespace dynamicgraph
         }
         m_numDofs = m_robot_util->m_nbJoints + 6;
 
-        //ControlManager::setLoggerVerbosityLevel((dynamicgraph::LoggerVerbosity) 4);
+        //TalosControlManager::setLoggerVerbosityLevel((dynamicgraph::LoggerVerbosity) 4);
         m_jointCtrlModes_current.resize(m_numDofs);
         m_jointCtrlModes_previous.resize(m_numDofs);
         m_jointCtrlModesCountDown.resize(m_numDofs,0);
@@ -252,7 +252,7 @@ namespace dynamicgraph
 
       /* --- COMMANDS ---------------------------------------------------------- */
 
-      void ControlManager::addCtrlMode(const string& name)
+      void TalosControlManager::addCtrlMode(const string& name)
       {
         // check there is no other control mode with the same name
         for(unsigned int i=0;i<m_ctrlModes.size(); i++)
@@ -280,13 +280,13 @@ namespace dynamicgraph
         updateJointCtrlModesOutputSignal();
       }
 
-      void ControlManager::ctrlModes()
+      void TalosControlManager::ctrlModes()
       {
         SEND_MSG(toString(m_ctrlModes), MSG_TYPE_INFO);
       }
 
 
-      void ControlManager::setCtrlMode(const string& jointName, const string& ctrlMode)
+      void TalosControlManager::setCtrlMode(const string& jointName, const string& ctrlMode)
       {
         CtrlMode cm;
         if(convertStringToCtrlMode(ctrlMode,cm)==false)
@@ -316,7 +316,7 @@ namespace dynamicgraph
         updateJointCtrlModesOutputSignal();
       }
 
-      void ControlManager::setCtrlMode(const int jid, const CtrlMode& cm)
+      void TalosControlManager::setCtrlMode(const int jid, const CtrlMode& cm)
       {
         if(m_jointCtrlModesCountDown[jid]!=0)
           return SEND_MSG("Cannot change control mode of joint "+toString(jid)+
@@ -341,7 +341,7 @@ namespace dynamicgraph
         }
       }
 
-      void ControlManager::getCtrlMode(const std::string& jointName)
+      void TalosControlManager::getCtrlMode(const std::string& jointName)
       {
         if(jointName=="all")
         {
@@ -358,25 +358,25 @@ namespace dynamicgraph
         SEND_MSG("The control mode of joint "+jointName+" is "+m_jointCtrlModes_current[i].name,MSG_TYPE_INFO);
       }
 
-      void ControlManager::resetProfiler()
+      void TalosControlManager::resetProfiler()
       {
         getProfiler().reset_all();
         getStatistics().reset_all();
       }
 
-//      void ControlManager::setStreamPrintPeriod(const double & s)
+//      void TalosControlManager::setStreamPrintPeriod(const double & s)
 //      {
 //        getLogger().setStreamPrintPeriod(s);
 //      }
 
-      void ControlManager::setSleepTime(const double &seconds)
+      void TalosControlManager::setSleepTime(const double &seconds)
       {
         if(seconds<0.0)
           return SEND_MSG("Sleep time cannot be negative!", MSG_TYPE_ERROR);
         m_sleep_time = seconds;
       }
 
-      void ControlManager::addEmergencyStopSIN(const string& name)
+      void TalosControlManager::addEmergencyStopSIN(const string& name)
       {
         SEND_MSG("New emergency signal input emergencyStop_" + name + " created",MSG_TYPE_INFO);
         // create a new input signal
@@ -390,7 +390,7 @@ namespace dynamicgraph
 
       /* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
 
-      void ControlManager::updateJointCtrlModesOutputSignal()
+      void TalosControlManager::updateJointCtrlModesOutputSignal()
       {
         if (m_numDofs==0)
         {
@@ -416,7 +416,7 @@ namespace dynamicgraph
 
       }
 
-      bool ControlManager::convertStringToCtrlMode(const std::string& name, CtrlMode& cm)
+      bool TalosControlManager::convertStringToCtrlMode(const std::string& name, CtrlMode& cm)
       {
         // Check if the ctrl mode name exists
         for(unsigned int i=0;i<m_ctrlModes.size(); i++)
@@ -430,7 +430,7 @@ namespace dynamicgraph
         return false;
       }
 
-      bool ControlManager::convertJointNameToJointId(const std::string& name, unsigned int& id)
+      bool TalosControlManager::convertJointNameToJointId(const std::string& name, unsigned int& id)
       {
         // Check if the joint name exists
         int jid = int(m_robot_util->get_id_from_name(name)); // cast needed due to bug in robot-utils
@@ -449,7 +449,7 @@ namespace dynamicgraph
       }
 
 /*
-      bool ControlManager::isJointInRange(unsigned int id, double q)
+      bool TalosControlManager::isJointInRange(unsigned int id, double q)
       {
         const JointLimits & JL = m_robot_util->get_joint_limits_from_id((Index)id);
 
@@ -475,9 +475,9 @@ namespace dynamicgraph
       /* ------------------------------------------------------------------- */
 
 
-      void ControlManager::display(std::ostream& os) const
+      void TalosControlManager::display(std::ostream& os) const
       {
-        os << "ControlManager "<<getName();
+        os << "TalosControlManager "<<getName();
         try
         {
           getProfiler().report_all(3, os);
diff --git a/src/utils/causal-filter.cpp b/src/utils/causal-filter.cpp
deleted file mode 100644
index d67397df9ebb1cd4a251fd6cfe0ae77639a2e56b..0000000000000000000000000000000000000000
--- a/src/utils/causal-filter.cpp
+++ /dev/null
@@ -1,128 +0,0 @@
-/*
- * Copyright 2017-, Rohan Budhiraja LAAS-CNRS
- *
- * This file is part of sot-torque-control.
- * sot-torque-control is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-torque-control is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.  You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <iostream>
-
-#include <sot/talos_balance/utils/causal-filter.hh>
-
-
-
-/*
-Filter data with an IIR or FIR filter.
-
-Filter a data sequence, x, using a digital filter. The filter is a direct form II transposed implementation of the standard difference equation.
-This means that the filter implements:
-
-a[0]*y[N] = b[0]*x[N] + b[1]*x[N-1] + ... + b[m-1]*x[N-(m-1)]
-                      - a[1]*y[N-1] - ... - a[n-1]*y[N-(n-1)]
-
-where m is the degree of the numerator, n is the degree of the denominator, and N is the sample number
-
-*/
-
-CausalFilter::CausalFilter(const double &timestep,
-                           const int& xSize,
-                           const Eigen::VectorXd& filter_numerator,
-                           const Eigen::VectorXd& filter_denominator)
-  
-  : m_dt(timestep)
-  , m_x_size(xSize)
-  , m_filter_order_m(filter_numerator.size())
-  , m_filter_order_n(filter_denominator.size())
-  , m_filter_numerator(filter_numerator)
-  , m_filter_denominator(filter_denominator)
-  , first_sample(true)
-  , pt_numerator(0)
-  , pt_denominator(0)
-  , input_buffer(Eigen::MatrixXd::Zero(xSize, filter_numerator.size()))
-  , output_buffer(Eigen::MatrixXd::Zero(xSize, filter_denominator.size()-1))
-{
-  assert(timestep>0.0 && "Timestep should be > 0");
-  assert(m_filter_numerator.size() == m_filter_order_m);
-  assert(m_filter_denominator.size() == m_filter_order_n);
-}
-
-
-void CausalFilter::get_x_dx_ddx(const Eigen::VectorXd& base_x,
-                                Eigen::VectorXd& x_output_dx_ddx)
-{
-  //const dynamicgraph::Vector &base_x = m_xSIN(iter);
-  if(first_sample)
-  {
-    for(int i=0;i<m_filter_order_m; i++)
-      input_buffer.col(i) = base_x;
-    for(int i=0;i<m_filter_order_n-1; i++)
-      output_buffer.col(i) = base_x*m_filter_numerator.sum()/m_filter_denominator.sum(); 
-    first_sample = false;
-  }
-
-  input_buffer.col(pt_numerator) = base_x;
-  
-  Eigen::VectorXd b(m_filter_order_m);
-  Eigen::VectorXd a(m_filter_order_n-1);
-  b.head(pt_numerator+1) = m_filter_numerator.head(pt_numerator+1).reverse();
-  b.tail(m_filter_order_m-pt_numerator-1) =
-    m_filter_numerator.tail(m_filter_order_m-pt_numerator-1).reverse();
-
-  a.head(pt_denominator+1) = m_filter_denominator.segment(1, pt_denominator+1).reverse();
-  a.tail(m_filter_order_n-pt_denominator-2) =
-    m_filter_denominator.tail(m_filter_order_n-pt_denominator-2).reverse();
-  x_output_dx_ddx.head(m_x_size) = (input_buffer*b-output_buffer*a)/m_filter_denominator[0];
-
-  //Finite Difference
-  Eigen::Index pt_denominator_prev = (pt_denominator == 0) ? m_filter_order_n-2 : pt_denominator-1;  
-  x_output_dx_ddx.segment(m_x_size,m_x_size) = (x_output_dx_ddx.head(m_x_size)-output_buffer.col(pt_denominator))/m_dt;
-  x_output_dx_ddx.tail(m_x_size) = (x_output_dx_ddx.head(m_x_size)-2*output_buffer.col(pt_denominator)+output_buffer.col(pt_denominator_prev))/m_dt/m_dt;
-
-  pt_numerator = (pt_numerator+1) < m_filter_order_m ? (pt_numerator+1) : 0;
-  pt_denominator = (pt_denominator+1) < m_filter_order_n-1 ? (pt_denominator+1) : 0;
-  output_buffer.col(pt_denominator) = x_output_dx_ddx.head(m_x_size);
-  return;
-}
-
-
-
-void CausalFilter::switch_filter(const Eigen::VectorXd& filter_numerator,
-                                 const Eigen::VectorXd& filter_denominator)
-{
-  Eigen::Index filter_order_m = filter_numerator.size();
-  Eigen::Index filter_order_n = filter_denominator.size();
-
-  Eigen::VectorXd current_x(input_buffer.col(pt_numerator));
-
-  input_buffer.resize(Eigen::NoChange, filter_order_m);
-  output_buffer.resize(Eigen::NoChange, filter_order_n-1);
-
-  for(int i=0;i<filter_order_m; i++)
-    input_buffer.col(i) = current_x;
-
-  for(int i=0;i<filter_order_n-1; i++)
-    output_buffer.col(i) = current_x*filter_numerator.sum()/filter_denominator.sum(); 
-
-
-  m_filter_order_m = filter_order_m;
-  m_filter_numerator.resize(filter_order_m);
-  m_filter_numerator = filter_numerator;
-
-  m_filter_order_n = filter_order_n;
-  m_filter_denominator.resize(filter_order_n);
-  m_filter_denominator = filter_denominator;
-
-  pt_numerator = 0;
-  pt_denominator = 0;
-
-  return;
-}
diff --git a/src/utils/stop-watch.cpp b/src/utils/stop-watch.cpp
deleted file mode 100644
index c6d846b163f03462a3e926cf90b11a104c83fa8c..0000000000000000000000000000000000000000
--- a/src/utils/stop-watch.cpp
+++ /dev/null
@@ -1,347 +0,0 @@
-/*
-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/talos_balance/utils/stop-watch.hh"
-
-using std::map;
-using std::string;
-using std::ostringstream;
-
-//#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;
-}