[Unit test] Add unit test of subtree info

unittest/CMakeLists.txt

@@ -92,6 +92,7 @@ IF(BUILD_PYTHON_INTERFACE)
 ENDIF(BUILD_PYTHON_INTERFACE)
 
 # Work in progress
+ADD_UNIT_TEST(model eigen3)
 ADD_UNIT_TEST(constraint eigen3)
 #ADD_UNIT_TEST(variant eigen3)
 ADD_UNIT_TEST(joints eigen3)

unittest/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()