Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/cartesian-product-liegroups.cpp	Lines:	67	67	100.0 %
Date:	2024-04-26 13:14:21	Branches:	264	510	51.8 %


//
// Copyright (c) 2020 INRIA
//

#include "pinocchio/multibody/liegroup/liegroup.hpp"
#include "pinocchio/multibody/liegroup/liegroup-collection.hpp"
#include "pinocchio/multibody/liegroup/liegroup-generic.hpp"
#include "pinocchio/multibody/liegroup/cartesian-product-variant.hpp"
#include "pinocchio/multibody/liegroup/cartesian-product.hpp"

#include "pinocchio/multibody/joint/joint-generic.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include <boost/algorithm/string.hpp>

using namespace pinocchio;

namespace pinocchio {
template<typename Derived>
std::ostream& operator<< (std::ostream& os, const LieGroupBase<Derived>& lg)
{
  return os << lg.name();
}
template<typename LieGroupCollection>
std::ostream& operator<< (std::ostream& os, const LieGroupGenericTpl<LieGroupCollection>& lg)
{
  return os << lg.name();
}
} // namespace pinocchio


template<typename Scalar, int Options, template<typename S, int O> class LieGroupCollectionTpl>
struct TestCartesianProduct
{

  typedef LieGroupCollectionTpl<Scalar,Options> LieGroupCollection;

  typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric;
  typedef typename LieGroupGeneric::ConfigVector_t ConfigVector_t;
  typedef typename LieGroupGeneric::TangentVector_t TangentVector_t;

  typedef CartesianProductOperationVariantTpl<Scalar, Options, LieGroupCollectionTpl > CartesianProduct;

  template<typename Derived>
  void operator() (const LieGroupBase<Derived> & lg) const
  {
    LieGroupGenericTpl<LieGroupCollection> lg_generic(lg.derived());
    CartesianProduct cp(lg_generic);
    test(lg,cp);

    CartesianProduct cp2;
    cp2.append(lg);
    BOOST_CHECK(cp == cp2);
  }

  template<typename LieGroup>
  static void test(const LieGroupBase<LieGroup> & lg,
                   const CartesianProduct & cp)
  {
    BOOST_CHECK(lg.nq() == cp.nq());
    BOOST_CHECK(lg.nv() == cp.nv());

    std::cout << "name: " << cp.name() << std::endl;

    BOOST_CHECK(lg.neutral() == cp.neutral());

    typedef typename LieGroup::ConfigVector_t ConfigVector;
    typedef typename LieGroup::TangentVector_t TangentVector;
    typedef typename LieGroup::JacobianMatrix_t JacobianMatrix;

    ConfigVector q0 = lg.random();
    ConfigVector q1 = lg.random();
    TangentVector v = TangentVector_t::Random(lg.nv());
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
    ConfigVector qout_ref(lg.nq()), qout(lg.nq());
    lg.integrate(q0, v, qout_ref);
    cp.integrate(q0, v, qout);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

    BOOST_CHECK(qout.isApprox(qout_ref));

PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
    TangentVector v_diff_ref(lg.nv()), v_diff(lg.nv());
    lg.difference(q0,q1,v_diff_ref);
    cp.difference(q0,q1,v_diff);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

    BOOST_CHECK(v_diff_ref.isApprox(v_diff));
    BOOST_CHECK_EQUAL(lg.squaredDistance(q0, q1), cp.squaredDistance(q0, q1));
    BOOST_CHECK_EQUAL(lg.distance(q0, q1), cp.distance(q0, q1));

    JacobianMatrix
    J_ref(JacobianMatrix::Zero(lg.nv(),lg.nv())),
    J(JacobianMatrix::Zero(lg.nv(),lg.nv()));

    lg.dDifference(q0, q1, J_ref, ARG0);
    cp.dDifference(q0, q1, J, ARG0);

    BOOST_CHECK(J.isApprox(J_ref));

    lg.dDifference(q0, q1, J_ref, ARG1);
    cp.dDifference(q0, q1, J, ARG1);

    BOOST_CHECK(J.isApprox(J_ref));

    lg.dIntegrate(q0, v, J_ref, ARG0);
    cp.dIntegrate(q0, v, J, ARG0);

    BOOST_CHECK(J.isApprox(J_ref));

    lg.dIntegrate(q0, v, J_ref, ARG1);
    cp.dIntegrate(q0, v, J, ARG1);

    BOOST_CHECK(J.isApprox(J_ref));

    BOOST_CHECK(cp.isSameConfiguration(q0,q0));
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
    ConfigVector q_rand;
    cp.random(q_rand);
    ConfigVector q_rand_copy = q_rand;

    lg.normalize(q_rand_copy);
    cp.normalize(q_rand);
    BOOST_CHECK(q_rand.isApprox(q_rand_copy));

    const ConfigVector lb(-ConfigVector::Ones(lg.nq()));
    const ConfigVector ub( ConfigVector::Ones(lg.nq()));

    cp.randomConfiguration(lb, ub, q_rand);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
  }
};

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_cartesian_product_with_liegroup_variant)
{
  boost::mpl::for_each<LieGroupCollectionDefault::LieGroupVariant::types>(TestCartesianProduct<double,0,LieGroupCollectionDefaultTpl>());
}

