Merge branch 'devel' into origin-devel

doc/additionalDoc/a-features.h

+/**
+   \defgroup Filters
+ */

doc/additionalDoc/e-operators.h

+/**
+\page subp_operators Operators on signals
+
+\section subp_unitary_op Unitary Algebraic operators
+
+\subsection ssubp_unitary_op Vector selector
+
+This entity output a vector remapping an input vector 
+\f${\bf v}_{in}=[v_0,v_1,\cdots,v_n]\f$.
+It is realized by specifying bounds such as \f(${[i,j],[k,l]}f$),
+then the output vector will be the contanetion of the
+intervals extracted from the input vector:
+\f${\bf v}_{out}=[v_i,v_{i+1},\cdots,v_{j-1},v_{j},v_{k},
+v_{k+1},\cdots,v_{l-1},v_l]\f$
+For instance if we have an input vector
+such that: 
+\code[bash]
+1
+2
+3
+4
+5
+6
+7
+9
+10
+\endcode
+then specifying the bounds \f$(3,5)\f$ and \f$( 7,10\f$) will gives
+the following output vector
+\code[bash]
+3
+4
+7
+8
+9
+\endcode
+
+ */

include/CMakeLists.txt

@@ -3,6 +3,7 @@
 SET(NEWHEADERS
   sot/core/api.hh
   sot/core/abstract-sot-external-interface.hh
+  sot/core/causal-filter.hh
   sot/core/device.hh
   sot/core/robot-simu.hh
   sot/core/matrix-geometry.hh
@@ -76,6 +77,7 @@ SET(NEWHEADERS
   sot/core/periodic-call.hh
   sot/core/periodic-call-entity.hh
   sot/core/trajectory.hh
+  sot/core/stop-watch.hh
   sot/core/switch.hh
   sot/core/variadic-op.hh
   sot/core/robot-utils.hh

include/sot/core/causal-filter.hh

+#ifndef _SOT_CORE_CAUSAL_FILTER_H_
+#define _SOT_CORE_CAUSAL_FILTER_H_
+/*
+ * 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>
+
+/** \addtogroup Filters
+    \section subsec_causalfilter CausalFilter
+    Filter data with an IIR or FIR filter.
+    
+    Filter a data sequence, \f$x\f$, using a digital filter. 
+    The filter is a direct form II transposed implementation 
+    of the standard difference equation.
+    This means that the filter implements:
+    
+    \f$ 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)] \f$
+    
+    where \f$m\f$ is the degree of the numerator, 
+    \f$n\f$ is the degree of the denominator, 
+    and \f$N\f$ is the sample number
+    
+    
+ */
+namespace dynamicgraph {
+  namespace sot {
+    
+    class CausalFilter
+    {
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+      
+      /** --- CONSTRUCTOR ---- 
+	  \param[in] timestep
+	  \param[in] xSize
+	  \param[in] filter_numerator
+	  \param[in] filter_denominator
+	  
+	  xSize is 
+      */
+      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:
+      /// sampling timestep of the input signal
+      double m_dt;
+      /// Size 
+      int m_x_size;
+      /// Size of the numerator \f$m\f$
+      Eigen::VectorXd::Index m_filter_order_m;
+      /// Size of the denominator \f$n\f$
+      Eigen::VectorXd::Index m_filter_order_n;
+      
+      /// Coefficients of the numerator \f$b\f$
+      Eigen::VectorXd m_filter_numerator;
+      /// Coefficients of the denominator \f$a\f$
+      Eigen::VectorXd m_filter_denominator;
+      bool m_first_sample;
+      /// 
+      int m_pt_numerator;
+      int m_pt_denominator;
+      Eigen::MatrixXd m_input_buffer;
+      Eigen::MatrixXd m_output_buffer;
+    }; // class CausalFilter
+  } /// core
+} /// sot 
+#endif /* _SOT_CORE_CAUSAL_FILTER_H_ */

include/sot/core/debug.hh

@@ -24,180 +24,183 @@
 # endif //! VP_TEMPLATE_DEBUG_MODE
 
 # define SOT_COMMON_TRACES						\
-  do {									\
-    va_list arg;							\
-    va_start(arg,format);						\
-    vsnprintf( charbuffer,SIZE,format,arg );				\
-    va_end(arg);							\
-    outputbuffer << tmpbuffer.str() << charbuffer <<std::endl;		\
-  } while(0)
-
-namespace dynamicgraph {
-  namespace sot {
-    class SOT_CORE_EXPORT DebugTrace
+    do {								\
+	va_list arg;							\
+	va_start(arg,format);						\
+	vsnprintf( charbuffer,SIZE,format,arg );			\
+	va_end(arg);							\
+	outputbuffer << tmpbuffer.str() << charbuffer <<std::endl;	\
+    } while(0)
+
+namespace dynamicgraph
+{
+    namespace sot
     {
-    public:
-      static const int SIZE = 512;
-
-      std::stringstream tmpbuffer;
-      std::ostream& outputbuffer;
-      char charbuffer[SIZE+1];
-      int traceLevel;
-      int traceLevelTemplate;
-
-      DebugTrace( std::ostream& os ): outputbuffer(os) {}
-
-      inline void trace(const int level,const char* format, ...)
-      {
-	if (level<=traceLevel)
-	  SOT_COMMON_TRACES;
-	tmpbuffer.str("");
-      }
-
-      inline void trace(const char* format,...)
-      {
-	SOT_COMMON_TRACES;
-	tmpbuffer.str("");
-      }
-
-      inline void trace(const int level=-1)
-      {
-	if (level<=traceLevel)
-	  outputbuffer << tmpbuffer.str(); tmpbuffer.str("");
-      }
-
-      inline void traceTemplate(const int level, const char* format, ...)
-      {
-	if (level<=traceLevelTemplate)
-	  SOT_COMMON_TRACES;
-	tmpbuffer.str("");
-      }
-
-      inline void traceTemplate(const char* format, ...)
-      {
-	SOT_COMMON_TRACES;
-	tmpbuffer.str("");
-      }
-
-      inline DebugTrace& pre(const std::ostream&)
-      {
-	return *this;
-      }
-
-      inline DebugTrace& pre(const std::ostream&, int level)
-      {
-	traceLevel = level;
-	return *this;
-      }
-
-      static const char* DEBUG_FILENAME_DEFAULT;
-      static void openFile(const char * filename = DEBUG_FILENAME_DEFAULT);
-      static void closeFile(const char * filename = DEBUG_FILENAME_DEFAULT);
-    };
-
-    SOT_CORE_EXPORT extern DebugTrace sotDEBUGFLOW;
-    SOT_CORE_EXPORT extern DebugTrace sotERRORFLOW;
-  } // namespace sot
+	class SOT_CORE_EXPORT DebugTrace
+	{
+	public:
+	    static const int SIZE = 512;
+
+	    std::stringstream tmpbuffer;
+	    std::ostream& outputbuffer;
+	    char charbuffer[SIZE+1];
+	    int traceLevel;
+	    int traceLevelTemplate;
+
+	    DebugTrace( std::ostream& os ): outputbuffer(os) {}
+
+	    inline void trace(const int level,const char* format, ...)
+	    {
+		if (level<=traceLevel)
+		    SOT_COMMON_TRACES;
+		tmpbuffer.str("");
+	    }
+
+	    inline void trace(const char* format,...)
+	    {
+		SOT_COMMON_TRACES;
+		tmpbuffer.str("");
+	    }
+
+	    inline void trace(const int level=-1)
+	    {
+		if (level<=traceLevel)
+		    outputbuffer << tmpbuffer.str();
+		tmpbuffer.str("");
+	    }
+
+	    inline void traceTemplate(const int level, const char* format, ...)
+	    {
+		if (level<=traceLevelTemplate)
+		    SOT_COMMON_TRACES;
+		tmpbuffer.str("");
+	    }
+
+	    inline void traceTemplate(const char* format, ...)
+	    {
+		SOT_COMMON_TRACES;
+		tmpbuffer.str("");
+	    }
+
+	    inline DebugTrace& pre(const std::ostream&)
+	    {
+		return *this;
+	    }
+
+	    inline DebugTrace& pre(const std::ostream&, int level)
+	    {
+		traceLevel = level;
+		return *this;
+	    }
+
+	    static const char* DEBUG_FILENAME_DEFAULT;
+	    static void openFile(const char * filename = DEBUG_FILENAME_DEFAULT);
+	    static void closeFile(const char * filename = DEBUG_FILENAME_DEFAULT);
+	};
+
+	SOT_CORE_EXPORT extern DebugTrace sotDEBUGFLOW;
+	SOT_CORE_EXPORT extern DebugTrace sotERRORFLOW;
+    } // namespace sot
 } // namespace dynamicgraph
 
 # ifdef VP_DEBUG
 #  define sotPREDEBUG				\
-  __FILE__ << ": " <<__FUNCTION__		\
-  << "(#" << __LINE__ << ") :"
+    __FILE__ << ": " <<__FUNCTION__		\
+    << "(#" << __LINE__ << ") :"
 
 #  define sotPREERROR				\
-  "\t!! "<<__FILE__ << ": " <<__FUNCTION__	\
-  << "(#" << __LINE__ << ") :"
+    "\t!! "<<__FILE__ << ": " <<__FUNCTION__	\
+    << "(#" << __LINE__ << ") :"
 
 #  define sotDEBUG(level)						\
-  if ((level>VP_DEBUG_MODE) || (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) ) \
-    ;									\
-  else									\
-    dynamicgraph::sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
+    if ((level>VP_DEBUG_MODE) || (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) ) \
+	;								\
+    else								\
+	dynamicgraph::sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
 
 #  define sotDEBUGMUTE(level)						\
-  if( (level>VP_DEBUG_MODE)||(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) ) \
-    ;									\
-  else									\
-    dynamicgraph::sot::sotDEBUGFLOW.outputbuffer
+    if( (level>VP_DEBUG_MODE)||(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) ) \
+	;								\
+    else								\
+	dynamicgraph::sot::sotDEBUGFLOW.outputbuffer
 
 #  define sotERROR							\
-  if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())		\
-    ;									\
-  else dynamicgraph::sot::sotERRORFLOW.outputbuffer << sotPREERROR
+    if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())		\
+	;								\
+    else dynamicgraph::sot::sotERRORFLOW.outputbuffer << sotPREERROR
 
 #  define sotDEBUGF							\
