Commit e775580d authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[liegroup] add isNormalized

parent 1497a8e8
......@@ -197,6 +197,10 @@ namespace pinocchio
template <class Config_t>
void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const;
template <class Config_t>
bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& qout,
const Scalar prec) const;
template <class Config_t>
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const;
......
......@@ -363,6 +363,25 @@ normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const
}
}
template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t>
bool CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
isNormalized_impl (const Eigen::MatrixBase<Config_t>& qin,
const Scalar prec) const
{
Index id_q = 0;
for(size_t k = 0; k < liegroups.size(); ++k)
{
const Index & nq = lg_nqs[k];
const bool res_k = ::pinocchio::isNormalized(liegroups[k],
PINOCCHIO_EIGEN_CONST_CAST(Config_t, qin).segment(id_q,nq), prec);
if(!res_k)
return false;
id_q += nq;
}
return true;
}
template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
......
......@@ -228,6 +228,12 @@ namespace pinocchio
lg2.normalize(Qo2(qout));
}
template <class Config_t>
bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& qin, const Scalar prec) const
{
return lg1.isNormalized(Qo1(qin), prec) && lg2.isNormalized(Qo2(qin), prec);
}
template <class Config_t>
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
......
......@@ -357,12 +357,25 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* For instance, the quaternion must be unitary.
*
* @note If the input vector is too small (i.e., qout.norm()==0), then it is left unchanged.
* It is therefore possible that after this method is called `isNormalized(qout)` is still false.
*
* @param[in,out] qout the normalized joint configuration.
*/
template <class Config_t>
void normalize(const Eigen::MatrixBase<Config_t> & qout) const;
/**
* @brief Check whether the input joint configuration is normalized.
* For instance, the quaternion must be unitary.
*
* @param[in] qin The joint configuration to check.
*
* @return true if the input vector is normalized, false otherwise.
*/
template <class Config_t>
bool isNormalized(const Eigen::MatrixBase<Config_t> & qin,
const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
/**
* @brief Generate a random joint configuration, normalizing quaternions when necessary.
*
......@@ -563,6 +576,10 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
template <class Config_t>
void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;
template <class Config_t>
bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
template <class ConfigL_t, class ConfigR_t>
Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1) const;
......
......@@ -321,6 +321,15 @@ namespace pinocchio {
return derived().normalize_impl(PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout));
}
template <class Derived>
template <class Config_t>
bool LieGroupBase<Derived>::isNormalized
(const Eigen::MatrixBase<Config_t> & qin, const Scalar prec) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
return derived().isNormalized_impl(qin, prec);
}
/**
* @brief Generate a random joint configuration, normalizing quaternions when necessary.
*
......
......@@ -430,6 +430,14 @@ namespace pinocchio
PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).template tail<2>().normalize();
}
template <class Config_t>
static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin,
const Scalar prec)
{
const Scalar norm = Scalar(qin.template tail<2>().norm());
return (std::abs(norm - Scalar(1.0)) < prec );
}
template <class Config_t>
void random_impl(const Eigen::MatrixBase<Config_t> & qout) const
{
......@@ -783,6 +791,14 @@ namespace pinocchio
qout_.template tail<4>().normalize();
}
template <class Config_t>
static bool isNormalized_impl(const Eigen::MatrixBase<Config_t>& qin,
const Scalar prec)
{
Scalar norm = Scalar(qin.template tail<4>().norm());
return (std::abs(norm - Scalar(1.0)) < prec );
}
template <class Config_t>
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
......
......@@ -305,6 +305,14 @@ namespace pinocchio
qout_.normalize();
}
template <class Config_t>
static bool isNormalized_impl (const Eigen::MatrixBase<Config_t> & qin,
const Scalar prec)
{
const Scalar norm = qin.norm();
return (std::abs(norm - Scalar(1.0)) < prec );
}
template <class Config_t>
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
......@@ -574,6 +582,14 @@ namespace pinocchio
qout_.normalize();
}
template <class Config_t>
static bool isNormalized_impl (const Eigen::MatrixBase<Config_t> & qin,
const Scalar prec)
{
const Scalar norm = qin.norm();
return (std::abs(norm - Scalar(1.0)) < prec );
}
template <class Config_t>
void random_impl(const Eigen::MatrixBase<Config_t> & qout) const
{
......
......@@ -237,6 +237,12 @@ namespace pinocchio
static void normalize_impl (const Eigen::MatrixBase<Config_t>& /*qout*/)
{}
template <class Config_t>
static bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& /*qout*/, const Scalar /*prec*/)
{
return true;
}
template <class Config_t>
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
......
......@@ -134,20 +134,25 @@ void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &)
Eigen::VectorXd q_normalize_ref(q_normalize);
if(jmodel.shortname() == "JointModelSpherical")
{
BOOST_CHECK_MESSAGE(!LieGroupType().isNormalized(q_normalize_ref), std::string(error_prefix + " - !isNormalized "));
q_normalize_ref /= q_normalize_ref.norm();
}
else if(jmodel.shortname() == "JointModelFreeFlyer")
{
BOOST_CHECK_MESSAGE(!LieGroupType().isNormalized(q_normalize_ref), std::string(error_prefix + " - !isNormalized "));
q_normalize_ref.template tail<4>() /= q_normalize_ref.template tail<4>().norm();
}
else if(boost::algorithm::istarts_with(jmodel.shortname(),"JointModelRUB"))
{
BOOST_CHECK_MESSAGE(!LieGroupType().isNormalized(q_normalize_ref), std::string(error_prefix + " - !isNormalized "));
q_normalize_ref /= q_normalize_ref.norm();
}
else if(jmodel.shortname() == "JointModelPlanar")
{
BOOST_CHECK_MESSAGE(!LieGroupType().isNormalized(q_normalize_ref), std::string(error_prefix + " - !isNormalized "));
q_normalize_ref.template tail<2>() /= q_normalize_ref.template tail<2>().norm();
}
BOOST_CHECK_MESSAGE(LieGroupType().isNormalized(q_normalize_ref), std::string(error_prefix + " - isNormalized "));
LieGroupType().normalize(q_normalize);
BOOST_CHECK_MESSAGE(q_normalize.isApprox(q_normalize_ref), std::string(error_prefix + " - normalize "));
}
......
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