[Algo] Change normalize algo to directly work with LieGroups

src/algorithm/joint-configuration.hpp

@@ -116,6 +116,16 @@ namespace se3
   template<typename LieGroup_t>
   inline Eigen::VectorXd randomConfiguration(const Model & model);
 
+  /**
+   * @brief         Normalize a configuration
+   *
+   * @param[in]     model      Model
+   * @param[in,out] q          Configuration to normalize
+   */
+  template<typename LieGroup_t>
+  inline void normalize(const Model & model,
+                        Eigen::VectorXd & q);
+  
   /**
    * @brief         Normalize a configuration
    *

src/algorithm/joint-configuration.hxx

@@ -390,31 +390,45 @@ namespace se3
   {
     return randomConfiguration<LieGroupTpl>(model);
   }
-
-  struct NormalizeStep : public fusion::JointModelVisitor<NormalizeStep>
+  
+  template<typename LieGroup_t, typename JointModel> struct NormalizeStepAlgo;
+  
+  template<typename LieGroup_t>
+  struct NormalizeStep : public fusion::JointModelVisitor< NormalizeStep<LieGroup_t> >
   {
     typedef boost::fusion::vector<Eigen::VectorXd &> ArgsType;
-
+    
     JOINT_MODEL_VISITOR_INIT(NormalizeStep);
-
-    template<typename JointModel>
-    static void algo(const se3::JointModelBase<JointModel> & jmodel,
-                     Eigen::VectorXd & q)
+    
+    SE3_DETAILS_VISITOR_METHOD_ALGO_1(NormalizeStepAlgo, LieGroup_t)
+  };
+  
+  template<typename LieGroup_t, typename JointModel>
+  struct NormalizeStepAlgo
+  {
+    static void run(const se3::JointModelBase<JointModel> & jmodel,
+                    Eigen::VectorXd & qout)
     {
-      jmodel.normalize(q);
+      LieGroup_t::template operation<JointModel>::type::normalize(jmodel.jointConfigSelector(qout));
     }
   };
+  
+  SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NormalizeStep, NormalizeStepAlgo);
 
-  inline void
-  normalize(const Model & model,
-            Eigen::VectorXd & q)
+  template<typename LieGroup_t>
+  inline void normalize(const Model & model, Eigen::VectorXd & qout)
   {
-    for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
+    for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i)
     {
-      NormalizeStep::run(model.joints[i],
-                         NormalizeStep::ArgsType (q));
+      NormalizeStep<LieGroup_t>::run(model.joints[i],
+                                     typename NormalizeStep<LieGroup_t>::ArgsType(qout));
     }
   }
+  
+  inline void normalize(const Model & model, Eigen::VectorXd & qout)
+  {
+    return normalize<LieGroupTpl>(model,qout);
+  }
 
   template<typename LieGroup_t, typename JointModel> struct IsSameConfigurationStepAlgo;
 

unittest/joint-configurations.cpp

@@ -60,13 +60,13 @@ void buildModel(Model & model)
 {
   addJointAndBody(model,JointModelFreeFlyer(),model.getJointId("universe"),SE3::Identity(),"freeflyer",Inertia::Random());
   addJointAndBody(model,JointModelSpherical(),model.getJointId("freeflyer_joint"),SE3::Identity(),"spherical",Inertia::Random());
-  addJointAndBody(model,JointModelRX(),model.getJointId("spherical_joint"),SE3::Identity(),"rx",Inertia::Random());
+  addJointAndBody(model,JointModelPlanar(),model.getJointId("spherical_joint"),SE3::Identity(),"planar",Inertia::Random());
+  addJointAndBody(model,JointModelRX(),model.getJointId("planar_joint"),SE3::Identity(),"rx",Inertia::Random());
   addJointAndBody(model,JointModelPX(),model.getJointId("rx_joint"),SE3::Identity(),"px",Inertia::Random());
   addJointAndBody(model,JointModelPrismaticUnaligned(SE3::Vector3(1,0,0)),model.getJointId("px_joint"),SE3::Identity(),"pu",Inertia::Random());
   addJointAndBody(model,JointModelRevoluteUnaligned(SE3::Vector3(0,0,1)),model.getJointId("pu_joint"),SE3::Identity(),"ru",Inertia::Random());
   addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("ru_joint"),SE3::Identity(),"sphericalZYX",Inertia::Random());
   addJointAndBody(model,JointModelTranslation(),model.getJointId("sphericalZYX_joint"),SE3::Identity(),"translation",Inertia::Random());
-  addJointAndBody(model,JointModelPlanar(),model.getJointId("translation_joint"),SE3::Identity(),"planar",Inertia::Random());
 }
 
 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
@@ -317,13 +317,13 @@ BOOST_AUTO_TEST_CASE ( neutral_configuration_test )
   Eigen::VectorXd expected(model.nq);
   expected << 0,0,0,0,0,0,1,
               0,0,0,1,
+              0,0,1,0,
               0,
               0,
               0,
               0,
               0,0,0,
-              0,0,0,
-              0,0,1,0;
+              0,0,0;
 
 
   BOOST_CHECK_MESSAGE(model.neutralConfiguration.isApprox(expected, 1e-12), "neutral configuration - wrong results");
@@ -365,7 +365,7 @@ BOOST_AUTO_TEST_CASE ( normalize_test )
   BOOST_CHECK(q.head<3>().isApprox(Eigen::VectorXd::Ones(3)));
   BOOST_CHECK(fabs(q.segment<4>(3).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of freeflyer
   BOOST_CHECK(fabs(q.segment<4>(7).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of spherical joint
-  const int n = model.nq - 7 - 4;
+  const int n = model.nq - 7 - 4 - 4; // free flyer + spherical + planar
   BOOST_CHECK(q.tail(n).isApprox(Eigen::VectorXd::Ones(n)));
 }