Go to the documentation of this file.
13 #include <boost/test/unit_test.hpp>
14 #include <boost/utility/binary.hpp>
15 #include <boost/algorithm/string.hpp>
20 template<
typename Derived>
23 return os << lg.
name();
25 template<
typename LieGroupCollection>
28 return os << lg.
name();
33 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class LieGroupCollectionTpl>
45 template<
typename Derived>
54 BOOST_CHECK(
cp == cp2);
57 template<
typename LieGroup>
61 BOOST_CHECK(lg.
nq() ==
cp.nq());
62 BOOST_CHECK(lg.
nv() ==
cp.nv());
64 std::cout <<
"name: " <<
cp.name() << std::endl;
66 BOOST_CHECK(lg.
neutral() ==
cp.neutral());
68 typedef typename LieGroup::ConfigVector_t ConfigVector;
69 typedef typename LieGroup::TangentVector_t TangentVector;
70 typedef typename LieGroup::JacobianMatrix_t JacobianMatrix;
74 TangentVector
v = TangentVector_t::Random(lg.
nv());
77 ConfigVector qout_ref(lg.
nq()), qout(lg.
nq());
79 cp.integrate(
q0,
v, qout);
82 BOOST_CHECK(qout.isApprox(qout_ref));
86 TangentVector v_diff_ref(lg.
nv()), v_diff(lg.
nv());
88 cp.difference(
q0,
q1,v_diff);
91 BOOST_CHECK(v_diff_ref.isApprox(v_diff));
96 J_ref(JacobianMatrix::Zero(lg.
nv(),lg.
nv())),
97 J(JacobianMatrix::Zero(lg.
nv(),lg.
nv()));
102 BOOST_CHECK(
J.isApprox(J_ref));
107 BOOST_CHECK(
J.isApprox(J_ref));
112 BOOST_CHECK(
J.isApprox(J_ref));
117 BOOST_CHECK(
J.isApprox(J_ref));
119 BOOST_CHECK(
cp.isSameConfiguration(
q0,
q0));
124 ConfigVector q_rand_copy = q_rand;
127 cp.normalize(q_rand);
128 BOOST_CHECK(q_rand.isApprox(q_rand_copy));
130 const ConfigVector lb(-ConfigVector::Ones(lg.
nq()));
131 const ConfigVector ub( ConfigVector::Ones(lg.
nq()));
133 cp.randomConfiguration(lb, ub, q_rand);
138 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
155 LieGroupGeneric lg1_variant(lg1);
156 LieGroupGeneric lg2_variant(lg2);
158 CP cartesian_product(lg1_variant,lg2_variant);
159 CP cartesian_product2(lg1_variant); cartesian_product2.append(lg2_variant);
160 std::cout <<
"cartesian_product: " << cartesian_product << std::endl;
162 BOOST_CHECK(cartesian_product == cartesian_product2);
163 CPRef cartesian_product_ref;
169 BOOST_AUTO_TEST_SUITE_END()
void append(const LieGroupGeneric &lg)
Append a Lie group to the Cartesian product.
LieGroupGeneric::ConfigVector_t ConfigVector_t
LieGroupGeneric::TangentVector_t TangentVector_t
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.
static void test(const LieGroupBase< LieGroup > &lg, const CartesianProduct &cp)
LieGroupGenericTpl< LieGroupCollection > LieGroupGeneric
BOOST_AUTO_TEST_CASE(test_cartesian_product_with_liegroup_variant)
Dynamic Cartesian product composed of elementary Lie groups defined in LieGroupVariant.
#define PINOCCHIO_COMPILER_DIAGNOSTIC_POP
void normalize(const Eigen::MatrixBase< Config_t > &qout) const
Normalize the joint configuration given as input. For instance, the quaternion must be unitary.
#define PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
std::string name() const
Get name of instance.
CartesianProductOperationVariantTpl< Scalar, Options, LieGroupCollectionTpl > CartesianProduct
Index nv() const
Get dimension of Lie Group tangent space.
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...
ConfigVector_t neutral() const
Get neutral element as a vector.
void random(const Eigen::MatrixBase< Config_t > &qout) const
Generate a random joint configuration, normalizing quaternions when necessary.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
Scalar squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Squared distance between two joint configurations.
#define PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
macros for pragma push/pop/ignore deprecated warnings
std::ostream & operator<<(std::ostream &os, const FrameTpl< Scalar, Options > &f)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
void integrate(const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout) const
Integrate a joint's configuration with a tangent vector during one unit time duration.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
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.
LieGroupCollectionTpl< Scalar, Options > LieGroupCollection
Scalar distance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Distance between two configurations of the joint.
Main pinocchio namespace.
pinocchio
Author(s):
autogenerated on Tue Feb 13 2024 03:43:57