cartesian-product-liegroups.cpp
Go to the documentation of this file.
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 std::ostream& operator<< (std::ostream& os, const LieGroupBase<Derived>& lg)
22 {
23  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>
35 {
36 
37  typedef LieGroupCollectionTpl<Scalar,Options> LieGroupCollection;
38 
42 
44 
45  template<typename Derived>
46  void operator() (const LieGroupBase<Derived> & lg) const
47  {
49  CartesianProduct cp(lg_generic);
50  test(lg,cp);
51 
52  CartesianProduct cp2;
53  cp2.append(lg);
54  BOOST_CHECK(cp == cp2);
55  }
56 
57  template<typename LieGroup>
58  static void test(const LieGroupBase<LieGroup> & lg,
59  const CartesianProduct & cp)
60  {
61  BOOST_CHECK(lg.nq() == cp.nq());
62  BOOST_CHECK(lg.nv() == cp.nv());
63 
64  std::cout << "name: " << cp.name() << std::endl;
65 
66  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  ConfigVector q0 = lg.random();
73  ConfigVector q1 = lg.random();
74  TangentVector v = TangentVector_t::Random(lg.nv());
75  ConfigVector qout_ref(lg.nq()), qout(lg.nq());
76  lg.integrate(q0, v, qout_ref);
77  cp.integrate(q0, v, qout);
78 
79  BOOST_CHECK(qout.isApprox(qout_ref));
80 
81  TangentVector v_diff_ref(lg.nv()), v_diff(lg.nv());
82  lg.difference(q0,q1,v_diff_ref);
83  cp.difference(q0,q1,v_diff);
84 
85  BOOST_CHECK(v_diff_ref.isApprox(v_diff));
86  BOOST_CHECK_EQUAL(lg.squaredDistance(q0, q1), cp.squaredDistance(q0, q1));
87  BOOST_CHECK_EQUAL(lg.distance(q0, q1), cp.distance(q0, q1));
88 
89  JacobianMatrix
90  J_ref(JacobianMatrix::Zero(lg.nv(),lg.nv())),
91  J(JacobianMatrix::Zero(lg.nv(),lg.nv()));
92 
93  lg.dDifference(q0, q1, J_ref, ARG0);
94  cp.dDifference(q0, q1, J, ARG0);
95 
96  BOOST_CHECK(J.isApprox(J_ref));
97 
98  lg.dDifference(q0, q1, J_ref, ARG1);
99  cp.dDifference(q0, q1, J, ARG1);
100 
101  BOOST_CHECK(J.isApprox(J_ref));
102 
103  lg.dIntegrate(q0, v, J_ref, ARG0);
104  cp.dIntegrate(q0, v, J, ARG0);
105 
106  BOOST_CHECK(J.isApprox(J_ref));
107 
108  lg.dIntegrate(q0, v, J_ref, ARG1);
109  cp.dIntegrate(q0, v, J, ARG1);
110 
111  BOOST_CHECK(J.isApprox(J_ref));
112 
113  BOOST_CHECK(cp.isSameConfiguration(q0,q0));
114  ConfigVector q_rand;
115  cp.random(q_rand);
116  ConfigVector q_rand_copy = q_rand;
117 
118  lg.normalize(q_rand_copy);
119  cp.normalize(q_rand);
120  BOOST_CHECK(q_rand.isApprox(q_rand_copy));
121 
122  const ConfigVector lb(-ConfigVector::Ones(lg.nq()));
123  const ConfigVector ub( ConfigVector::Ones(lg.nq()));
124 
125  cp.randomConfiguration(lb, ub, q_rand);
126  }
127 };
128 
129 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
130 
131 BOOST_AUTO_TEST_CASE(test_cartesian_product_with_liegroup_variant)
132 {
133  boost::mpl::for_each<LieGroupCollectionDefault::LieGroupVariant::types>(TestCartesianProduct<double,0,LieGroupCollectionDefaultTpl>());
134 }
135 
136 BOOST_AUTO_TEST_CASE(test_cartesian_product_vs_cartesian_product_variant)
137 {
140 
143 
144  SE3 lg1; Rn lg2;
145  typedef LieGroupGenericTpl<CP::LieGroupCollection> LieGroupGeneric;
146  LieGroupGeneric lg1_variant(lg1);
147  LieGroupGeneric lg2_variant(lg2);
148 
149  CP cartesian_product(lg1_variant,lg2_variant);
150  CP cartesian_product2(lg1_variant); cartesian_product2.append(lg2_variant);
151  std::cout << "cartesian_product: " << cartesian_product << std::endl;
152 
153  BOOST_CHECK(cartesian_product == cartesian_product2);
154  CPRef cartesian_product_ref;
155 
157  cartesian_product);
158 }
159 
160 BOOST_AUTO_TEST_SUITE_END()
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
def test(dtype)
void difference(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< Tangent_t > &v) const
Computes the tangent vector that must be integrated during one unit time to go from q0 to q1...
void append(const LieGroupGeneric &lg)
Append a Lie group to the Cartesian product.
Scalar distance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Distance between two configurations of the joint.
void dIntegrate(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, AssignmentOperatorType op=SETTO) const
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tan...
bool isSameConfiguration(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Check if two configurations are equivalent within the given precision.
void integrate(const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout) const
Integrate a joint&#39;s configuration with a tangent vector during one unit time duration.
BOOST_AUTO_TEST_CASE(test_cartesian_product_with_liegroup_variant)
Dynamic Cartesian product composed of elementary Lie groups defined in LieGroupVariant.
void randomConfiguration(const Eigen::MatrixBase< ConfigL_t > &lower_pos_limit, const Eigen::MatrixBase< ConfigR_t > &upper_pos_limit, const Eigen::MatrixBase< ConfigOut_t > &qout) const
Generate a configuration vector uniformly sampled among provided limits.
Scalar squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Squared distance between two joint configurations.
static void test(const LieGroupBase< LieGroup > &lg, const CartesianProduct &cp)
void random(const Eigen::MatrixBase< Config_t > &qout) const
Generate a random joint configuration, normalizing quaternions when necessary.
LieGroupGeneric::ConfigVector_t ConfigVector_t
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
#define BOOST_TEST_MODULE
void dDifference(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J) const
Computes the Jacobian of the difference operation with respect to q0 or q1.
CartesianProductOperationVariantTpl< Scalar, Options, LieGroupCollectionTpl > CartesianProduct
void normalize(const Eigen::MatrixBase< Config_t > &qout) const
Normalize the joint configuration given as input. For instance, the quaternion must be unitary...
Main pinocchio namespace.
Definition: timings.cpp:28
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
LieGroupCollectionTpl< Scalar, Options > LieGroupCollection
LieGroupGeneric::TangentVector_t TangentVector_t
LieGroupGenericTpl< LieGroupCollection > LieGroupGeneric
SE3Tpl< double, 0 > SE3
Index nv() const
Get dimension of Lie Group tangent space.
ConfigVector_t neutral() const
Get neutral element as a vector.


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:28