-  if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())		\
-    ;									\
-  else									\
-    dynamicgraph::sot::sotDEBUGFLOW.pre					\
-      (dynamicgraph::sot::sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_DEBUG_MODE).trace
+    if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())		\
+	;								\
+    else								\
+	dynamicgraph::sot::sotDEBUGFLOW.pre				\
+	    (dynamicgraph::sot::sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_DEBUG_MODE).trace
 
 #  define sotERRORF							\
-  if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())		\
-    ;									\
-  else									\
-    sot::sotERRORFLOW.pre(sot::sotERRORFLOW.tmpbuffer<<sotPREERROR).trace
+    if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())		\
+	;								\
+    else								\
+	sot::sotERRORFLOW.pre(sot::sotERRORFLOW.tmpbuffer<<sotPREERROR).trace
 
 // TEMPLATE
 #  define sotTDEBUG(level)						\
-  if((level>VP_TEMPLATE_DEBUG_MODE)||(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())) \
-    ;									\
-  else									\
-    dynamicgraph::sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
+    if((level>VP_TEMPLATE_DEBUG_MODE)||(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())) \
+	;								\
+    else								\
+	dynamicgraph::sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
 
 #  define sotTDEBUGF							\
-  if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())		\
-    ;									\
-  else									\
-    dynamicgraph::sot::sotDEBUGFLOW.pre					\
-      (dynamicgraph::sot::sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_TEMPLATE_DEBUG_MODE).trace
+    if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())		\
+	;								\
+    else								\
+	dynamicgraph::sot::sotDEBUGFLOW.pre				\
+	    (dynamicgraph::sot::sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_TEMPLATE_DEBUG_MODE).trace
 
 namespace dynamicgraph {
-  namespace sot {
-    inline bool sotDEBUG_ENABLE(const int & level)
-    {
-      return level <= VP_DEBUG_MODE;
-    }
-
-    inline bool sotTDEBUG_ENABLE(const int & level)
-    {
-      return level<=VP_TEMPLATE_DEBUG_MODE;
-    }
-  } // namespace sot
+    namespace sot {
+	inline bool sotDEBUG_ENABLE(const int & level)
+	{
+	    return level <= VP_DEBUG_MODE;
+	}
+
+	inline bool sotTDEBUG_ENABLE(const int & level)
+	{
+	    return level<=VP_TEMPLATE_DEBUG_MODE;
+	}
+    } // namespace sot
 } // namespace dynamicgraph
 
   /* -------------------------------------------------------------------------- */
 # else // VP_DEBUG
 #  define sotPREERROR				\
