diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2ab62046732338a9a8107ecaf3baf04917de33be..fdfa254a8af06fe498c333785777ff966048d377 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -191,6 +191,9 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
algorithm/frames.hpp
algorithm/compute-all-terms.hpp
algorithm/copy.hpp
+ algorithm/check.hpp
+ algorithm/check.hxx
+ algorithm/default-check.hpp
)
SET(${PROJECT_NAME}_PARSERS_HEADERS
diff --git a/src/algorithm/aba.hpp b/src/algorithm/aba.hpp
index cb5314ffe933df4c45c9dfff7dd25d7e77528b48..77b4dc6f3022eb61192693d27d7baf78dfcfc70e 100644
--- a/src/algorithm/aba.hpp
+++ b/src/algorithm/aba.hpp
@@ -19,6 +19,7 @@
#define __se3_aba_hpp__
#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/check.hpp"
namespace se3
{
@@ -40,6 +41,8 @@ namespace se3
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau);
+ DEFINE_ALGO_CHECKER(ABA);
+
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
diff --git a/src/algorithm/aba.hxx b/src/algorithm/aba.hxx
index e0d175532dcee8a80913342ad64d386aaaa4d961..6b49a33a4ac8db993b8704474a41b0cd6230b043 100644
--- a/src/algorithm/aba.hxx
+++ b/src/algorithm/aba.hxx
@@ -217,6 +217,24 @@ namespace se3
return data.ddq;
}
+
+ // --- CHECKER ---------------------------------------------------------------
+ // --- CHECKER ---------------------------------------------------------------
+ // --- CHECKER ---------------------------------------------------------------
+
+ // Check whether all masses are nonzero and diagonal of inertia is nonzero
+ // The second test is overconstraining.
+ inline bool ABAChecker::checkModel_impl( const Model& model ) const
+ {
+ for(JointIndex j=1;int(j)<model.njoint;j++)
+ if( (model.inertias[j].mass () < 1e-5)
+ || (model.inertias[j].inertia().data()[0] < 1e-5)
+ || (model.inertias[j].inertia().data()[3] < 1e-5)
+ || (model.inertias[j].inertia().data()[5] < 1e-5) )
+ return false;
+ return true;
+ }
+
} // namespace se3
/// @endcond
diff --git a/src/algorithm/check.hpp b/src/algorithm/check.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a4a2dca2ac0b9ae0734e1997f788bd0b79effe80
--- /dev/null
+++ b/src/algorithm/check.hpp
@@ -0,0 +1,58 @@
+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio 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.
+//
+// Pinocchio 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
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_check_hpp__
+#define __se3_check_hpp__
+
+namespace se3
+{
+
+ /// CRTP class describing the API of the checkers
+ template<typename AlgorithmCheckerDerived>
+ struct AlgorithmCheckerBase
+ {
+ inline AlgorithmCheckerDerived& derived()
+ { return *static_cast< AlgorithmCheckerDerived*>(this); }
+
+ inline const AlgorithmCheckerDerived& derived() const
+ { return *static_cast<const AlgorithmCheckerDerived*>(this); }
+
+ inline bool checkModel(const Model & model) const { return derived().checkModel_impl(model); }
+ };
+
+#define DEFINE_ALGO_CHECKER(NAME) \
+ struct NAME##Checker : public AlgorithmCheckerBase<NAME##Checker> \
+ { \
+ inline bool checkModel_impl( const Model& ) const; \
+ }
+
+ /// Simple model checker, that assert that model.parents is indeed a tree.
+ DEFINE_ALGO_CHECKER(Parent);
+
+ /// Check the validity of data wrt to model, in particular if model has been modified.
+ ///
+ /// \param[in] model reference model
+ /// \param[in] data corresponding data
+ inline bool checkData(const Model & model, const Data & data);
+
+} // namespace se3
+
+
+ /* --- Details -------------------------------------------------------------------- */
+#include "pinocchio/algorithm/check.hxx"
+
+#endif // ifndef __se3_check_hpp__
diff --git a/src/algorithm/check.hxx b/src/algorithm/check.hxx
new file mode 100644
index 0000000000000000000000000000000000000000..b029859e079ffec30632d7b7ebc527001deef79f
--- /dev/null
+++ b/src/algorithm/check.hxx
@@ -0,0 +1,139 @@
+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio 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.
+//
+// Pinocchio 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
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_check_hxx__
+#define __se3_check_hxx__
+
+#include <boost/fusion/algorithm.hpp>
+#include <boost/foreach.hpp>
+
+namespace se3
+{
+ namespace internal
+ {
+ // Dedicated structure for the fusion::accumulate algorithm: validate the check-algorithm
+ // for all elements in a fusion list of AlgoCheckers.
+ struct AlgoFusionChecker
+ {
+ typedef bool result_type;
+ const Model& model;
+
+ AlgoFusionChecker(const Model&model) : model(model) {}
+
+ template<typename T>
+ inline bool operator()(const bool& accumul, const T& t) const
+ { return accumul && t.checkModel(model); }
+ };
+ } // namespace internal
+
+ // Calls model.check for each checker in the fusion::list.
+ // Each list element is supposed to implement the AlgorithmCheckerBase API.
+ template<class T1,class T2,class T3,class T4,class T5,
+ class T6,class T7,class T8,class T9,class T10>
+ inline bool Model::check( const boost::fusion::list<T1,T2,T3,T4,T5,T6,T7,T8,T9,T10> & checkerList ) const
+ { return boost::fusion::accumulate(checkerList,true,internal::AlgoFusionChecker(*this)); }
+
+ // Check the validity of the kinematic tree defined by parents.
+ inline bool ParentChecker::checkModel_impl( const Model& model ) const
+ {
+ for( JointIndex j=1;(int)j<model.njoint;++j )
+ if( model.parents[j]>=j ) return false;
+
+ return true;
+ }
+
+ inline bool checkData(const Model & model, const Data & data)
+ {
+#define CHECK_DATA(a) if(!(a)) return false;
+
+ // TODO JMinvJt,sDUiJt are never explicitly initialized.
+ // TODO impulse_c
+ // They are not check neither
+
+ CHECK_DATA( (int)data.joints.size() == model.njoint );
+ CHECK_DATA( (int)data.a.size() == model.njoint );
+ CHECK_DATA( (int)data.a_gf.size() == model.njoint );
+ CHECK_DATA( (int)data.v.size() == model.njoint );
+ CHECK_DATA( (int)data.f.size() == model.njoint );
+ CHECK_DATA( (int)data.oMi.size() == model.njoint );
+ CHECK_DATA( (int)data.liMi.size() == model.njoint );
+ CHECK_DATA( (int)data.Ycrb.size() == model.njoint );
+ CHECK_DATA( (int)data.Yaba.size() == model.njoint );
+ CHECK_DATA( (int)data.Fcrb.size() == model.njoint );
+ BOOST_FOREACH(const Data::Matrix6x & F,data.Fcrb) CHECK_DATA( F.cols() == model.nv );
+ CHECK_DATA( (int)data.iMf.size() == model.njoint );
+ CHECK_DATA( (int)data.iMf.size() == model.njoint );
+ CHECK_DATA( (int)data.com.size() == model.njoint );
+ CHECK_DATA( (int)data.vcom.size() == model.njoint );
+ CHECK_DATA( (int)data.acom.size() == model.njoint );
+ CHECK_DATA( (int)data.mass.size() == model.njoint );
+
+ CHECK_DATA( data.tau.size() == model.nv );
+ CHECK_DATA( data.nle.size() == model.nv );
+ CHECK_DATA( data.ddq.size() == model.nv );
+ CHECK_DATA( data.u.size() == model.nv );
+ CHECK_DATA( data.M.rows() == model.nv );
+ CHECK_DATA( data.M.cols() == model.nv );
+ CHECK_DATA( data.Ag.cols() == model.nv );
+ CHECK_DATA( data.U.cols() == model.nv );
+ CHECK_DATA( data.U.rows() == model.nv );
+ CHECK_DATA( data.D.size() == model.nv );
+ CHECK_DATA( data.tmp.size() == model.nv );
+ CHECK_DATA( data.J.cols() == model.nv );
+ CHECK_DATA( data.Jcom.cols() == model.nv );
+ CHECK_DATA( data.torque_residual.size() == model.nv );
+ CHECK_DATA( data.dq_after.size() == model.nv );
+ //CHECK_DATA( data.impulse_c.size()== model.nv );
+
+ CHECK_DATA( (int)data.oMf.size() == model.nFrames );
+
+ CHECK_DATA( (int)data.lastChild.size() == model.njoint );
+ CHECK_DATA( (int)data.nvSubtree.size() == model.njoint );
+ CHECK_DATA( (int)data.parents_fromRow.size() == model.nv );
+ CHECK_DATA( (int)data.nvSubtree_fromRow.size() == model.nv );
+
+ for( JointIndex j=1;int(j)<model.njoint;++j )
+ {
+ JointIndex c = data.lastChild[j];
+ CHECK_DATA((int)c<model.njoint);
+ int nv=model.joints[j].nv();
+ for( JointIndex d=j+1;d<=c;++d ) // explore all descendant
+ {
+ CHECK_DATA( model.parents[d]>=j );
+ nv+=model.joints[d].nv();
+ }
+ CHECK_DATA(nv==data.nvSubtree[j]);
+
+ for( JointIndex d=c+1;(int)d<model.njoint;++d)
+ CHECK_DATA( (model.parents[d]<j)||(model.parents[d]>c) );
+
+ int row = model.joints[j].idx_v();
+ CHECK_DATA(data.nvSubtree[j] == data.nvSubtree_fromRow[row]);
+
+ const JointModel & jparent = model.joints[model.parents[j]];
+ if(row==0) { CHECK_DATA(data.parents_fromRow[row]==-1); }
+ else { CHECK_DATA(jparent.idx_v()+jparent.nv()-1 == data.parents_fromRow[row]); }
+ }
+
+#undef CHECK_DATA
+ return true;
+ }
+
+
+} // namespace se3
+
+#endif // ifndef __se3_check_hxx__
diff --git a/src/algorithm/crba.hpp b/src/algorithm/crba.hpp
index 642cdcfbc1cefcfc6b764ee2951d29b2344fdb51..b708c68c9d9eb7b2e0425165c22320e891c15059 100644
--- a/src/algorithm/crba.hpp
+++ b/src/algorithm/crba.hpp
@@ -19,6 +19,7 @@
#define __se3_crba_hpp__
#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/check.hpp"
namespace se3
{
@@ -64,6 +65,8 @@ namespace se3
const Eigen::VectorXd & q,
const Eigen::VectorXd & v);
+ DEFINE_ALGO_CHECKER(CRBA);
+
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
diff --git a/src/algorithm/crba.hxx b/src/algorithm/crba.hxx
index eb9511711fb208c3067ad9cdbe9c41949960cf6f..c84190f7ff2d68ae473b9f51445d4b7f0e5b71a9 100644
--- a/src/algorithm/crba.hxx
+++ b/src/algorithm/crba.hxx
@@ -209,6 +209,38 @@ namespace se3
return data.Ag;
}
+
+ // --- CHECKER ---------------------------------------------------------------
+ // --- CHECKER ---------------------------------------------------------------
+ // --- CHECKER ---------------------------------------------------------------
+
+ namespace internal
+ {
+ inline bool isDescendant(const Model& model, const JointIndex j, const JointIndex root)
+ {
+ if(int(j)>=model.njoint) return false;
+ if(j==0) return root==0;
+ return (j==root) || isDescendant(model,model.parents[j],root);
+ }
+ }
+
+ inline bool CRBAChecker::checkModel_impl( const Model& model ) const
+ {
+ // For CRBA, the tree must be "compact", i.e. all descendants of a node i are stored
+ // immediately after i in the "parents" map, i.e. forall joint i, the interval i+1..n-1
+ // can be separated in two intervals [i+1..k] and [k+1..n-1], where any [i+1..k] is a descendant
+ // of i and none of [k+1..n-1] is a descendant of i.
+ for( JointIndex i=1; int(i)<model.njoint-1; ++i ) // no need to check joints 0 and N-1
+ {
+ JointIndex k=i+1;
+ while(internal::isDescendant(model,k,i)) ++k;
+ for( ; int(k)<model.njoint; ++k )
+ if( internal::isDescendant(model,k,i) ) return false;
+ }
+ return true;
+ }
+
+
} // namespace se3
/// @endcond
diff --git a/src/algorithm/default-check.hpp b/src/algorithm/default-check.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d6859efc684bfc310c1a3e140991742e67611bcb
--- /dev/null
+++ b/src/algorithm/default-check.hpp
@@ -0,0 +1,39 @@
+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio 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.
+//
+// Pinocchio 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
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_default_check_hpp__
+#define __se3_default_check_hpp__
+
+#include <pinocchio/algorithm/check.hpp>
+#include <pinocchio/algorithm/aba.hpp>
+#include <pinocchio/algorithm/crba.hpp>
+
+namespace se3
+{
+ /// Default checker-list, used as the default argument in Model::check().
+ inline boost::fusion::list<ParentChecker,CRBAChecker,ABAChecker> makeDefaultCheckerList()
+ { return boost::fusion::make_list(ParentChecker(),CRBAChecker(),ABAChecker()); }
+
+#define DEFAULT_CHECKERS makeDefaultCheckerList()
+
+ bool Model::check() const { this->check(DEFAULT_CHECKERS); }
+
+} // namespace se3
+
+#endif // ifndef __se3_default_check_hpp__
+
+
diff --git a/src/multibody/fwd.hpp b/src/multibody/fwd.hpp
index 24aa801cd7a43b46c2e5cff4e9087f19612e2b53..5d9a3032c6cbe81412d2753a54a07e8c3327583e 100644
--- a/src/multibody/fwd.hpp
+++ b/src/multibody/fwd.hpp
@@ -32,6 +32,10 @@ namespace se3
struct GeometryData;
struct JointModel;
struct JointData;
+
+ // Forward declaration needed for Model::check
+ template<class D> struct AlgorithmCheckerBase;
+
} // namespace se3
#endif // #ifndef __se3_multibody_fwd_hpp__
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 4fdebcb73411ed8dd177e53f3c25ea6bd0eee28d..8e38896c6bccaafe4278f5ca088f422822008077 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -341,6 +341,32 @@ namespace se3
///
PINOCCHIO_DEPRECATED bool addFrame(const std::string & name, const JointIndex parent, const SE3 & placement, const FrameType type = OP_FRAME);
+ /// Check the validity of the attributes of Model with respect to the specification of some
+ /// algorithms.
+ ///
+ /// The method is a template so that the checkers can be defined in each algorithms.
+ /// \param[in] checker a class, typically defined in the algorithm module, that
+ /// validates the attributes of model.
+ /// \return true if the Model is valid, false otherwise.
+ template<typename D>
+ inline bool check(const AlgorithmCheckerBase<D> & checker = AlgorithmCheckerBase<D>()) const
+ { return checker.checkModel(*this); }
+
+ /// Multiple checks for a fusion::vector of AlgorithmCheckerBase.
+ ///
+ /// Run the check test for several conditons.
+ /// \param[in] v fusion::vector of algo checkers. The param is typically initialize with
+ /// boost::fusion::make_list( AlgoChecker1(), AlgoChecker2(), ...)
+ /// make_list is defined in #include <boost/fusion/include/make_list.hpp>
+ /// \warning no more than 10 checkers can be added (or Model API should be extended).
+ /// \note This method is implemented in src/algo/check.hxx.
+ template<class T1,class T2,class T3,class T4,class T5,
+ class T6,class T7,class T8,class T9,class T10>
+ bool check( const boost::fusion::list<T1,T2,T3,T4,T5,T6,T7,T8,T9,T10> & checkerList ) const;
+
+ /// Run check(fusion::list) with DEFAULT_CHECKERS as argument.
+ bool check() const;
+
protected:
/// \brief Add the joint_id to its parent subtrees.
@@ -359,7 +385,8 @@ namespace se3
typedef Eigen::Matrix<double,3,Eigen::Dynamic> Matrix3x;
typedef SE3::Vector3 Vector3;
- /// \brief Vector of se3::JointData associated to the se3::JointModel stored in model, encapsulated in JointDataAccessor.
+ /// \brief Vector of se3::JointData associated to the se3::JointModel stored in model,
+ /// encapsulated in JointDataAccessor.
JointDataVector joints;
/// \brief Vector of joint accelerations.
diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt
index 329a780b5f26c9a0074a2558fbd37f697b9f746c..023d1b3db9153233922030630e9554e9a3d798a4 100644
--- a/unittest/CMakeLists.txt
+++ b/unittest/CMakeLists.txt
@@ -114,3 +114,4 @@ ADD_UNIT_TEST(joint eigen3)
ADD_UNIT_TEST(explog eigen3)
ADD_UNIT_TEST(finite-differences eigen3)
ADD_UNIT_TEST(visitor eigen3)
+ADD_UNIT_TEST(algo-check eigen3)
diff --git a/unittest/algo-check.cpp b/unittest/algo-check.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9fafdc7167169dba4c9a567b601c75b88b826351
--- /dev/null
+++ b/unittest/algo-check.cpp
@@ -0,0 +1,47 @@
+
+#include <boost/fusion/container/generation/make_list.hpp>
+#include <pinocchio/multibody/model.hpp>
+#include "pinocchio/parsers/sample-models.hpp"
+#include <pinocchio/algorithm/crba.hpp>
+#include <pinocchio/algorithm/aba.hpp>
+#include <pinocchio/algorithm/check.hpp>
+#include <pinocchio/algorithm/default-check.hpp>
+#include <iostream>
+
+using namespace se3;
+
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE AlgoCheckTest
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
+// Dummy checker.
+struct Check1 : public AlgorithmCheckerBase<Check1>
+{
+ bool checkModel_impl( const Model& ) const { return true; }
+};
+
+BOOST_AUTO_TEST_SUITE ( AlgoCheck )
+
+BOOST_AUTO_TEST_CASE ( test_check )
+{
+ using namespace boost::fusion;
+
+ se3::Model model; buildModels::humanoidSimple(model);
+
+ BOOST_CHECK(model.check (Check1()));
+ BOOST_CHECK(model.check (CRBAChecker()));
+ BOOST_CHECK(! model.check (ABAChecker())); // some inertias are negative ... check fail.
+ BOOST_FOREACH(Inertia& Y,model.inertias)
+ Y.mass() = Y.inertia().data()[0] = Y.inertia().data()[3] = Y.inertia().data()[5] = 1.0 ;
+ BOOST_CHECK(model.check (ABAChecker())); // some inertias are negative ... check fail.
+
+ BOOST_CHECK(model.check(boost::fusion::make_list(Check1(),ParentChecker(),CRBAChecker()) ));
+ BOOST_CHECK(model.check(DEFAULT_CHECKERS));
+
+ se3::Data data(model);
+ BOOST_CHECK(checkData(model,data));
+}
+
+BOOST_AUTO_TEST_SUITE_END ()
+