model.cpp

//
// 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/>.

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE JointTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/sample-models.hpp"

using namespace se3;

BOOST_AUTO_TEST_SUITE(Modeltest)

BOOST_AUTO_TEST_CASE(test_model_subtree)
{
  Model model;
  std::cout << "build model" << std::endl;
  buildModels::humanoidSimple(model);
  
  Model::JointIndex idx_larm1 = model.getJointId("larm1_joint");
  BOOST_CHECK(idx_larm1<model.njoint);
  Model::IndexVector subtree = model.subtrees[idx_larm1];
  BOOST_CHECK(subtree.size()==6);
  
  for(size_t i=1; i<subtree.size();++i)
    BOOST_CHECK(model.parents[subtree[i]]==subtree[i-1]);
}

BOOST_AUTO_TEST_SUITE_END()