Add JointNormalizedVisitor and JointModel::normalized_impl

src/multibody/joint/joint-base.hpp

@@ -349,7 +349,7 @@ namespace se3
     /**
      * @brief      Default implementation of normalized
      */
-    ConfigVector_t normalized_impl(const Eigen::VectorXd& q) const
+    ConfigVector_t normalized_impl(const Eigen::VectorXd & q) const
     {
       return q.segment<NQ>(idx_q());
     }

src/multibody/joint/joint-basic-visitors.hpp

@@ -155,6 +155,18 @@ namespace se3
    * @return     The joint's neutral configuration
    */
   inline Eigen::VectorXd neutralConfiguration(const JointModelVariant & jmodel);
+  
+  /**
+   * @brief      Visit a JointModelVariant through JointNormalizedVisitor to compute
+   *             the normalized configuration.
+   *
+   * @param[in]  jmodel  The JointModelVariant
+   * @param[in]  q       configuration to normalize
+   *
+   * @return     The normalized configuration
+   */
+  inline Eigen::VectorXd normalized(const JointModelVariant & jmodel,
+                                    const Eigen::VectorXd & q);
 
   /**
    * @brief      Visit a JointModelVariant through JointNvVisitor to get the dimension of 

src/multibody/joint/joint-basic-visitors.hxx

@@ -232,6 +232,28 @@ namespace se3
     return JointNeutralConfigurationVisitor::run(jmodel);
   }
 
+  /**
+   * @brief      JointNormalizedVisitor visitor
+   */
+  class JointNormalizedVisitor: public boost::static_visitor<Eigen::VectorXd>
+  {
+  public:
+    const Eigen::VectorXd & q;
+
+    JointNormalizedVisitor(const Eigen::VectorXd & q) : q(q) {}
+
+    template<typename D>
+    Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
+    { return jmodel.normalized(q); }
+    
+    static Eigen::VectorXd run(const JointModelVariant & jmodel, const Eigen::VectorXd & q)
+    { return boost::apply_visitor( JointNormalizedVisitor(q), jmodel ); }
+  };
+  inline Eigen::VectorXd normalized(const JointModelVariant & jmodel, const Eigen::VectorXd & q)
+  {
+    return JointNormalizedVisitor::run(jmodel, q);
+  }
+
 
   /**
    * @brief      JointDifferenceVisitor visitor

src/multibody/joint/joint-dense.hpp

@@ -197,6 +197,13 @@ namespace se3
       return result; 
     }
 
+    ConfigVector_t normalized_impl(const Eigen::VectorXd &) const
+    {
+      ConfigVector_t result;
+      assert(false && "JointModelDense is read-only, should not perform any calc");
+      return result;
+    }
+
     JointModelDense() {}
     JointModelDense(JointIndex idx, int idx_q, int idx_v)
     {

src/multibody/joint/joint.hpp

@@ -146,6 +146,11 @@ namespace se3
       return ::se3::distance(*this, q0, q1);
     }
 
+    ConfigVector_t normalized_impl(const Eigen::VectorXd & q) const
+    { 
+      return ::se3::normalized(*this, q);
+    }
+
     ConfigVector_t neutralConfiguration_impl() const
     { 
       return ::se3::neutralConfiguration(*this);