GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2020 INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/multibody/liegroup/liegroup.hpp" |
||
6 |
#include "pinocchio/multibody/liegroup/liegroup-collection.hpp" |
||
7 |
#include "pinocchio/multibody/liegroup/liegroup-generic.hpp" |
||
8 |
#include "pinocchio/multibody/liegroup/cartesian-product-variant.hpp" |
||
9 |
#include "pinocchio/multibody/liegroup/cartesian-product.hpp" |
||
10 |
|||
11 |
#include "pinocchio/multibody/joint/joint-generic.hpp" |
||
12 |
|||
13 |
#include <boost/test/unit_test.hpp> |
||
14 |
#include <boost/utility/binary.hpp> |
||
15 |
#include <boost/algorithm/string.hpp> |
||
16 |
|||
17 |
using namespace pinocchio; |
||
18 |
|||
19 |
namespace pinocchio { |
||
20 |
template<typename Derived> |
||
21 |
1 |
std::ostream& operator<< (std::ostream& os, const LieGroupBase<Derived>& lg) |
|
22 |
{ |
||
23 |
✓✗ | 1 |
return os << lg.name(); |
24 |
} |
||
25 |
template<typename LieGroupCollection> |
||
26 |
std::ostream& operator<< (std::ostream& os, const LieGroupGenericTpl<LieGroupCollection>& lg) |
||
27 |
{ |
||
28 |
return os << lg.name(); |
||
29 |
} |
||
30 |
} // namespace pinocchio |
||
31 |
|||
32 |
|||
33 |
template<typename Scalar, int Options, template<typename S, int O> class LieGroupCollectionTpl> |
||
34 |
struct TestCartesianProduct |
||
35 |
{ |
||
36 |
|||
37 |
typedef LieGroupCollectionTpl<Scalar,Options> LieGroupCollection; |
||
38 |
|||
39 |
typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric; |
||
40 |
typedef typename LieGroupGeneric::ConfigVector_t ConfigVector_t; |
||
41 |
typedef typename LieGroupGeneric::TangentVector_t TangentVector_t; |
||
42 |
|||
43 |
typedef CartesianProductOperationVariantTpl<Scalar, Options, LieGroupCollectionTpl > CartesianProduct; |
||
44 |
|||
45 |
template<typename Derived> |
||
46 |
16 |
void operator() (const LieGroupBase<Derived> & lg) const |
|
47 |
{ |
||
48 |
✓✗ | 32 |
LieGroupGenericTpl<LieGroupCollection> lg_generic(lg.derived()); |
49 |
✓✗ | 32 |
CartesianProduct cp(lg_generic); |
50 |
✓✗ | 16 |
test(lg,cp); |
51 |
|||
52 |
✓✗ | 32 |
CartesianProduct cp2; |
53 |
✓✗ | 16 |
cp2.append(lg); |
54 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
16 |
BOOST_CHECK(cp == cp2); |
55 |
16 |
} |
|
56 |
|||
57 |
template<typename LieGroup> |
||
58 |
18 |
static void test(const LieGroupBase<LieGroup> & lg, |
|
59 |
const CartesianProduct & cp) |
||
60 |
{ |
||
61 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✓ ✗✓✗✗ ✓✗✓ |
18 |
BOOST_CHECK(lg.nq() == cp.nq()); |
62 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✓ ✗✓✗✗ ✓✗✓ |
18 |
BOOST_CHECK(lg.nv() == cp.nv()); |
63 |
|||
64 |
✓✗✓✗ ✓✗✓✗ |
18 |
std::cout << "name: " << cp.name() << std::endl; |
65 |
|||
66 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓ |
18 |
BOOST_CHECK(lg.neutral() == cp.neutral()); |
67 |
|||
68 |
typedef typename LieGroup::ConfigVector_t ConfigVector; |
||
69 |
typedef typename LieGroup::TangentVector_t TangentVector; |
||
70 |
typedef typename LieGroup::JacobianMatrix_t JacobianMatrix; |
||
71 |
|||
72 |
✓✗ | 20 |
ConfigVector q0 = lg.random(); |
73 |
✓✗ | 20 |
ConfigVector q1 = lg.random(); |
74 |
✓✓✗✓ ✓✗✓✗ |
20 |
TangentVector v = TangentVector_t::Random(lg.nv()); |
75 |
✓✓✗✓ ✗✓✓✗ ✓✗ |
20 |
ConfigVector qout_ref(lg.nq()), qout(lg.nq()); |
76 |
✓✗ | 18 |
lg.integrate(q0, v, qout_ref); |
77 |
✓✗ | 18 |
cp.integrate(q0, v, qout); |
78 |
|||
79 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
18 |
BOOST_CHECK(qout.isApprox(qout_ref)); |
80 |
|||
81 |
✓✓✗✓ ✗✓✓✗ ✓✗ |
20 |
TangentVector v_diff_ref(lg.nv()), v_diff(lg.nv()); |
82 |
✓✗ | 18 |
lg.difference(q0,q1,v_diff_ref); |
83 |
✓✗ | 18 |
cp.difference(q0,q1,v_diff); |
84 |
|||
85 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
18 |
BOOST_CHECK(v_diff_ref.isApprox(v_diff)); |
86 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
18 |
BOOST_CHECK_EQUAL(lg.squaredDistance(q0, q1), cp.squaredDistance(q0, q1)); |
87 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
18 |
BOOST_CHECK_EQUAL(lg.distance(q0, q1), cp.distance(q0, q1)); |
88 |
|||
89 |
JacobianMatrix |
||
90 |
✓✗✓✓ ✗✓✓✗ ✓✗ |
20 |
J_ref(JacobianMatrix::Zero(lg.nv(),lg.nv())), |
91 |
✓✗✓✓ ✗✓✓✗ ✓✗ |
20 |
J(JacobianMatrix::Zero(lg.nv(),lg.nv())); |
92 |
|||
93 |
✓✗ | 18 |
lg.dDifference(q0, q1, J_ref, ARG0); |
94 |
✓✗ | 18 |
cp.dDifference(q0, q1, J, ARG0); |
95 |
|||
96 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
18 |
BOOST_CHECK(J.isApprox(J_ref)); |
97 |
|||
98 |
✓✗ | 18 |
lg.dDifference(q0, q1, J_ref, ARG1); |
99 |
✓✗ | 18 |
cp.dDifference(q0, q1, J, ARG1); |
100 |
|||
101 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
18 |
BOOST_CHECK(J.isApprox(J_ref)); |
102 |
|||
103 |
✓✗ | 18 |
lg.dIntegrate(q0, v, J_ref, ARG0); |
104 |
✓✗ | 18 |
cp.dIntegrate(q0, v, J, ARG0); |
105 |
|||
106 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
18 |
BOOST_CHECK(J.isApprox(J_ref)); |
107 |
|||
108 |
✓✗ | 18 |
lg.dIntegrate(q0, v, J_ref, ARG1); |
109 |
✓✗ | 18 |
cp.dIntegrate(q0, v, J, ARG1); |
110 |
|||
111 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
18 |
BOOST_CHECK(J.isApprox(J_ref)); |
112 |
|||
113 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
18 |
BOOST_CHECK(cp.isSameConfiguration(q0,q0)); |
114 |
✓✗ | 20 |
ConfigVector q_rand; |
115 |
✓✗ | 18 |
cp.random(q_rand); |
116 |
✓✗ | 20 |
ConfigVector q_rand_copy = q_rand; |
117 |
|||
118 |
✓✗ | 18 |
lg.normalize(q_rand_copy); |
119 |
✓✗ | 18 |
cp.normalize(q_rand); |
120 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
18 |
BOOST_CHECK(q_rand.isApprox(q_rand_copy)); |
121 |
|||
122 |
✓✓✗✓ ✓✗✓✓ ✗✓✗ |
20 |
const ConfigVector lb(-ConfigVector::Ones(lg.nq())); |
123 |
✓✓✗✓ ✓✗✓✗ |
20 |
const ConfigVector ub( ConfigVector::Ones(lg.nq())); |
124 |
|||
125 |
✓✗ | 18 |
cp.randomConfiguration(lb, ub, q_rand); |
126 |
18 |
} |
|
127 |
}; |
||
128 |
|||
129 |
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
||
130 |
|||
131 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_cartesian_product_with_liegroup_variant) |
132 |
{ |
||
133 |
✓✗ | 2 |
boost::mpl::for_each<LieGroupCollectionDefault::LieGroupVariant::types>(TestCartesianProduct<double,0,LieGroupCollectionDefaultTpl>()); |
134 |
2 |
} |
|
135 |
|||
136 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_cartesian_product_vs_cartesian_product_variant) |
137 |
{ |
||
138 |
typedef SpecialEuclideanOperationTpl<3,double,0> SE3; |
||
139 |
typedef VectorSpaceOperationTpl<3,double,0> Rn; |
||
140 |
|||
141 |
typedef CartesianProductOperation<SE3, Rn> CPRef; |
||
142 |
typedef CartesianProductOperationVariantTpl<double, 0, LieGroupCollectionDefaultTpl > CP; |
||
143 |
|||
144 |
✓✗✓✗ |
2 |
SE3 lg1; Rn lg2; |
145 |
typedef LieGroupGenericTpl<CP::LieGroupCollection> LieGroupGeneric; |
||
146 |
✓✗ | 4 |
LieGroupGeneric lg1_variant(lg1); |
147 |
✓✗ | 4 |
LieGroupGeneric lg2_variant(lg2); |
148 |
|||
149 |
✓✗ | 4 |
CP cartesian_product(lg1_variant,lg2_variant); |
150 |
✓✗✓✗ |
4 |
CP cartesian_product2(lg1_variant); cartesian_product2.append(lg2_variant); |
151 |
✓✗✓✗ ✓✗ |
2 |
std::cout << "cartesian_product: " << cartesian_product << std::endl; |
152 |
|||
153 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cartesian_product == cartesian_product2); |
154 |
✓✗ | 2 |
CPRef cartesian_product_ref; |
155 |
|||
156 |
✓✗ | 2 |
TestCartesianProduct<double,0,LieGroupCollectionDefaultTpl>::test(cartesian_product_ref, |
157 |
cartesian_product); |
||
158 |
2 |
} |
|
159 |
|||
160 |
BOOST_AUTO_TEST_SUITE_END() |
Generated by: GCOVR (Version 4.2) |