Modified the initialiazor of python, to add various small math entities such as Derivator_matrix.

src/CMakeLists.txt

@@ -233,6 +233,7 @@ INSTALL(FILES
   )
 INSTALL(FILES
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/core/__init__.py
+  ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/core/math_small_entities.py
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/core/feature_position.py
   DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/core
   )

src/dynamic_graph/sot/core/__init__.py

 from robot_simu import RobotSimu
-from vector_constant import VectorConstant
-from matrix_constant import MatrixConstant
 from unary_op import RPYToMatrix, MatrixHomoToPoseRollPitchYaw
-from derivator import Derivator_of_Vector
 from feature_point6d import FeaturePoint6d
 from feature_position import FeaturePosition
 from feature_point6d_relative import FeaturePoint6dRelative
@@ -15,11 +12,8 @@ from gain_adaptive import GainAdaptive
 from sot import SOT
 
 RobotSimu('')
-VectorConstant('')
-MatrixConstant('')
 RPYToMatrix('')
 MatrixHomoToPoseRollPitchYaw('')
-Derivator_of_Vector('')
 FeaturePoint6d('')
 FeaturePoint6dRelative('')
 FeatureGeneric('')
@@ -33,8 +27,7 @@ SOT('')
 from op_point_modifier import OpPointModifier
 OpPointModifier('')
 
-
-
+from math_small_entities import *
 
 
 

src/matrix/derivator.cpp

@@ -25,53 +25,50 @@ using namespace sot;
 using namespace ml;
 using namespace dynamicgraph;
 
+// Unrolled Code:
+// template<>
+// const double Derivator<double>::TIMESTEP_DEFAULT = 1.;
+
+// template<>
+// std::string Derivator<double>::
+// getTypeName( void ) { return #double; }
+
+// template<>
+// const std::string Derivator<double>::CLASS_NAME = std::string("Derivator_of_double");
+// extern "C" {
+//   Entity *regFunctiondoubleDerivator( const std::string& objname )
+//   {
+//     return new Derivator<double>( objname );
+//   }
+//   EntityRegisterer regObjdoubleDerivator
+//   ( std::string("Derivator_of_double"),
+//     &regFunctiondoubleDerivator );
+// }
+
+
 #define SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(sotClassType,sotType,className)              \
   template<>                                                                            \
-  const double Derivator<sotType>::TIMESTEP_DEFAULT = 1.;                            \
+  const double Derivator<sotType>::TIMESTEP_DEFAULT = 1.;                               \
   template<>                                                                            \
   std::string sotClassType<sotType>::                                                   \
   getTypeName( void ) { return #sotType; }                                              \
   template<>                                                                            \
   const std::string sotClassType<sotType>::CLASS_NAME                                   \
-     = std::string(className)+"<"+#sotType+">";                                         \
+    = std::string(className)+"_of_"+#sotType;                                           \
   extern "C" {                                                                          \
-    Entity *regFunction##_##sotType##_##sotClassType( const std::string& objname )                    \
+    Entity *regFunction##_##sotType##_##sotClassType( const std::string& objname )      \
     {                                                                                   \
       return new sotClassType<sotType>( objname );                                      \
     }                                                                                   \
-  EntityRegisterer regObj##_##sotType##_##sotClassType( std::string(className)+"_of_"+#sotType,      \
-					  &regFunction##_##sotType##_##sotClassType );                   \
+  EntityRegisterer regObj##_##sotType##_##sotClassType                                  \
+     ( std::string(className)+"_of_"+#sotType,				                \
+       &regFunction##_##sotType##_##sotClassType );                                     \
   }
 
 namespace sot {
-SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator,double,"Derivator")
-SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator,Vector,"Derivator")
-SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator,Matrix,"Derivator")
-SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator,VectorQuaternion,"Derivator")
+  SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator,double,"Derivator")
+  SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator,Vector,"Derivator")
+  SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator,Matrix,"Derivator")
+  SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator,VectorQuaternion,"Derivator")
 }
 
-//SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator,double,"T");
-
-
-/*
-#define SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(sotClassType,sotType,className,typeName,regFunction,regObj) \
-  template<>                                                \
-  std::string sotClassType<sotType>::                       \
-  getTypeName( void ) { return typeName; }                  \
-  template<>                                                \
-  const std::string sotClassType<sotType>::CLASS_NAME = std::string(className)+typeName; \
-  extern "C" {                                              \
-    Entity *regFunction( const std::string& objname )    \
-    {                                                       \
-      return new sotClassType<sotType>( objname );          \
-    }                                                       \
-  EntityRegisterer regObj( std::string(className)+typeName,&regFunction );   \
-  }
-
-
-SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator,double,"derivator","Double",rfd,rod);
-SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator,ml::Vector,"derivator","Vector",rfv,rov);
-SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator,ml::Matrix,"derivator","Matrix",rfm,rom);
-
-*/
-