Newer
Older
Nicolas Mansard
committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
#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 ()