[liegroup] Implement isNormalized for variant

src/multibody/liegroup/liegroup-variant-visitors.hpp

@@ -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,

src/multibody/liegroup/liegroup-variant-visitors.hxx

@@ -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

unittest/liegroups.cpp

@@ -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));
   }
 };