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