Skip to content

Commit 19c9096

Browse files
fbaillyolivier-stasse
fbailly
authored and
olivier-stasse
committed
[topic_robotUtils] Added class RobotUtil and signal-helper.hh and logger.hh from sot-torque-control. Extended matrix-geometry.hh to include definitions from sot-torque-control/vector-conversions.hh
1 parent 76516a9 commit 19c9096

File tree

7 files changed

+1198
-0
lines changed

7 files changed

+1198
-0
lines changed

‎include/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -89,6 +89,9 @@ SET(NEWHEADERS
8989
sot/core/trajectory.hh
9090
sot/core/switch.hh
9191
sot/core/variadic-op.hh
92+
sot/core/logger.hh
93+
sot/core/signal-helper.hh
94+
sot/core/robot-utils.hh
9295
)
9396
INSTALL(FILES ${NEWHEADERS}
9497
DESTINATION include/sot/core

‎include/sot/core/logger.hh‎

Lines changed: 227 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,227 @@
1+
/*
2+
* Copyright 2015, Andrea Del Prete, LAAS-CNRS
3+
*
4+
* This file is part of sot-torque-control.
5+
* sot-torque-control is free software: you can redistribute it and/or
6+
* modify it under the terms of the GNU Lesser General Public License
7+
* as published by the Free Software Foundation, either version 3 of
8+
* the License, or (at your option) any later version.
9+
* sot-torque-control is distributed in the hope that it will be
10+
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11+
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12+
* GNU Lesser General Public License for more details. You should
13+
* have received a copy of the GNU Lesser General Public License along
14+
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
15+
*/
16+
17+
#ifndef __sot_torque_control_logger_H__
18+
#define __sot_torque_control_logger_H__
19+
20+
/* --------------------------------------------------------------------- */
21+
/* --- API ------------------------------------------------------------- */
22+
/* --------------------------------------------------------------------- */
23+
24+
#if defined (WIN32)
25+
# if defined (logger_EXPORTS)
26+
# define LOGGER_EXPORT __declspec(dllexport)
27+
# else
28+
# define LOGGER_EXPORT __declspec(dllimport)
29+
# endif
30+
#else
31+
# define LOGGER_EXPORT
32+
#endif
33+
34+
35+
/* --------------------------------------------------------------------- */
36+
/* --- INCLUDE --------------------------------------------------------- */
37+
/* --------------------------------------------------------------------- */
38+
39+
#include <sot/core/signal-helper.hh>
40+
#include <sot/core/matrix-geometry.hh>
41+
#include <map>
42+
#include <iomanip> // std::setprecision
43+
#include "boost/assign.hpp"
44+
45+
46+
namespace dynamicgraph {
47+
namespace sot {
48+
49+
//#define LOGGER_VERBOSITY_INFO_WARNING_ERROR
50+
#define LOGGER_VERBOSITY_ALL
51+
52+
#define SEND_MSG(msg,type) sendMsg(msg,type,__FILE__,__LINE__)
53+
54+
#ifdef LOGGER_VERBOSITY_ERROR
55+
#define SEND_DEBUG_STREAM_MSG(msg)
56+
#define SEND_INFO_STREAM_MSG(msg)
57+
#define SEND_WARNING_STREAM_MSG(msg)
58+
#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
59+
#endif
60+
61+
#ifdef LOGGER_VERBOSITY_WARNING_ERROR
62+
#define SEND_DEBUG_STREAM_MSG(msg)
63+
#define SEND_INFO_STREAM_MSG(msg)\
64+
#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
65+
#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
66+
#endif
67+
68+
#ifdef LOGGER_VERBOSITY_INFO_WARNING_ERROR
69+
#define SEND_DEBUG_STREAM_MSG(msg)
70+
#define SEND_INFO_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_INFO_STREAM)
71+
#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
72+
#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
73+
#endif
74+
75+
#ifdef LOGGER_VERBOSITY_ALL
76+
#define SEND_DEBUG_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_DEBUG_STREAM)
77+
#define SEND_INFO_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_INFO_STREAM)
78+
#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
79+
#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
80+
#endif
81+
82+
/** Enum representing the different kind of messages.
83+
*/
84+
enum MsgType
85+
{
86+
MSG_TYPE_DEBUG =0,
87+
MSG_TYPE_INFO =1,
88+
MSG_TYPE_WARNING =2,
89+
MSG_TYPE_ERROR =3,
90+
MSG_TYPE_DEBUG_STREAM =4,
91+
MSG_TYPE_INFO_STREAM =5,
92+
MSG_TYPE_WARNING_STREAM =6,
93+
MSG_TYPE_ERROR_STREAM =7
94+
};
95+
96+
template<typename T>
97+
std::string toString(const T& v, const int precision=3, const int width=-1)
98+
{
99+
std::stringstream ss;
100+
if(width>precision)
101+
ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v;
102+
else
103+
ss<<std::fixed<<std::setprecision(precision)<<v;
104+
return ss.str();
105+
}
106+
107+
template<typename T>
108+
std::string toString(const std::vector<T>& v, const int precision=3, const int width=-1,
109+
const std::string separator=", ")
110+
{
111+
std::stringstream ss;
112+
if(width>precision)
113+
{
114+
for(int i=0; i<v.size()-1; i++)
115+
ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[i]<<separator;
116+
ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[v.size()-1];
117+
}
118+
else
119+
{
120+
for(int i=0; i<v.size()-1; i++)
121+
ss<<std::fixed<<std::setprecision(precision)<<v[i]<<separator;
122+
ss<<std::fixed<<std::setprecision(precision)<<v[v.size()-1];
123+
}
124+
125+
return ss.str();
126+
}
127+
128+
// template<typename T, int N>
129+
// std::string toString(const Eigen::Matrix<T, N, 1, 0, N, 1>& v, const std::string separator=", ",
130+
// const int precision=3, const int width=-1)
131+
template<typename T>
132+
std::string toString(const Eigen::MatrixBase<T>& v,
133+
const int precision=3, const int width=-1, const std::string separator=", ")
134+
{
135+
std::stringstream ss;
136+
if(width>precision)
137+
{
138+
for(int i=0; i<v.size()-1; i++)
139+
ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[i]<<separator;
140+
ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[v.size()-1];
141+
}
142+
else
143+
{
144+
for(int i=0; i<v.size()-1; i++)
145+
ss<<std::fixed<<std::setprecision(precision)<<v[i]<<separator;
146+
ss<<std::setprecision(precision)<<v[v.size()-1];
147+
}
148+
149+
return ss.str();
150+
}
151+
152+
enum LoggerVerbosity
153+
{
154+
VERBOSITY_ALL,
155+
VERBOSITY_INFO_WARNING_ERROR,
156+
VERBOSITY_WARNING_ERROR,
157+
VERBOSITY_ERROR,
158+
VERBOSITY_NONE
159+
};
160+
161+
/** A simple class for logging messages
162+
*/
163+
class Logger
164+
{
165+
public:
166+
167+
/** Constructor */
168+
Logger(double timeSample=0.001, double streamPrintPeriod=1.0);
169+
170+
/** Destructor */
171+
~Logger(){}
172+
173+
/** Method to be called at every control iteration
174+
* to decrement the internal Logger's counter. */
175+
void countdown();
176+
177+
/** Print the specified message on standard output if the verbosity level
178+
* allows it. The file name and the line number are used to identify
179+
* the point where sendMsg is called so that streaming messages are
180+
* printed only every streamPrintPeriod iterations.
181+
*/
182+
void sendMsg(std::string msg, MsgType type, const char* file="", int line=0);
183+
184+
/** Set the sampling time at which the method countdown()
185+
* is going to be called. */
186+
bool setTimeSample(double t);
187+
188+
/** Set the time period for printing of streaming messages. */
189+
bool setStreamPrintPeriod(double s);
190+
191+
/** Set the verbosity level of the logger. */
192+
void setVerbosity(LoggerVerbosity lv);
193+
194+
protected:
195+
LoggerVerbosity m_lv; /// verbosity of the logger
196+
double m_timeSample; /// specify the period of call of the countdown method
197+
double m_streamPrintPeriod; /// specify the time period of the stream prints
198+
double m_printCountdown; /// every time this is < 0 (i.e. every _streamPrintPeriod sec) print stuff
199+
200+
/** Pointer to the dynamic structure which holds the collection of streaming messages */
201+
std::map<std::string, double> m_stream_msg_counters;
202+
203+
bool isStreamMsg(MsgType m)
204+
{ return m==MSG_TYPE_ERROR_STREAM || m==MSG_TYPE_DEBUG_STREAM || m==MSG_TYPE_INFO_STREAM || m==MSG_TYPE_WARNING_STREAM; }
205+
206+
bool isDebugMsg(MsgType m)
207+
{ return m==MSG_TYPE_DEBUG_STREAM || m==MSG_TYPE_DEBUG; }
208+
209+
bool isInfoMsg(MsgType m)
210+
{ return m==MSG_TYPE_INFO_STREAM || m==MSG_TYPE_INFO; }
211+
212+
bool isWarningMsg(MsgType m)
213+
{ return m==MSG_TYPE_WARNING_STREAM || m==MSG_TYPE_WARNING; }
214+
215+
bool isErrorMsg(MsgType m)
216+
{ return m==MSG_TYPE_ERROR_STREAM || m==MSG_TYPE_ERROR; }
217+
};
218+
219+
/** Method to get the logger (singleton). */
220+
Logger& getLogger();
221+
222+
} // namespace sot
223+
} // namespace dynamicgraph
224+
225+
226+
227+
#endif // #ifndef __sot_torque_control_logger_H__

‎include/sot/core/matrix-geometry.hh‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,54 @@
3434
/* --------------------------------------------------------------------- */
3535
/* --------------------------------------------------------------------- */
3636
/* --------------------------------------------------------------------- */
37+
38+
namespace Eigen
39+
{
40+
#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
41+
/** \ingroup matrixtypedefs */ \
42+
typedef Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
43+
/** \ingroup matrixtypedefs */ \
44+
typedef Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix; \
45+
/** \ingroup matrixtypedefs */ \
46+
typedef Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;
47+
48+
#define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \
49+
/** \ingroup matrixtypedefs */ \
50+
typedef Matrix<Type, Size, Dynamic> Matrix##Size##X##TypeSuffix; \
51+
/** \ingroup matrixtypedefs */ \
52+
typedef Matrix<Type, Dynamic, Size> Matrix##X##Size##TypeSuffix;
53+
54+
#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
55+
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
56+
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5) \
57+
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6) \
58+
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 7, 7) \
59+
EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 1) \
60+
EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5) \
61+
EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6) \
62+
EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7)
63+
64+
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
65+
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
66+
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
67+
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
68+
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
69+
70+
#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
71+
#undef EIGEN_MAKE_TYPEDEFS
72+
73+
typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRXd;
74+
typedef Map<MatrixRXd> SigMatrixXd;
75+
typedef Map<VectorXd> SigVectorXd;
76+
typedef const Map<const MatrixRXd> const_SigMatrixXd;
77+
typedef const Map<const VectorXd> const_SigVectorXd;
78+
79+
typedef Eigen::Ref<Eigen::VectorXd> RefVector;
80+
typedef const Eigen::Ref<const Eigen::VectorXd>& ConstRefVector;
81+
typedef Eigen::Ref<Eigen::MatrixXd> RefMatrix;
82+
typedef const Eigen::Ref<const Eigen::MatrixXd> ConstRefMatrix;
83+
}
84+
3785
namespace dynamicgraph {
3886
namespace sot {
3987
typedef Eigen::Transform<double,3, Eigen::Affine> SOT_CORE_EXPORT MatrixHomogeneous;

0 commit comments

Comments
 (0)