algo-check.cpp 2.1 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
//
// Copyright (c) 2016-2017 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/>.
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

#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;

#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; }
};

38
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
39
40
41
42
43
44
45
46
47
48
49
50
51
52

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.

jcarpent's avatar
jcarpent committed
53
  BOOST_CHECK(model.check(makeAlgoCheckerList(Check1(),ParentChecker(),CRBAChecker()) ));
54
55
56
57
  BOOST_CHECK(model.check(DEFAULT_CHECKERS));

  se3::Data data(model);
  BOOST_CHECK(checkData(model,data));
58
59
  BOOST_CHECK(model.check(data));
  
60
61
62
63
}

BOOST_AUTO_TEST_SUITE_END ()