Add JointModelBase::normalized

src/algorithm/joint-configuration.hpp

@@ -104,7 +104,15 @@ namespace se3
    */
   inline Eigen::VectorXd randomConfiguration(const Model & model);
 
-
+  /**
+   * @brief      Normalize a configuration
+   *
+   * @param[in]  model      Model
+   * @param[in]  q          Configuration to normalize
+   * @return     The normalized configuration
+   */
+  inline Eigen::VectorXd normalized(const Model & model,
+                                    const Eigen::VectorXd & q);
 } // namespace se3 
 
 /* --- Details -------------------------------------------------------------------- */
@@ -299,6 +307,38 @@ namespace se3
     return randomConfiguration(model, model.lowerPositionLimit, model.upperPositionLimit);
   }
 
+  struct NormalizedStep : public fusion::JointModelVisitor<NormalizedStep>
+  {
+    typedef boost::fusion::vector<const Eigen::VectorXd &,
+                                  Eigen::VectorXd &
+                                  > ArgsType;
+
+    JOINT_MODEL_VISITOR_INIT(NormalizedStep);
+
+    template<typename JointModel>
+    static void algo(const se3::JointModelBase<JointModel> & jmodel,
+                     const Eigen::VectorXd & q,
+                     Eigen::VectorXd & result)
+    {
+      jmodel.jointConfigSelector(result) = jmodel.normalized(q);
+    }
+  };
+
+  inline Eigen::VectorXd
+  normalized(const Model & model,
+             const Eigen::VectorXd & q)
+  {
+    Eigen::VectorXd norma(model.nq);
+    for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
+    {
+      NormalizedStep::run(model.joints[i],
+                          NormalizedStep::ArgsType (q, norma)
+                          );
+    }
+    return norma;
+  }
+
+
 } // namespace se3
 
 #endif // ifndef __se3_joint_configuration_hpp__

src/multibody/joint/joint-base.hpp

@@ -336,6 +336,23 @@ namespace se3
     ConfigVector_t neutralConfiguration() const
     { return derived().neutralConfiguration_impl(); } 
 
+    /**
+     * @brief      Normalize a configuration
+     *
+     * @param[in]  q     Configuration to normalize (size full model.nq)
+     *
+     * @return     The normalized configuration
+     */
+    ConfigVector_t normalized(const Eigen::VectorXd & q) const
+    { return derived().normalized_impl(q); }
+
+    /**
+     * @brief      Default implementation of normalized
+     */
+    ConfigVector_t normalized_impl(const Eigen::VectorXd& q) const
+    {
+      return q.segment<NQ>(idx_q());
+    }
 
     JointIndex i_id; // ID of the joint in the multibody list.
     int i_q;    // Index of the joint configuration in the joint configuration vector.

src/multibody/joint/joint-free-flyer.hpp

@@ -374,6 +374,13 @@ namespace se3
       return q;
     } 
 
+    ConfigVector_t normalized_impl(const Eigen::VectorXd& q) const
+    {
+      ConfigVector_t result (q.segment<NQ>(idx_q()));
+      result.tail<4>().normalize();
+      return result;
+    }
+
     JointModelDense<NQ, NV> toDense_impl() const
     {
       return JointModelDense<NQ, NV>( id(),

src/multibody/joint/joint-spherical.hpp

@@ -399,6 +399,11 @@ namespace se3
       return q;
     } 
 
+    ConfigVector_t normalized_impl(const Eigen::VectorXd& q) const
+    {
+      return q.segment<NQ>(idx_q()).normalized();
+    }
+
     JointModelDense<NQ, NV> toDense_impl() const
     {
       return JointModelDense<NQ, NV>( id(),

unittest/joint-configurations.cpp

@@ -518,4 +518,20 @@ BOOST_AUTO_TEST_CASE ( integrate_difference_test )
 
 }
 
+BOOST_AUTO_TEST_CASE ( normalized_test )
+{
+  using namespace se3;
+
+  // Creating the Model and Data
+  Model model = createModelWithAllJoints();
+  se3::Data data(model);
+
+  Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
+
+  Eigen::VectorXd qn = se3::normalized(model, q);
+
+  BOOST_CHECK(fabs(qn.segment<4>(3).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of freeflyer
+  BOOST_CHECK(fabs(qn.segment<4>(7).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of spherical joint
+}
+
 BOOST_AUTO_TEST_SUITE_END ()