-  "\t!! "<<__FILE__ << ": " <<__FUNCTION__	\
-  << "(#" << __LINE__ << ") :"
+    "\t!! "<<__FILE__ << ": " <<__FUNCTION__	\
+    << "(#" << __LINE__ << ") :"
 #  define sotDEBUG(level) if( 1 ) ; else ::dynamicgraph::sot::__null_stream()
 #  define sotDEBUGMUTE(level) if( 1 ) ; else ::dynamicgraph::sot::__null_stream()
 #  define sotERROR sotERRORFLOW.outputbuffer << sotPREERROR
 
 namespace dynamicgraph {
-  namespace sot {
-    inline void sotDEBUGF(const int,const char*,...) {}
-    inline void sotDEBUGF(const char*,...) {}
-    inline void sotERRORF(const int, const char*,...) {}
-    inline void sotERRORF(const char*,...) {}
-    inline std::ostream& __null_stream () {
-      // This function should never be called. With -O3,
-      // it should not appear in the generated binary.
-      static std::ostream os (NULL); return os;
-    }
-  } // namespace sot
+    namespace sot {
+	inline void sotDEBUGF(const int,const char*,...) {}
+	inline void sotDEBUGF(const char*,...) {}
+	inline void sotERRORF(const int, const char*,...) {}
+	inline void sotERRORF(const char*,...) {}
+	inline std::ostream& __null_stream () {
+	    // This function should never be called. With -O3,
+	    // it should not appear in the generated binary.
+	    static std::ostream os (NULL); return os;
+	}
+    } // namespace sot
 } // namespace dynamicgraph
 
   // TEMPLATE
 #  define sotTDEBUG(level) if( 1 ) ; else ::dynamicgraph::sot::__null_stream()
 
 namespace dynamicgraph {
-  namespace sot {
-    inline void sotTDEBUGF(const int, const char*,...) {}
-    inline void sotTDEBUGF(const char*,...) {}
-  } // namespace sot
+    namespace sot {
+	inline void sotTDEBUGF(const int, const char*,...) {}
+	inline void sotTDEBUGF(const char*,...) {}
+    } // namespace sot
 } // namespace dynamicgraph
 
 #  define sotDEBUG_ENABLE(level) false
