Commit 3317e88c authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[liegroup] Implement isNormalized for variant

parent e775580d
......@@ -75,6 +75,11 @@ namespace pinocchio
inline void normalize(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<Config_t> & qout);
template<typename LieGroupCollection, class Config_t>
inline bool isNormalized(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<Config_t> & qin,
const typename Config_t::Scalar prec = Eigen::NumTraits<typename Config_t::Scalar>::dummy_precision());
template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
inline bool isSameConfiguration(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<ConfigL_t> & q0,
......
......@@ -190,6 +190,38 @@ namespace pinocchio
#undef PINOCCHIO_LG_VISITOR
/** @brief Visitor of the Lie Group isNormalized method */
template <class Matrix_t>
struct LieGroupIsNormalizedVisitor
: visitor::LieGroupVisitorBase< LieGroupIsNormalizedVisitor<Matrix_t> >
{
typedef boost::fusion::vector<const Eigen::MatrixBase<Matrix_t> &,
const typename Matrix_t::Scalar,
bool&> ArgsType;
LIE_GROUP_VISITOR(LieGroupIsNormalizedVisitor);
template<typename LieGroupDerived>
static void algo(const LieGroupBase<LieGroupDerived> & lg,
const Eigen::MatrixBase<Matrix_t>& qin,
const typename Matrix_t::Scalar prec,
bool& res)
{
res = lg.isNormalized(qin, prec);
}
};
template<typename LieGroupCollection, class Config_t>
inline bool isNormalized(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<Config_t> & qin,
const typename Config_t::Scalar prec)
{
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, qin, nq(lg));
bool res;
typedef LieGroupIsNormalizedVisitor<Config_t> Operation;
Operation::run(lg,typename Operation::ArgsType(qin,prec,res));
return res;
}
/** @brief Visitor of the Lie Group isSameConfiguration method */
template <class Matrix1_t, class Matrix2_t>
struct LieGroupIsSameConfigurationVisitor
......
......@@ -710,6 +710,11 @@ struct TestLieGroupVariantVisitor
random (lg_generic, q1);
difference(lg_generic, q0, q1, v);
BOOST_CHECK_EQUAL(lg.distance(q0, q1), distance (lg_generic, q0, q1));
ConfigVector_t q2(nq(lg_generic));
random(lg_generic, q2);
normalize(lg_generic, q2);
BOOST_CHECK(isNormalized(lg_generic, q2));
}
};
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment