Unverified Commit 2130f9b7 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1324 from gabrielebndn/topic/normalize

Add isNormalized
parents 3daef0a1 4a4331b8
Pipeline #11817 passed with stage
in 144 minutes and 55 seconds
......@@ -10,6 +10,8 @@ namespace pinocchio
namespace python
{
BOOST_PYTHON_FUNCTION_OVERLOADS(isNormalized_overload,isNormalized,2,3)
static Eigen::VectorXd normalize_proxy(const Model & model,
const Eigen::VectorXd & config)
{
......@@ -207,6 +209,18 @@ namespace pinocchio
"\tq1: a joint configuration vector (size model.nq)\n"
"\tq2: a joint configuration vector (size model.nq)\n"
"\tprec: requested accuracy for the comparison\n");
bp::def("isNormalized",
&isNormalized<double,0,JointCollectionDefaultTpl,VectorXd>,
isNormalized_overload(
bp::args("model","q","prec"),
"Check whether a configuration vector is normalized within the given precision provided by prec.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
"\tq: a joint configuration vector (size model.nq)\n"
"\tprec: requested accuracy for the check\n"
)
);
}
} // namespace python
......
......@@ -610,6 +610,39 @@ namespace pinocchio
normalize<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout));
}
/**
*
* @brief Check whether a configuration vector is normalized within the given precision provided by prec.
*
* @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 Check whether a configuration vector is normalized within the given precision provided by prec.
*
* @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,
......
......@@ -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
{
......
......@@ -506,7 +506,38 @@ namespace pinocchio
};
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NormalizeStepAlgo);
template<typename Visitor, typename JointModel> struct IsNormalizedStepAlgo;
template<typename LieGroup_t, typename ConfigVectorIn, typename Scalar>
struct IsNormalizedStep
: public fusion::JointUnaryVisitorBase< IsNormalizedStep<LieGroup_t,ConfigVectorIn,Scalar> >
{
typedef boost::fusion::vector<const ConfigVectorIn &,
const Scalar&,
bool &> ArgsType;
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(IsNormalizedStepAlgo, IsNormalizedStep)
};
template<typename Visitor, typename JointModel>
struct IsNormalizedStepAlgo
{
template<typename ConfigVectorIn>
static void run(const JointModelBase<JointModel> & jmodel,
const Eigen::MatrixBase<ConfigVectorIn> & q,
const typename ConfigVectorIn::Scalar& prec,
bool& res)
{
typedef typename Visitor::LieGroupMap LieGroupMap;
typename LieGroupMap::template operation<JointModel>::type lgo;
res &= lgo.isNormalized(jmodel.jointConfigSelector(q.derived()), prec);
}
};
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_3(IsNormalizedStepAlgo);
template<typename Visitor, typename JointModel> struct IsSameConfigurationStepAlgo;
template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar>
......
......@@ -229,9 +229,9 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
*
* @details This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \f$ q \oplus v \f$,
* to the tangent space at \f$ q \f$.
* In other words, this functions transforms a tangent vector expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of configuration between
* \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector.
* A typical example of parallel transportation is the action operated by a rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$.
* In other words, this functions transforms a tangent vector expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of configuration between
* \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector.
* A typical example of parallel transportation is the action operated by a rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$.
* In the context of configuration spaces assimilated as vectorial spaces, this operation corresponds to Identity.
* For Lie groups, its corresponds to the canonical vector field transportation.
*
......@@ -356,11 +356,26 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @brief Normalize the joint configuration given as input.
* For instance, the quaternion must be unitary.
*
* @param[out] qout the normalized joint configuration.
* @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.
*
......@@ -561,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.
*
......
......@@ -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
......
......@@ -430,6 +430,15 @@ 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());
using std::abs;
return abs(norm - Scalar(1.0)) < prec;
}
template <class Config_t>
void random_impl(const Eigen::MatrixBase<Config_t> & qout) const
{
......@@ -783,6 +792,15 @@ 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());
using std::abs;
return abs(norm - Scalar(1.0)) < prec;
}
template <class Config_t>
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
......
......@@ -305,6 +305,15 @@ 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();
using std::abs;
return abs(norm - Scalar(1.0)) < prec;
}
template <class Config_t>
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
......@@ -574,6 +583,15 @@ 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();
using std::abs;
return 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
{
......
......@@ -362,6 +362,31 @@ BOOST_AUTO_TEST_CASE ( normalize_test )
BOOST_CHECK(q.tail(n).isApprox(Eigen::VectorXd::Ones(n)));
}
BOOST_AUTO_TEST_CASE ( is_normalized_test )
{
Model model; buildAllJointsModel(model);
Eigen::VectorXd q;
q = Eigen::VectorXd::Ones(model.nq);
BOOST_CHECK(!pinocchio::isNormalized(model, q));
BOOST_CHECK(!pinocchio::isNormalized(model, q, 1e-8));
BOOST_CHECK(pinocchio::isNormalized(model, q, 1e2));
pinocchio::normalize(model, q);
BOOST_CHECK(pinocchio::isNormalized(model, q));
BOOST_CHECK(pinocchio::isNormalized(model, q, 1e-8));
q = pinocchio::neutral(model);
BOOST_CHECK(pinocchio::isNormalized(model, q));
BOOST_CHECK(pinocchio::isNormalized(model, q, 1e-8));
model.lowerPositionLimit = -1 * Eigen::VectorXd::Ones(model.nq);
model.upperPositionLimit = Eigen::VectorXd::Ones(model.nq);
q = pinocchio::randomConfiguration(model);
BOOST_CHECK(pinocchio::isNormalized(model, q));
BOOST_CHECK(pinocchio::isNormalized(model, q, 1e-8));
}
BOOST_AUTO_TEST_CASE ( integrateCoeffWiseJacobian_test )
{
Model model; buildModels::humanoidRandom(model);
......
......@@ -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 "));
}
......@@ -705,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));
}
};
......
......@@ -16,8 +16,15 @@ class TestJointsAlgo(TestCase):
def test_basic(self):
model = self.model
q_ones = np.ones((model.nq))
self.assertFalse(pin.isNormalized(model, q_ones))
self.assertFalse(pin.isNormalized(model, q_ones, 1e-8))
self.assertTrue(pin.isNormalized(model, q_ones, 1e2))
q_rand = np.random.rand((model.nq))
q_rand = pin.normalize(model,q_rand)
self.assertTrue(pin.isNormalized(model, q_rand))
self.assertTrue(pin.isNormalized(model, q_rand, 1e-8))
self.assertTrue(abs(np.linalg.norm(q_rand[3:7])-1.) <= 1e-8)
......
Markdown is supported
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