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