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

[algorithm] Add isNormalized

parent 78fab040
......@@ -610,6 +610,39 @@ namespace pinocchio
normalize<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout));
}
/**
*
* @brief Normalize a configuration vector.
*
* @param[in] model Model of the kinematic tree.
* @param[in] q Configuration to check (size model.nq).
* @param[in] prec Precision.
*
* @return Whether the configuration is normalized or not, within the given precision.
*/
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline bool isNormalized(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision());
/**
*
* @brief Normalize a configuration vector.
*
* @param[in] model Model of the kinematic tree.
* @param[in] q Configuration to check (size model.nq).
* @param[in] prec Precision.
*
* @return Whether the configuration is normalized or not, within the given precision.
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline bool isNormalized(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision())
{
return isNormalized<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,q,prec);
}
/**
*
* @brief Return true if the given configurations are equivalents, within the given precision.
......@@ -628,7 +661,7 @@ namespace pinocchio
isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorIn1> & q1,
const Eigen::MatrixBase<ConfigVectorIn2> & q2,
const Scalar & prec);
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
/**
*
......
......@@ -292,6 +292,31 @@ namespace pinocchio
}
}
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn>
inline bool
isNormalized(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorIn> & q,
const Scalar prec)
{
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(prec >= 0, "The precision should be positive");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
bool result = true;
typedef IsNormalizedStep<LieGroup_t,ConfigVectorIn,Scalar> Algo;
typename Algo::ArgsType args(q.derived(),prec,result);
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
{
Algo::run(model.joints[i], args);
if(!result)
return false;
}
return true;
}
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
inline bool
isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
......
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