Commit 4bc286e0 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Merge branch 'devel' into origin-devel

parents 1fa91774 363d2953
/**
\defgroup Filters
*/
/**
\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
*/
......@@ -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
......
#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_ */
......@@ -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:
......@@ -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;
......
/*
* 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