Commit 5d7faff5 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

[C++] Added checkers to validate a particular model with respect to the needs of some algorithms.

parent 3b2307c2
......@@ -192,6 +192,8 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
algorithm/compute-all-terms.hpp
algorithm/copy.hpp
algorithm/check.hpp
algorithm/check.hxx
algorithm/default-check.hpp
)
SET(${PROJECT_NAME}_PARSERS_HEADERS
......
......@@ -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 -------------------------------------------------------------------- */
......
......@@ -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
......
//
// 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__
//
// 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__
......@@ -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 -------------------------------------------------------------------- */
......
......@@ -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
......
//
// 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__
......@@ -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__
......@@ -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.
......@@ -350,9 +376,6 @@ namespace se3
void addJointIndexToParentSubtrees(const JointIndex joint_id);
};
// Forward declaration needed for Data::check
template<class D> struct AlgorithmCheckerBase;
struct Data
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
......@@ -362,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.
......@@ -508,24 +532,6 @@ namespace se3
///
Data (const Model & model);
/// Check the validity of the Data attributes 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 data.
/// \return true if the Data are valid, false otherwise.
template<typename D>
inline bool check(const AlgorithmCheckerBase<D> & checker) { return checker.checkData(*this); }
/// Multiple check 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_vector( AlgoChecker1(), AlgoChecker2(), ...)
template<typename FusionVectorCheckers>
bool checkAll(const FusionVectorCheckers & v);
private:
void computeLastChild(const Model& model);
void computeParents_fromRow(const Model& model);
......
#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 ()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment