Add method normalize in JointConfiguration class

  - add inline function to normalize a configuration.

include/hpp/model/configuration.hh

@@ -76,6 +76,21 @@ namespace hpp {
       }
     }
 
+    /// Normalize configuration
+    ///
+    /// Configuration space is a represented by a sub-manifold of a vector
+    /// space. Normalization consists in projecting a vector on this
+    /// sub-manifold. It mostly consists in normalizing quaternions for 
+    /// SO3 joints and 2D-vectors for unbounded rotations.
+    inline void normalize (const DevicePtr_t& robot, ConfigurationOut_t q)
+    {
+      const JointVector_t& jv (robot->getJointVector ());
+      for (model::JointVector_t::const_iterator itJoint = jv.begin ();
+	   itJoint != jv.end (); itJoint++) {
+	size_type indexConfig = (*itJoint)->rankInConfiguration ();
+	(*itJoint)->configuration ()->normalize (indexConfig, q);
+      }
+    }
     /// Write configuration in a string
     inline std::string displayConfig (ConfigurationIn_t q)
     {

include/hpp/model/joint-configuration.hh

@@ -108,12 +108,16 @@ namespace hpp {
 			       const size_type& indexVelocity,
 			       vectorOut_t result) const = 0;
 
+      /// Normalize configuration of joint
+      virtual void normalize (const size_type& index,
+			      ConfigurationOut_t result) const = 0;
       /// Uniformly sample the configuration space of the joint
       /// \param index index of first component of q corresponding to the joint.
       /// \retval result write joint configuration in
       /// result [index:index+nb dofs]
       virtual void uniformlySample (const size_type& index,
 				    ConfigurationOut_t result) const = 0;
+
       /// \name Bounds
       /// @{
       /// Set whether given degree of freedom is bounded
@@ -167,6 +171,9 @@ namespace hpp {
 			       const size_type& indexConfig,
 			       const size_type& indexVelocity,
 			       vectorOut_t result) const;
+      /// Normalize configuration of joint
+      virtual void normalize (const size_type& index,
+			      ConfigurationOut_t result) const;
       virtual void uniformlySample (const size_type& index,
 				    ConfigurationOut_t result) const;
     }; // class AnchorJointConfig
@@ -218,6 +225,9 @@ namespace hpp {
 			       const size_type& indexConfig,
 			       const size_type& indexVelocity,
 			       vectorOut_t result) const;
+      /// Normalize configuration of joint
+      virtual void normalize (const size_type& index,
+			      ConfigurationOut_t result) const;
       virtual void uniformlySample (const size_type& index,
 				    ConfigurationOut_t result) const;
     }; // class SO3JointConfig
@@ -281,6 +291,9 @@ namespace hpp {
 			       const size_type& indexVelocity,
 			       vectorOut_t result) const = 0;
 
+      /// Normalize configuration of joint
+      virtual void normalize (const size_type& index,
+			      ConfigurationOut_t result) const;
       virtual void uniformlySample (const size_type& index,
 				    ConfigurationOut_t result) const = 0;
     }; // class RotationJointConfig
@@ -306,6 +319,9 @@ namespace hpp {
 			 const size_type& indexConfig,
 			 const size_type& indexVelocity,
 			 vectorOut_t result) const;
+	/// Normalize configuration of joint
+	virtual void normalize (const size_type& index,
+			      ConfigurationOut_t result) const;
 	void uniformlySample (const size_type& index,
 			      ConfigurationOut_t result) const;
       }; // class UnBounded
@@ -330,6 +346,9 @@ namespace hpp {
 			 const size_type& indexConfig,
 			 const size_type& indexVelocity,
 			 vectorOut_t result) const;
+	/// Normalize configuration of joint
+	virtual void normalize (const size_type& index,
+				ConfigurationOut_t result) const;
 	void uniformlySample (const size_type& index,
 			      ConfigurationOut_t result) const;
       }; // class Bounded
@@ -383,6 +402,9 @@ namespace hpp {
 			       const size_type& indexVelocity,
 			       vectorOut_t result) const;
 
+      /// Normalize configuration of joint
+      virtual void normalize (const size_type& index,
+			      ConfigurationOut_t result) const;
       virtual void uniformlySample (const size_type& index,
 				    ConfigurationOut_t result) const;
     }; // class TranslationJointConfig

src/joint-configuration.cc

@@ -140,6 +140,12 @@ namespace hpp {
     {
     }
 
+    /// Normalize configuration of joint
+    void AnchorJointConfig::normalize (const size_type&,
+				       ConfigurationOut_t) const
+    {
+    }
+
     void AnchorJointConfig::uniformlySample (const size_type&,
 					     ConfigurationOut_t) const
     {
@@ -242,6 +248,21 @@ namespace hpp {
       result [indexVelocity + 2] = angle*axis [2];
     }
 
+    /// Normalize configuration of joint
+    void SO3JointConfig::normalize (const size_type& index,
+				    ConfigurationOut_t result) const
+    {
+      value_type p0 = result [index];
+      value_type p1 = result [index + 1];
+      value_type p2 = result [index + 2];
+      value_type p3 = result [index + 3];
+      value_type d = sqrt (p0*p0 + p1*p1 + p2*p2 + p3*p3);
+      result [index] /= d;
+      result [index + 1] /= d;
+      result [index + 2] /= d;
+      result [index + 3] /= d;
+    }
+
     void SO3JointConfig::uniformlySample (const size_type& index,
 					  ConfigurationOut_t result) const
     {
@@ -306,6 +327,14 @@ namespace hpp {
 	q2.segment <dimension> (indexConfig);
     }
 
+    /// Normalize configuration of joint
+    /// Do nothing
+    template <size_type dimension>
+    void TranslationJointConfig <dimension>::normalize
+    (const size_type&, ConfigurationOut_t) const
+    {
+    }
+
     template <size_type dimension>
     void TranslationJointConfig <dimension>::uniformlySample
     (const size_type& index, ConfigurationOut_t result) const
@@ -399,6 +428,17 @@ namespace hpp {
 	result [indexVelocity] = atan2 (s1*c2 - s2*c1, c1*c2 + s1*s2);
       }
 
+      /// Normalize configuration of joint
+      void UnBounded::normalize (const size_type& index,
+				 ConfigurationOut_t result) const
+      {
+	value_type c = result [index];
+	value_type s = result [index + 1];
+	value_type d = sqrt (c*c + s*s);
+	result [index] /= d;
+	result [index + 1] /= d;
+      }
+
       void UnBounded::uniformlySample (const size_type& index,
 				       ConfigurationOut_t result) const
       {
@@ -448,6 +488,13 @@ namespace hpp {
 	  result [indexVelocity] = q1 [indexConfig] - q2 [indexConfig];
       }
 
+      /// Normalize configuration of joint
+      /// Do nothing.
+      void Bounded::normalize (const size_type&,
+			       ConfigurationOut_t) const
+      {
+      }
+
       void Bounded::uniformlySample (const size_type& index,
 				     ConfigurationOut_t result) const
       {