Program Listing for File check.hxx

↰ Return to documentation for file (include/pinocchio/algorithm/check.hxx)

//
// Copyright (c) 2016-2020 CNRS INRIA
//

#ifndef __pinocchio_check_hxx__
#define __pinocchio_check_hxx__

#include <boost/fusion/algorithm.hpp>
#include <boost/foreach.hpp>

namespace pinocchio
{
  namespace internal
  {
    // Dedicated structure for the fusion::accumulate algorithm: validate the check-algorithm
    // for all elements in a fusion list of AlgoCheckers.
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    struct AlgoFusionChecker
    {
      typedef bool result_type;
      typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
      const Model & model;

      AlgoFusionChecker(const Model & model) : model(model) {}

      inline bool operator()(const bool & accumul, const boost::fusion::void_ &) const
      { return accumul; }

      template<typename T>
      inline bool operator()(const bool & accumul, const AlgorithmCheckerBase<T> & t) const
      { return accumul && t.checkModel(model); }
    };
  } // namespace internal

  // Check the validity of the kinematic tree defined by parents.
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool ParentChecker::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;

    for(JointIndex j=1;j<(JointIndex)model.njoints;++j)
      if(model.parents[j]>=j)
        return false;

    return true;
  }

#if !defined(BOOST_FUSION_HAS_VARIADIC_LIST)
  template<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE,class T)>
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  bool AlgorithmCheckerList<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE,T)>
  ::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const
  {
    return boost::fusion::accumulate(checkerList,
                                     true,
                                     internal::AlgoFusionChecker<Scalar,Options,JointCollectionTpl>(model));
  }
#else
  template<class ...T>
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  bool AlgorithmCheckerList<T...>::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const
  {
    return boost::fusion::accumulate(checkerList,
                                     true,
                                     internal::AlgoFusionChecker<Scalar,Options,JointCollectionTpl>(model));
  }
#endif

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool checkData(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data)
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typedef typename Model::JointModel JointModel;

#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.njoints );
    CHECK_DATA( (int)data.a.size()        == model.njoints );
    CHECK_DATA( (int)data.a_gf.size()     == model.njoints );
    CHECK_DATA( (int)data.v.size()        == model.njoints );
    CHECK_DATA( (int)data.f.size()        == model.njoints );
    CHECK_DATA( (int)data.oMi.size()      == model.njoints );
    CHECK_DATA( (int)data.liMi.size()     == model.njoints );
    CHECK_DATA( (int)data.Ycrb.size()     == model.njoints );
    CHECK_DATA( (int)data.Yaba.size()     == model.njoints );
    CHECK_DATA( (int)data.Fcrb.size()     == model.njoints );
    BOOST_FOREACH(const typename Data::Matrix6x & F,data.Fcrb)
    {
      CHECK_DATA( F.cols() == model.nv );
    }
    CHECK_DATA( (int)data.iMf.size()      == model.njoints );
    CHECK_DATA( (int)data.iMf.size()      == model.njoints );
    CHECK_DATA( (int)data.com.size()      == model.njoints );
    CHECK_DATA( (int)data.vcom.size()     == model.njoints );
    CHECK_DATA( (int)data.acom.size()     == model.njoints );
    CHECK_DATA( (int)data.mass.size()     == model.njoints );

    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( data.kinematic_hessians.dimension(0) == 6);
#if EIGEN_VERSION_AT_LEAST(3,2,90) && !EIGEN_VERSION_AT_LEAST(3,2,93)
    CHECK_DATA( data.kinematic_hessians.dimension(1) == std::max(1,model.nv));
    CHECK_DATA( data.kinematic_hessians.dimension(2) == std::max(1,model.nv));
#else
    CHECK_DATA( data.kinematic_hessians.dimension(1) == model.nv);
    CHECK_DATA( data.kinematic_hessians.dimension(2) == model.nv);
#endif

    CHECK_DATA( (int)data.oMf.size()      == model.nframes );

    CHECK_DATA( (int)data.lastChild.size()         == model.njoints );
    CHECK_DATA( (int)data.nvSubtree.size()         == model.njoints );
    CHECK_DATA( (int)data.parents_fromRow.size()   == model.nv );
    CHECK_DATA( (int)data.nvSubtree_fromRow.size() == model.nv );

    for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
    {
      const typename Model::JointModel & jmodel = model.joints[joint_id];

      CHECK_DATA(model.nqs[joint_id] == jmodel.nq());
      CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q());
      CHECK_DATA(model.nvs[joint_id] == jmodel.nv());
      CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v());
    }

    for( JointIndex j=1;int(j)<model.njoints;++j )
    {
      JointIndex c = (JointIndex)data.lastChild[j];
      CHECK_DATA((int)c<model.njoints);
      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.njoints;++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[(size_t)row]);

      const JointModel & jparent = model.joints[model.parents[j]];
      if(row==0) { CHECK_DATA(data.parents_fromRow[(size_t)row]==-1); }
      else       { CHECK_DATA(jparent.idx_v()+jparent.nv()-1 == data.parents_fromRow[(size_t)row]); }
    }

#undef CHECK_DATA
    return true;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool ModelTpl<Scalar,Options,JointCollectionTpl>::
  check(const DataTpl<Scalar,Options,JointCollectionTpl> & data) const
  { return checkData(*this,data); }


} // namespace pinocchio

#endif // ifndef __pinocchio_check_hxx__