Go to the documentation of this file.
12 #ifndef STATEOBSERVATIONDEFINITIONSHPP
13 #define STATEOBSERVATIONDEFINITIONSHPP
22 #ifdef STATEOBSERVATION_VERBOUS_CONSTRUCTORS
26 #include <boost/assert.hpp>
27 #include <boost/timer/timer.hpp>
30 #include <Eigen/Geometry>
33 # include <boost/math/constants/constants.hpp>
34 # define M_PI boost::math::constants::pi<double>()
37 #include <state-observation/api.h>
50 static constexpr
bool value = std::is_base_of<Eigen::EigenBase<T>, T>
::value;
60 template<
typename _Scalar,
int _Rows,
int _Cols,
int _Options,
int _MaxRows,
int _MaxCols>
61 struct isMatrix<Eigen::
Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>> : std::true_type
66 struct EigenType : std::enable_if<isEigen<T>::value, T>
71 struct MatrixType : std::enable_if<isMatrix<T>::value, T>
79 typedef Eigen::Matrix<double, 1, 1>
Vector1;
82 typedef Eigen::Matrix<double, 2, 1>
Vector2;
94 typedef Eigen::Matrix<double, 5, 1>
Vector5;
97 typedef Eigen::Matrix<double, 6, 1>
Vector6;
143 static const bool isDebug =
true;
145 static const bool isDebug =
false;
148 template<
int compileTimeRows = -1,
int compileTimeCols = -1>
152 template<
typename CustomNullaryOp>
153 static const Eigen::CwiseNullaryOp<CustomNullaryOp, typename Eigen::Matrix<double, compileTimeRows, compileTimeCols>>
160 return Eigen::Matrix<double, compileTimeRows, compileTimeCols>::NullaryExpr(func);
164 template<
int compileTimeCols>
168 template<
typename CustomNullaryOp>
170 const CustomNullaryOp & func,
172 Index cols = compileTimeCols)
174 return Eigen::Matrix<double, -1, compileTimeCols>::NullaryExpr(rows, cols, func);
178 template<
int compileTimeRows>
182 template<
typename CustomNullaryOp>
184 const CustomNullaryOp & func,
185 Index rows = compileTimeRows,
188 return Eigen::Matrix<double, compileTimeRows, -1>::NullaryExpr(rows, cols, func);
196 template<
typename CustomNullaryOp>
198 const CustomNullaryOp & func,
202 return Eigen::Matrix<double, -1, -1>::NullaryExpr(rows, cols, func);
206 template<
typename MatrixT>
208 MatrixType<MatrixT>::type::ColsAtCompileTime>
214 template<
typename T, const T defaultValue = T()>
221 template<
typename T, const T defaultValue>
235 template<errorType i = message,
int dummy = 0>
244 static const char *
v;
251 static const std::runtime_error
v;
258 static const std::runtime_error *
v;
263 If this happened during initialization then run command chckitm_set() \
264 to switch it to set. And if the initialization is incomplete, run \
265 chckitm_reset() afterwards.";
280 const Vector & stateVector2,
287 template<
typename T,
typename defaultValue = DebugItemDefaultValue<T>,
bool debug = true>
297 inline operator T()
const
315 template<
typename T,
typename defaultValue>
326 inline operator T()
const
328 return defaultValue::v;
332 return defaultValue::v;
336 return defaultValue::v;
374 bool alwaysCheck =
false,
375 bool assertion =
true,
376 bool eigenAlignedNew =
false,
389 inline operator T()
const;
390 inline operator const T &()
const;
400 inline bool isSet()
const;
404 inline void set(
bool value);
432 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(eigenAlignedNew)
449 inline static constexpr
char errorMessage[] =
"Matrix contains a NaN.";
468 template<
typename MatrixType = Matrix,
bool lazy = false>
484 inline void set(
bool value =
true);
499 inline bool isSet()
const;
504 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
509 inline bool check_()
const;
526 template<
typename MatrixType = Matrix,
typename Allocator = std::allocator<MatrixType>>
539 typedef std::vector<MatrixType, Allocator>
Array;
629 void writeInFile(
const char * filename,
bool clear =
false,
bool append =
false);
636 void writeInFile(
const std::string & filename,
bool clear =
false,
bool append =
false);
639 typedef std::deque<MatrixType, Allocator>
Deque;
647 inline void check_()
const;
672 constexpr
double epsilon1 = std::numeric_limits<double>::epsilon();
703 using clock =
typename std::conditional<std::chrono::high_resolution_clock::is_steady,
704 std::chrono::high_resolution_clock,
705 std::chrono::steady_clock>::type;
711 startTime = clock::now();
718 auto elapsed = clock::now() - startTime;
719 return static_cast<double>(elapsed.count());
734 #include <state-observation/tools/definitions.hxx>
737 #endif // STATEOBSERVATIONDEFINITIONSHPP
bool isSet() const
Says whether the matrix is initialized or not.
Eigen::VectorXd Vector
Dynamic sized scalar vector.
Definition: definitions.hpp:76
IndexedMatrixT< Vector3 > IndexedVector3
Definition: definitions.hpp:516
Checks if a class is a specialization of Eigen::Matrix.
Definition: definitions.hpp:56
ExceptionPtr exceptionPtr_
Definition: definitions.hpp:426
void STATE_OBSERVATION_DLLAPI defaultSum(const Vector &stateVector, const Vector &tangentVector, Vector &sum)
TimeIndex setLastIndex(int index)
Set the time index of the last element.
IndexedMatrixT & set(const MatrixType &v, TimeIndex k)
Set the value of the matrix and the time sample.
void writeInFile(const char *filename, bool clear=false, bool append=false)
IndexedMatrixT< Matrix3 > IndexedMatrix3
Definition: definitions.hpp:517
DebugItem(T)
Definition: definitions.hpp:320
DebugItem< const std::exception *, detail::defaultExceptionAddr, do_exception_ > ExceptionPtr
Definition: definitions.hpp:422
Eigen::Matrix4d Matrix4
4x4 Scalar Matrix
Definition: definitions.hpp:115
boost::timer::cpu_timer cpu_timer
Definition: definitions.hpp:695
TimeIndex getNextIndex() const
Definition: definitions.hpp:215
Definition: definitions.hpp:71
Checks if it is derived from EigenBase (the base class of all dense functions)
Definition: definitions.hpp:48
Eigen::Matrix< double, 6, 6 > Matrix6
6x6 Scalar Matrix
Definition: definitions.hpp:121
void setAssertMessage(std::string s)
CheckedItem< Vector3, false, false, true, true, CheckNaN > CheckedVector3
Definition: definitions.hpp:456
errorType
Definition: definitions.hpp:228
void reset()
Switch off the initalization flag, the value is no longer accessible.
TimeIndex getTime() const
Get the time index.
void STATE_OBSERVATION_DLLAPI defaultDifference(const Vector &stateVector1, const Vector &stateVector2, Vector &difference)
CheckedItem< Vector6, false, false, true, true, CheckNaN > CheckedVector6
Definition: definitions.hpp:457
DebugItem< const char *, detail::defaultErrorMSG, do_assert_ > AssertMsg
Definition: definitions.hpp:421
bool isApproxAbs(double a, double b, double absolutePrecision=cst::epsilon1)
checks if two scalars have approximately the same value up to a given absolute precision
const T & chckitm_getValue() const
bool isApprox(double a, double b, double relativePrecision=cst::epsilon1)
checks if two scalars have approximately the same value up to a given relative precision
Eigen::Matrix< double, 2, 1 > Vector2
2d Vector
Definition: definitions.hpp:82
Eigen::Matrix< double, 5, 5 > Matrix5
5x5 Scalar Matrix
Definition: definitions.hpp:118
T set(const T &)
Definition: definitions.hpp:330
@ message
Definition: definitions.hpp:230
Definition: definitions.hpp:66
void resize(TimeSize i, const MatrixType &m=MatrixType())
resizes the array
IndexedMatrixT< Vector > IndexedVector
Definition: definitions.hpp:515
IndexedMatrixArrayT< Vector > IndexedVectorArray
Definition: definitions.hpp:659
void setValue(const MatrixType &v, TimeIndex k)
T & operator=(T v)
Definition: definitions.hpp:293
const Vector gravity
Gravity Vector along Z.
Definition: definitions.hpp:666
void truncateAfter(TimeIndex timeIndex)
removes all the elements with larger indexes than timeIndex
This class describes a structure that enables to store array of matrices with time indexation.
Definition: definitions.hpp:527
Index TimeSize
Definition: definitions.hpp:140
CheckedItem & operator=(const CheckedItem &)
Definition: definitions.hpp:236
DebugItemDefaultError< exception > defaultException
Definition: definitions.hpp:275
static const std::runtime_error * v
Definition: definitions.hpp:258
static const bool do_exception_
Definition: definitions.hpp:418
std::vector< MatrixType, Allocator > Array
Definition: definitions.hpp:539
MatrixType operator[](TimeIndex index) const
gets the value with the given time index
T get() const
Definition: definitions.hpp:334
static const char * v
Definition: definitions.hpp:244
IsSet isSet_
Definition: definitions.hpp:424
Eigen::MatrixXd Matrix
Dynamic sized Matrix.
Definition: definitions.hpp:100
Definition: definitions.hpp:288
Eigen::Matrix2d Matrix2
2D scalar Matrix
Definition: definitions.hpp:106
T & operator=(T v)
Definition: definitions.hpp:321
Additional checker that allows to check for the presence of NaN values in the item.
Definition: definitions.hpp:436
DebugItem()
Definition: definitions.hpp:291
DebugItem()
Definition: definitions.hpp:319
long int TimeIndex
Definition: definitions.hpp:139
MatrixType v_
Definition: definitions.hpp:511
void setExceptionPtr(std::exception *e)
static bool check(const T &)
Definition: definitions.hpp:349
CheckedItem< Matrix3, false, false, true, true, CheckNaN > CheckedMatrix3
Definition: definitions.hpp:453
boost::timer::cpu_timer cpu_times
Definition: definitions.hpp:696
void setIndex(TimeIndex index)
set the index of the matrix
T get() const
Definition: definitions.hpp:305
Eigen::Matrix3d Matrix3
3x3 Scalar Matrix
Definition: definitions.hpp:109
static bool checkAssert(const T &m)
Definition: definitions.hpp:444
static constexpr bool value
Definition: definitions.hpp:50
constexpr double m
mass of the robot
Definition: hrp2.hpp:36
Eigen::Matrix< double, 5, 1 > Vector5
5D vector
Definition: definitions.hpp:94
bool checkIndex(TimeIndex k) const
checks whether the index is present in the array
static const T v
Definition: definitions.hpp:218
IndexedMatrixArrayT()
Default constructor.
void truncateBefore(TimeIndex timeIndex)
removes all the elements with smaller indexes than timeIndex
Eigen::Matrix< double, 6, 1 > Vector6
6D vector
Definition: definitions.hpp:97
Eigen::Index Index
Definition: definitions.hpp:138
const MatrixType & back() const
gets the last value
DebugItemDefaultError< exceptionAddr > defaultExceptionAddr
Definition: definitions.hpp:276
Eigen::Quaternion< double, Eigen::DontAlign > QuaternionUnaligned
Quaternion Unaligned.
Definition: definitions.hpp:130
const MatrixType & operator()() const
Get the matrix value (const version)
std::deque< MatrixType, Allocator > Deque
Definition: definitions.hpp:639
static constexpr char errorMessage[]
Definition: definitions.hpp:358
Eigen::AngleAxis< double > AngleAxis
Euler Axis/Angle representation of orientation.
Definition: definitions.hpp:133
DebugItemDefaultValue< bool, true > defaultTrue
Definition: definitions.hpp:226
this is a structure allowing for automatically verifying that the item has been initialized or not....
Definition: definitions.hpp:378
static const bool do_assert_
Definition: definitions.hpp:417
void clear()
Clears the vector but keeps the last index.
Eigen::Matrix< double, 3, 1, Eigen::DontAlign > Vector3Unaligned
3D vector unaligned
Definition: definitions.hpp:88
boost::timer::auto_cpu_timer auto_cpu_timer
Definition: definitions.hpp:694
void popFront()
removes the first (oldest) element of the array
DebugItem(const T &v)
Definition: definitions.hpp:292
IndexedMatrixArrayT< Matrix > IndexedMatrixArray
Definition: definitions.hpp:658
static const std::runtime_error v
Definition: definitions.hpp:251
CheckedItem< Matrix6, false, false, true, true, CheckNaN > CheckedMatrix6
Definition: definitions.hpp:454
IndexedMatrixT< Matrix > IndexedMatrix
Definition: definitions.hpp:514
void readVectorsFromFile(const std::string &filename, bool withTimeStamp=true)
const MatrixType & front() const
gets the first value
std::runtime_error ExceptionT
Definition: definitions.hpp:450
Eigen::Matrix< double, 12, 12 > Matrix12
12x12 scalar Matrix
Definition: definitions.hpp:124
Array getArray() const
converts the array into a standard vector
constexpr double gravityConstant
Definition: definitions.hpp:663
constexpr double epsilon1
number considered zero when compared to 1
Definition: definitions.hpp:672
Eigen::Matrix< double, 3, 3, Eigen::DontAlign > Matrix3Unaligned
3x3 Scalar Matrix Unaligned
Definition: definitions.hpp:112
void checkNext_(TimeIndex time) const
static bool check(const T &m)
Definition: definitions.hpp:439
constexpr double epsilonAngle
angles considered Zero
Definition: definitions.hpp:669
This structure is used as an additionalChecker for a CheckedItem that doesn't require additional test...
Definition: definitions.hpp:346
DebugItemDefaultError< message > defaultErrorMSG
Definition: definitions.hpp:274
Eigen::Vector3d Vector3
3D vector
Definition: definitions.hpp:85
@ exceptionAddr
Definition: definitions.hpp:232
Eigen::Quaterniond Quaternion
Quaternion.
Definition: definitions.hpp:127
DebugItem< bool, detail::defaultTrue, do_check_ > IsSet
Definition: definitions.hpp:420
@ exception
Definition: definitions.hpp:231
Eigen::Rotation2D< double > Rotation2D
2D rotations
Definition: definitions.hpp:136
T v_
this can throw(std::exception)
Definition: definitions.hpp:429
TimeIndex setFirstIndex(int index)
set the time index of the first element
void pushBack(const MatrixType &v)
Pushes back the matrix to the array, the new value will take the next time.
This class describes a structure composed by a matrix of a given size and a time-index parameter....
Definition: definitions.hpp:469
Definition: bidim-elastic-inv-pendulum-dyn-sys.hpp:20
CheckedItem< Matrix12, false, false, true, true, CheckNaN > CheckedMatrix12
Definition: definitions.hpp:455
Deque v_
Definition: definitions.hpp:655
T set(const T &v)
Definition: definitions.hpp:301
TimeIndex k_
Definition: definitions.hpp:653
Eigen::Vector4d Vector4
4D vector
Definition: definitions.hpp:91
IndexedMatrixT()
Default constructor.
CheckedItem(bool initialize=true)
The parameter initialize sets whether the isSet() parameter is initialized to false.
TimeIndex getFirstIndex() const
Get the time index.
AssertMsg assertMsg_
Definition: definitions.hpp:425
static const bool do_check_
Definition: definitions.hpp:416
TimeIndex k_
Definition: definitions.hpp:510
std::runtime_error ExceptionT
Definition: definitions.hpp:359
bool chckitm_check_() const
virtual ~CheckedItem()
Definition: definitions.hpp:385
TimeIndex getLastIndex() const
Get the time index.
void readFromFile(const char *filename, Index rows, Index cols=1, bool withTimeStamp=true)
Eigen::Matrix< double, 1, 1 > Matrix1
1D scalar Matrix
Definition: definitions.hpp:103
CheckedItem< Quaternion, false, false, true, true > CheckedQuaternion
Definition: definitions.hpp:458
Eigen::Matrix< double, 1, 1 > Vector1
1D Vector
Definition: definitions.hpp:79
static constexpr char errorMessage[]
Definition: definitions.hpp:449
static bool checkAssert(const T &)
Definition: definitions.hpp:354