@@ -216,6 +219,6 @@ namespace dynamicgraph {
 
 #endif //! #ifdef SOT_CORE_DEBUG_HH
 
-  // Local variables:
-  // c-basic-offset: 4
-  // End:
+// Local variables:
+// c-basic-offset: 2
+// End:

include/sot/core/device.hh

@@ -107,6 +107,7 @@ namespace dynamicgraph {
 
     public: /* --- DISPLAY --- */
       virtual void display(std::ostream& os) const;
+      virtual void cmdDisplay();
       SOT_CORE_EXPORT friend std::ostream&
       operator<<(std::ostream& os,const Device& r) {
         r.display(os); return os;

include/sot/core/filter-differentiator.hh

+/*
+ * 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/core/stop-watch.hh>
+#include <sot/core/causal-filter.hh>
+
+namespace dynamicgraph {
+  namespace sot {
+      /** \addtogroup Filters 
+	  \section subsec_filterdiff FilterDifferentiator
+        This Entity takes as inputs a signal and applies a low pass filter (implemented through CasualFilter)
+        and computes finite difference derivative.
+	
+	The input signal is provided through m_xSIN (an entity signal). 
+	The filtered signal is given through m_x_filteredSOUT.
+	The first derivative of the filtered signal is provided with m_dxSOUT.
+	The second derivative of the filtered signal is provided with m_ddxSOUT.
+        */
+      class SOTFILTERDIFFERENTIATOR_EXPORT FilterDifferentiator
+          :public ::dynamicgraph::Entity
+      {
+        DYNAMIC_GRAPH_ENTITY_DECL();
+
+      public:  /* --- SIGNALS --- */
+	/// Input signals
+        DECLARE_SIGNAL_IN(x,                 dynamicgraph::Vector);
+	/// Output signal x_filtered
+        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 sot
+} // namespace dynamicgraph
+
+