GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/cartesian-product-liegroups.cpp Lines: 67 67 100.0 %
Date: 2023-08-09 08:43:58 Branches: 264 510 51.8 %

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