diff --git a/src/algorithm/joint-configuration.hpp b/src/algorithm/joint-configuration.hpp
index 113e54c32252b1e7e0cfc015dd9d743592b11ded..5c9faad60084f4e92a1622e0810ee890749b540e 100644
--- a/src/algorithm/joint-configuration.hpp
+++ b/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__
diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp
index 923b5dff96347ae275d52ec76cd4ac5d21f8231b..418adbb5e0155f0027a39064b3067792502f60ba 100644
--- a/src/multibody/joint/joint-base.hpp
+++ b/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.
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index 4fd2c0dfc30086d85f6a0e025d23d499805b15c6..c7bf15faf1f05d421ea95ed9538e6a8fe229130b 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/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(),
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index ca736be4e1125aa91d3a7968abdfd20c7e92f2d8..15c7404e790d0643f8514f9b2d9a1a6b7541bd34 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/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(),
diff --git a/unittest/joint-configurations.cpp b/unittest/joint-configurations.cpp
index bfa14c59557dccad500246511ac8ea07feaf422a..b135d8ef09b6d45c6970cd7858b3203e580e1a60 100644
--- a/unittest/joint-configurations.cpp
+++ b/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 ()