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" 11 #include "pinocchio/multibody/joint/joint-generic.hpp" 13 #include <boost/test/unit_test.hpp> 14 #include <boost/utility/binary.hpp> 15 #include <boost/algorithm/string.hpp> 20 template<
typename Derived>
21 std::ostream& operator<< (std::ostream& os, const LieGroupBase<Derived>& lg)
23 return os << lg.name();
25 template<
typename LieGroupCollection>
26 std::ostream& operator<< (std::ostream& os, const LieGroupGenericTpl<LieGroupCollection>& lg)
28 return os << lg.name();
33 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class LieGroupCollectionTpl>
45 template<
typename Derived>
49 CartesianProduct
cp(lg_generic);
54 BOOST_CHECK(cp == cp2);
57 template<
typename LieGroup>
59 const CartesianProduct &
cp)
61 BOOST_CHECK(lg.
nq() == cp.
nq());
62 BOOST_CHECK(lg.
nv() == cp.
nv());
64 std::cout <<
"name: " << cp.
name() << std::endl;
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());
75 ConfigVector qout_ref(lg.
nq()), qout(lg.
nq());
79 BOOST_CHECK(qout.isApprox(qout_ref));
81 TangentVector v_diff_ref(lg.
nv()), v_diff(lg.
nv());
85 BOOST_CHECK(v_diff_ref.isApprox(v_diff));
90 J_ref(JacobianMatrix::Zero(lg.
nv(),lg.
nv())),
91 J(JacobianMatrix::Zero(lg.
nv(),lg.
nv()));
96 BOOST_CHECK(J.isApprox(J_ref));
101 BOOST_CHECK(J.isApprox(J_ref));
106 BOOST_CHECK(J.isApprox(J_ref));
111 BOOST_CHECK(J.isApprox(J_ref));
116 ConfigVector q_rand_copy = q_rand;
120 BOOST_CHECK(q_rand.isApprox(q_rand_copy));
122 const ConfigVector lb(-ConfigVector::Ones(lg.
nq()));
123 const ConfigVector ub( ConfigVector::Ones(lg.
nq()));
146 LieGroupGeneric lg1_variant(lg1);
147 LieGroupGeneric lg2_variant(lg2);
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;
153 BOOST_CHECK(cartesian_product == cartesian_product2);
154 CPRef cartesian_product_ref;
160 BOOST_AUTO_TEST_SUITE_END()
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
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...
Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
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...
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
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.
ConfigVector_t neutral() const
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.
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.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
LieGroupCollectionTpl< Scalar, Options > LieGroupCollection
LieGroupGeneric::TangentVector_t TangentVector_t
LieGroupGenericTpl< LieGroupCollection > LieGroupGeneric
Index nv() const
Get dimension of Lie Group tangent space.
ConfigVector_t neutral() const
Get neutral element as a vector.