BOOST_AUTO_TEST_CASE(test_cartesian_product_vs_cartesian_product_variant)
{
  typedef SpecialEuclideanOperationTpl<3,double,0> SE3;
  typedef VectorSpaceOperationTpl<3,double,0> Rn;

  typedef CartesianProductOperation<SE3, Rn> CPRef;
  typedef CartesianProductOperationVariantTpl<double, 0, LieGroupCollectionDefaultTpl > CP;

  SE3 lg1; Rn lg2;
  typedef LieGroupGenericTpl<CP::LieGroupCollection> LieGroupGeneric;
  LieGroupGeneric lg1_variant(lg1);
  LieGroupGeneric lg2_variant(lg2);

  CP cartesian_product(lg1_variant,lg2_variant);
  CP cartesian_product2(lg1_variant); cartesian_product2.append(lg2_variant);
  std::cout << "cartesian_product: " << cartesian_product << std::endl;

  BOOST_CHECK(cartesian_product == cartesian_product2);
  CPRef cartesian_product_ref;

  TestCartesianProduct<double,0,LieGroupCollectionDefaultTpl>::test(cartesian_product_ref,
                                                                    cartesian_product);
}

BOOST_AUTO_TEST_SUITE_END()


Generated by: GCOVR (Version 4.2)

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			PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
76			PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
77	✓✓✗✓ ✗✓✓✗ ✓✗	20	ConfigVector qout_ref(lg.nq()), qout(lg.nq());
78	✓✗	18	lg.integrate(q0, v, qout_ref);
79	✓✗	18	cp.integrate(q0, v, qout);
80			PINOCCHIO_COMPILER_DIAGNOSTIC_POP
81
82	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	18	BOOST_CHECK(qout.isApprox(qout_ref));
83
84			PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
85			PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
86	✓✓✗✓ ✗✓✓✗ ✓✗	20	TangentVector v_diff_ref(lg.nv()), v_diff(lg.nv());
87	✓✗	18	lg.difference(q0,q1,v_diff_ref);
88	✓✗	18	cp.difference(q0,q1,v_diff);
89			PINOCCHIO_COMPILER_DIAGNOSTIC_POP
90
91	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	18	BOOST_CHECK(v_diff_ref.isApprox(v_diff));
92	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	18	BOOST_CHECK_EQUAL(lg.squaredDistance(q0, q1), cp.squaredDistance(q0, q1));
93	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	18	BOOST_CHECK_EQUAL(lg.distance(q0, q1), cp.distance(q0, q1));
94
95			JacobianMatrix
96	✓✗✓✓ ✗✓✓✗ ✓✗	20	J_ref(JacobianMatrix::Zero(lg.nv(),lg.nv())),
97	✓✗✓✓ ✗✓✓✗ ✓✗	20	J(JacobianMatrix::Zero(lg.nv(),lg.nv()));
98
99	✓✗	18	lg.dDifference(q0, q1, J_ref, ARG0);
100	✓✗	18	cp.dDifference(q0, q1, J, ARG0);
101
102	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	18	BOOST_CHECK(J.isApprox(J_ref));
103
104	✓✗	18	lg.dDifference(q0, q1, J_ref, ARG1);
105	✓✗	18	cp.dDifference(q0, q1, J, ARG1);
106
107	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	18	BOOST_CHECK(J.isApprox(J_ref));
108
109	✓✗	18	lg.dIntegrate(q0, v, J_ref, ARG0);
110	✓✗	18	cp.dIntegrate(q0, v, J, ARG0);
111
112	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	18	BOOST_CHECK(J.isApprox(J_ref));
113
114	✓✗	18	lg.dIntegrate(q0, v, J_ref, ARG1);
115	✓✗	18	cp.dIntegrate(q0, v, J, ARG1);
116
117	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	18	BOOST_CHECK(J.isApprox(J_ref));
118
119	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	18	BOOST_CHECK(cp.isSameConfiguration(q0,q0));
120			PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
121			PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
122	✓✗	20	ConfigVector q_rand;
123	✓✗	18	cp.random(q_rand);
124	✓✗	20	ConfigVector q_rand_copy = q_rand;
125
126	✓✗	18	lg.normalize(q_rand_copy);
127	✓✗	18	cp.normalize(q_rand);
128	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	18	BOOST_CHECK(q_rand.isApprox(q_rand_copy));
129
130	✓✓✗✓ ✓✗✓✓ ✗✓✗	20	const ConfigVector lb(-ConfigVector::Ones(lg.nq()));
131	✓✓✗✓ ✓✗✓✗	20	const ConfigVector ub( ConfigVector::Ones(lg.nq()));
132
133	✓✗	18	cp.randomConfiguration(lb, ub, q_rand);
134			PINOCCHIO_COMPILER_DIAGNOSTIC_POP
135		18	}
136			};
137
138			BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
139
140	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_cartesian_product_with_liegroup_variant)
141			{
142	✓✗	2	boost::mpl::for_each<LieGroupCollectionDefault::LieGroupVariant::types>(TestCartesianProduct<double,0,LieGroupCollectionDefaultTpl>());
143		2	}
144
145	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_cartesian_product_vs_cartesian_product_variant)
146			{
147			typedef SpecialEuclideanOperationTpl<3,double,0> SE3;
148			typedef VectorSpaceOperationTpl<3,double,0> Rn;
149
150			typedef CartesianProductOperation<SE3, Rn> CPRef;
151			typedef CartesianProductOperationVariantTpl<double, 0, LieGroupCollectionDefaultTpl > CP;
152
153	✓✗✓✗	2	SE3 lg1; Rn lg2;
154			typedef LieGroupGenericTpl<CP::LieGroupCollection> LieGroupGeneric;
155	✓✗	4	LieGroupGeneric lg1_variant(lg1);
156	✓✗	4	LieGroupGeneric lg2_variant(lg2);
157
158	✓✗	4	CP cartesian_product(lg1_variant,lg2_variant);
159	✓✗✓✗	4	CP cartesian_product2(lg1_variant); cartesian_product2.append(lg2_variant);
160	✓✗✓✗ ✓✗	2	std::cout << "cartesian_product: " << cartesian_product << std::endl;
161
162	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(cartesian_product == cartesian_product2);
163	✓✗	2	CPRef cartesian_product_ref;
164
165	✓✗	2	TestCartesianProduct<double,0,LieGroupCollectionDefaultTpl>::test(cartesian_product_ref,
166			cartesian_product);
167		2	}
168
169			BOOST_AUTO_TEST_SUITE_END()