Merge pull request #833 from jcarpent/devel

Solve bugs in unit tests

cmake @ 9e335a1c

-Subproject commit adfd258cb9368f67b4e84d110110d57ee3ab2f42
+Subproject commit 9e335a1c690403f6dab12ea6707fd9cfd0f596d2

unittest/constraint.cpp

@@ -172,46 +172,133 @@ void test_constraint_operations(const JointModelBase<JointModel> & jmodel)
   
 }
 
-struct TestJointConstraint
+template<typename JointModel_> struct init;
+
+template<typename JointModel_>
+struct init
+{
+  static JointModel_ run()
+  {
+    JointModel_ jmodel;
+    jmodel.setIndexes(0,0,0);
+    return jmodel;
+  }
+};
+
+template<typename Scalar, int Options>
+struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
 {
+  typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel;
   
-  template <typename JointModel>
-  void operator()(const JointModelBase<JointModel> &) const
+  static JointModel run()
   {
-    JointModel jmodel;
+    typedef typename JointModel::Vector3 Vector3;
+    JointModel jmodel(Vector3::Random().normalized());
+    
     jmodel.setIndexes(0,0,0);
+    return jmodel;
+  }
+};
+
+template<typename Scalar, int Options>
+struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
+{
+  typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel;
+  
+  static JointModel run()
+  {
+    typedef typename JointModel::Vector3 Vector3;
+    JointModel jmodel(Vector3::Random().normalized());
     
-    test_constraint_operations(jmodel);
+    jmodel.setIndexes(0,0,0);
+    return jmodel;
   }
+};
+
+template<typename Scalar, int Options>
+struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
+{
+  typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel;
   
-  void operator()(const JointModelBase<JointModelRevoluteUnaligned> &) const
+  static JointModel run()
   {
-    JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
+    typedef typename JointModel::Vector3 Vector3;
+    JointModel jmodel(Vector3::Random().normalized());
+    
     jmodel.setIndexes(0,0,0);
+    return jmodel;
+  }
+};
+
+template<typename Scalar, int Options, template<typename,int> class JointCollection>
+struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
+{
+  typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;
+  
+  static JointModel run()
+  {
+    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
+    JointModel jmodel((JointModelRX()));
     
-    test_constraint_operations(jmodel);
+    jmodel.setIndexes(0,0,0);
+    return jmodel;
   }
+};
+
+template<typename Scalar, int Options, template<typename,int> class JointCollection>
+struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
+{
+  typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;
   
-  void operator()(const JointModelBase<JointModelPrismaticUnaligned> &) const
+  static JointModel run()
   {
-    JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
+    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
+    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
+    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,2> JointModelRZ;
+    
+    JointModel jmodel(JointModelRX(),SE3::Random());
+    jmodel.addJoint(JointModelRY(),SE3::Random());
+    jmodel.addJoint(JointModelRZ(),SE3::Random());
+    
     jmodel.setIndexes(0,0,0);
     
-    test_constraint_operations(jmodel);
+    return jmodel;
   }
+};
+
+template<typename JointModel_>
+struct init<pinocchio::JointModelMimic<JointModel_> >
+{
+  typedef pinocchio::JointModelMimic<JointModel_> JointModel;
   
-  void operator()(const JointModelBase<JointModelComposite> &) const
+  static JointModel run()
   {
-    JointModelComposite jmodel;
+    JointModel_ jmodel_ref = init<JointModel_>::run();
+    
+    JointModel jmodel(jmodel_ref,1.,0.);
     jmodel.setIndexes(0,0,0);
     
-    jmodel.addJoint(JointModelRX(),SE3::Random());
-    jmodel.addJoint(JointModelRY(),SE3::Random());
-    jmodel.addJoint(JointModelRZ(),SE3::Random());
+    return jmodel;
+  }
+};
+
+struct TestJointConstraint
+{
+  
+  template <typename JointModel>
+  void operator()(const JointModelBase<JointModel> &) const
+  {
+    JointModel jmodel = init<JointModel>::run();
+    jmodel.setIndexes(0,0,0);
     
     test_constraint_operations(jmodel);
   }
   
+  void operator()(const JointModelComposite &) const
+  {
+    /* TODO: test JointModelComposite */
+  }
+  
 };
 
 BOOST_AUTO_TEST_CASE(test_joint_constraint_operations)