1 |
|
|
// |
2 |
|
|
// Copyright (c) 2016-2018 CNRS |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_check_hpp__ |
6 |
|
|
#define __pinocchio_check_hpp__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/multibody/model.hpp" |
9 |
|
|
#include "pinocchio/multibody/data.hpp" |
10 |
|
|
#include <boost/fusion/container/list.hpp> |
11 |
|
|
#include <boost/fusion/container/generation/make_list.hpp> |
12 |
|
|
|
13 |
|
|
#ifndef PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE |
14 |
|
|
#define PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE 5 |
15 |
|
|
#endif |
16 |
|
|
|
17 |
|
|
namespace pinocchio |
18 |
|
|
{ |
19 |
|
|
|
20 |
|
|
/// CRTP class describing the API of the checkers |
21 |
|
|
template<typename AlgorithmCheckerDerived> |
22 |
|
|
struct AlgorithmCheckerBase |
23 |
|
|
{ |
24 |
|
|
inline AlgorithmCheckerDerived& derived() |
25 |
|
|
{ return *static_cast< AlgorithmCheckerDerived*>(this); } |
26 |
|
|
|
27 |
|
24 |
inline const AlgorithmCheckerDerived& derived() const |
28 |
|
24 |
{ return *static_cast<const AlgorithmCheckerDerived*>(this); } |
29 |
|
|
|
30 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
31 |
|
24 |
inline bool checkModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const |
32 |
|
24 |
{ return derived().checkModel_impl(model); } |
33 |
|
|
}; |
34 |
|
|
|
35 |
|
|
#define PINOCCHIO_DEFINE_ALGO_CHECKER(NAME) \ |
36 |
|
|
struct NAME##Checker : public AlgorithmCheckerBase<NAME##Checker> \ |
37 |
|
|
{ \ |
38 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> \ |
39 |
|
|
inline bool checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> &) const; \ |
40 |
|
|
} |
41 |
|
|
|
42 |
|
|
/// Simple model checker, that assert that model.parents is indeed a tree. |
43 |
|
|
PINOCCHIO_DEFINE_ALGO_CHECKER(Parent); |
44 |
|
|
|
45 |
|
|
#if !defined(BOOST_FUSION_HAS_VARIADIC_LIST) |
46 |
|
|
/// Checker having a list of Checker as input argument |
47 |
|
|
template<BOOST_PP_ENUM_PARAMS_WITH_A_DEFAULT(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE,class D,boost::fusion::void_)> |
48 |
|
|
struct AlgorithmCheckerList: AlgorithmCheckerBase< AlgorithmCheckerList<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE,D)> > |
49 |
|
|
{ |
50 |
|
|
typedef typename boost::fusion::list<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE,D)> ArgType; |
51 |
|
|
|
52 |
|
|
AlgorithmCheckerList(const ArgType & checkerList) |
53 |
|
|
: checkerList(checkerList) {} |
54 |
|
|
|
55 |
|
|
// Calls model.check for each checker in the fusion::list. |
56 |
|
|
// Each list element is supposed to implement the AlgorithmCheckerBase API. |
57 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
58 |
|
|
bool checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const; |
59 |
|
|
|
60 |
|
|
const ArgType & checkerList; |
61 |
|
|
}; |
62 |
|
|
|
63 |
|
|
#define MAKE_ALGO_CHECKER_LIST(z,n,_) \ |
64 |
|
|
/**/ \ |
65 |
|
|
template<BOOST_PP_ENUM_PARAMS(BOOST_PP_INC(n),class D)> \ |
66 |
|
|
AlgorithmCheckerList<BOOST_PP_ENUM_PARAMS(BOOST_PP_INC(n),D)> makeAlgoCheckerList(BOOST_PP_ENUM_BINARY_PARAMS(BOOST_PP_INC(n) , D, const & arg)) \ |
67 |
|
|
{ return AlgorithmCheckerList<BOOST_PP_ENUM_PARAMS(BOOST_PP_INC(n),D)>(boost::fusion::make_list(BOOST_PP_ENUM_PARAMS(BOOST_PP_INC(n),arg))); } \ |
68 |
|
|
|
69 |
|
|
BOOST_PP_REPEAT(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE, MAKE_ALGO_CHECKER_LIST, BOOST_PP_EMPTY) |
70 |
|
|
#else |
71 |
|
|
template <class ...D> |
72 |
|
|
struct AlgorithmCheckerList: AlgorithmCheckerBase< AlgorithmCheckerList<D...> > |
73 |
|
|
{ |
74 |
|
|
typedef typename boost::fusion::list<D...> ArgType; |
75 |
|
|
|
76 |
|
4 |
AlgorithmCheckerList(const ArgType & checkerList) |
77 |
|
4 |
: checkerList(checkerList) {} |
78 |
|
|
|
79 |
|
|
// Calls model.check for each checker in the fusion::list. |
80 |
|
|
// Each list element is supposed to implement the AlgorithmCheckerBase API. |
81 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
82 |
|
|
bool checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const; |
83 |
|
|
|
84 |
|
|
const ArgType & checkerList; |
85 |
|
|
}; |
86 |
|
|
|
87 |
|
|
template <class ...T> |
88 |
|
2 |
AlgorithmCheckerList<T...> makeAlgoCheckerList(const T&... args) |
89 |
|
|
{ |
90 |
|
2 |
return AlgorithmCheckerList<T...>(boost::fusion::make_list(args...)); |
91 |
|
|
} |
92 |
|
|
|
93 |
|
|
#endif |
94 |
|
|
|
95 |
|
|
/// Check the validity of data wrt to model, in particular if model has been modified. |
96 |
|
|
/// |
97 |
|
|
/// \param[in] model reference model |
98 |
|
|
/// \param[in] data corresponding data |
99 |
|
|
/// |
100 |
|
|
/// \returns True if data is valid wrt model. |
101 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
102 |
|
|
inline bool checkData(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
103 |
|
|
const DataTpl<Scalar,Options,JointCollectionTpl> & data); |
104 |
|
|
|
105 |
|
|
} // namespace pinocchio |
106 |
|
|
|
107 |
|
|
|
108 |
|
|
/* --- Details -------------------------------------------------------------------- */ |
109 |
|
|
#include "pinocchio/algorithm/check.hxx" |
110 |
|
|
|
111 |
|
|
#endif // ifndef __pinocchio_check_hpp__ |