From 6392588c32f5baf79f807b439d8260bbade3d004 Mon Sep 17 00:00:00 2001
From: Antoine Hoarau <hoarau.robotics@gmail.com>
Date: Tue, 21 Nov 2017 20:14:10 +0100
Subject: [PATCH 01/10] add eigen_vector2/3/4

---
 eigen_typekit/eigen_typekit.cpp               | 185 +++++++++++-------
 .../orocos/eigen_typekit/eigen_typekit.hpp    |  97 +++++++--
 2 files changed, 192 insertions(+), 90 deletions(-)

diff --git a/eigen_typekit/eigen_typekit.cpp b/eigen_typekit/eigen_typekit.cpp
index 6c333c6..0c2d042 100644
--- a/eigen_typekit/eigen_typekit.cpp
+++ b/eigen_typekit/eigen_typekit.cpp
@@ -34,19 +34,24 @@
 #include <rtt/internal/DataSourceGenerator.hpp>
 #include <boost/lexical_cast.hpp>
 
-
-template class RTT_EXPORT RTT::internal::DataSourceTypeInfo< Eigen::VectorXd >;
-template class RTT_EXPORT RTT::internal::DataSource< Eigen::VectorXd >;
-template class RTT_EXPORT RTT::internal::AssignableDataSource< Eigen::VectorXd >;
-template class RTT_EXPORT RTT::internal::AssignCommand< Eigen::VectorXd >;
-template class RTT_EXPORT RTT::internal::ValueDataSource< Eigen::VectorXd >;
-template class RTT_EXPORT RTT::internal::ConstantDataSource< Eigen::VectorXd >;
-template class RTT_EXPORT RTT::internal::ReferenceDataSource< Eigen::VectorXd >;
-template class RTT_EXPORT RTT::OutputPort< Eigen::VectorXd >;
-template class RTT_EXPORT RTT::InputPort< Eigen::VectorXd >;
-template class RTT_EXPORT RTT::Property< Eigen::VectorXd >;
-template class RTT_EXPORT RTT::Attribute< Eigen::VectorXd >;
-template class RTT_EXPORT RTT::Constant< Eigen::VectorXd >;
+#define DECLARE_RTT_VECTOR_EXPORTS( VectorType) \
+template class RTT_EXPORT RTT::internal::DataSourceTypeInfo< VectorType >; \
+template class RTT_EXPORT RTT::internal::DataSource< VectorType >; \
+template class RTT_EXPORT RTT::internal::AssignableDataSource< VectorType >; \
+template class RTT_EXPORT RTT::internal::AssignCommand< VectorType >; \
+template class RTT_EXPORT RTT::internal::ValueDataSource< VectorType >; \
+template class RTT_EXPORT RTT::internal::ConstantDataSource< VectorType >; \
+template class RTT_EXPORT RTT::internal::ReferenceDataSource< VectorType >; \
+template class RTT_EXPORT RTT::OutputPort< VectorType >; \
+template class RTT_EXPORT RTT::InputPort< VectorType >; \
+template class RTT_EXPORT RTT::Property< VectorType >; \
+template class RTT_EXPORT RTT::Attribute< VectorType >; \
+template class RTT_EXPORT RTT::Constant< VectorType >;
+
+DECLARE_RTT_VECTOR_EXPORTS( Eigen::VectorXd )
+DECLARE_RTT_VECTOR_EXPORTS( Eigen::Vector2d )
+DECLARE_RTT_VECTOR_EXPORTS( Eigen::Vector3d )
+DECLARE_RTT_VECTOR_EXPORTS( Eigen::Vector4d )
 
 template class RTT_EXPORT RTT::internal::DataSourceTypeInfo< Eigen::MatrixXd >;
 template class RTT_EXPORT RTT::internal::DataSource< Eigen::MatrixXd >;
@@ -72,46 +77,55 @@ namespace Eigen{
     std::istream& operator>>(std::istream &is,MatrixXd& v){
       return is;
     }
-    std::istream& operator>>(std::istream &is,VectorXd& v){
-      return is;
-    }
-
-    double& get_item(VectorXd& v, int index)
-    {
-        if (index >= (int) (v.size()) || index < 0)
-            return RTT::internal::NA<double&>::na();
-        return v[index];
-    }
 
-    double get_item_copy(const VectorXd& v, int index)
-    {
-        if (index >= (int) (v.size()) || index < 0)
-            return RTT::internal::NA<double>::na();
-        return v[index];
+#define DECLARE_VECTOR_GET_FUNCTIONS(VectorType) \
+    std::istream& operator>>(std::istream &is,VectorType& v){ \
+      return is; \
+    } \
+    double& get_item(VectorType& v, int index) \
+    { \
+        if (index >= (int) (v.size()) || index < 0) \
+            return RTT::internal::NA<double&>::na(); \
+        return v[index]; \
+    } \
+    double get_item_copy(const VectorType& v, int index) \
+    { \
+        if (index >= (int) (v.size()) || index < 0) \
+            return RTT::internal::NA<double>::na(); \
+        return v[index]; \
+    } \
+    int get_size(const VectorType& v) \
+    { \
+        return v.size(); \
     }
-
-
-    int get_size(const VectorXd& v)
-    {
-        return v.size();
-    }
-
-    struct VectorTypeInfo : public types::TemplateTypeInfo<VectorXd,true>
+    
+    DECLARE_VECTOR_GET_FUNCTIONS( Eigen::VectorXd )
+    DECLARE_VECTOR_GET_FUNCTIONS( Eigen::Vector2d )
+    DECLARE_VECTOR_GET_FUNCTIONS( Eigen::Vector3d )
+    DECLARE_VECTOR_GET_FUNCTIONS( Eigen::Vector4d )
+    
+
+    template<class VectorType>
+    struct VectorTypeInfo : public types::TemplateTypeInfo<VectorType,true>
 #if (RTT_VERSION_MAJOR*100+RTT_VERSION_MINOR) >= 206
                           , public MemberFactory
 #endif
     {
-        VectorTypeInfo():TemplateTypeInfo<VectorXd, true >("eigen_vector")
+        VectorTypeInfo(const std::string& type_name):TemplateTypeInfo<VectorType, true >(type_name), type_name_(type_name)
         {
         };
         
+        const std::string type_name_; 
+        
 #if (RTT_VERSION_MAJOR*100+RTT_VERSION_MINOR) >= 206
         bool installTypeInfoObject(TypeInfo* ti) {
             // aquire a shared reference to the this object
-            boost::shared_ptr< VectorTypeInfo > mthis = boost::dynamic_pointer_cast<VectorTypeInfo >( this->getSharedPtr() );
+            typedef typename boost::shared_ptr< VectorTypeInfo > sh_ptr;
+            sh_ptr mthis = boost::dynamic_pointer_cast<VectorTypeInfo >( this->getSharedPtr() );
+            
             assert(mthis);
             // Allow base to install first
-            TemplateTypeInfo<VectorXd,true>::installTypeInfoObject(ti);
+            TemplateTypeInfo<VectorType,true>::installTypeInfoObject(ti);
             // Install the factories for primitive types
             ti->setMemberFactory( mthis );
             // Don't delete us, we're memory-managed.
@@ -122,7 +136,8 @@ namespace Eigen{
         bool resize(base::DataSourceBase::shared_ptr arg, int size) const
         {
             if (arg->isAssignable()) {
-	        RTT::internal::AssignableDataSource<VectorXd>::shared_ptr asarg = RTT::internal::AssignableDataSource<VectorXd>::narrow( arg.get() );
+	        typedef typename RTT::internal::AssignableDataSource<VectorType>::shared_ptr sh_ptr;
+            sh_ptr asarg = RTT::internal::AssignableDataSource<VectorType>::narrow( arg.get() );
                 asarg->set().resize( size );
                 asarg->updated();
                 return true;
@@ -157,7 +172,8 @@ namespace Eigen{
             if ( id_name ) {
                 if ( id_name->get() == "size" || id_name->get() == "capacity") {
                     try {
-                        return RTT::internal::newFunctorDataSource(&get_size, RTT::internal::GenerateDataSource()(item.get()) );
+                        int (*get_size_pf)(const VectorType&) = get_size;
+                        return RTT::internal::newFunctorDataSource(get_size_pf, RTT::internal::GenerateDataSource()(item.get()) );
                     } catch(...) {}
                 }
             }
@@ -165,9 +181,15 @@ namespace Eigen{
             if ( id_indx ) {
                 try {
                     if ( item->isAssignable() )
-                        return RTT::internal::newFunctorDataSource(&get_item, RTT::internal::GenerateDataSource()(item.get(), id_indx.get() ) );
+                    {
+                        double& (*get_item_pf)(VectorType&,int) = get_item;
+                        return RTT::internal::newFunctorDataSource(get_item_pf, RTT::internal::GenerateDataSource()(item.get(), id_indx.get() ) );
+                    }
                     else
-                        return RTT::internal::newFunctorDataSource(&get_item_copy, RTT::internal::GenerateDataSource()(item.get(), id_indx.get() ) );
+                    {
+                        double (*get_item_copy_pf)(const VectorType&,int) = get_item_copy;
+                        return RTT::internal::newFunctorDataSource(get_item_copy_pf, RTT::internal::GenerateDataSource()(item.get(), id_indx.get() ) );
+                    }
                 } catch(...) {}
             }
             if (id_name) {
@@ -182,9 +204,9 @@ namespace Eigen{
         }
 
 
-        virtual bool decomposeTypeImpl(const VectorXd& vec, PropertyBag& targetbag) const
+        virtual bool decomposeTypeImpl(const VectorType& vec, PropertyBag& targetbag) const
         {
-            targetbag.setType("eigen_vector");
+            targetbag.setType(type_name_);
             int dimension = vec.rows();
             std::string str;
 
@@ -201,9 +223,9 @@ namespace Eigen{
             return true;
         };
 
-      virtual bool composeTypeImpl(const PropertyBag& bag, VectorXd& result) const{
+      virtual bool composeTypeImpl(const PropertyBag& bag, VectorType& result) const{
 
-            if ( bag.getType() == "eigen_vector" ) {
+            if ( bag.getType() == type_name_ ) {
                 int dimension = bag.size();
                 result.resize( dimension );
 
@@ -220,7 +242,7 @@ namespace Eigen{
                     }
                 }
             }else{
-                log(Error) << "Composing Property< VectorXd > :"
+                log(Error) << "Composing Property< VectorType > :"
                            << " type mismatch, got type '"<< bag.getType()
                            << "', expected type "<<"eigen_vector."<<endlog();
                 return false;
@@ -292,10 +314,11 @@ namespace Eigen{
         };
     };
 
+    template<class VectorType>
     struct vector_index
-        : public std::binary_function<const VectorXd&, int, double>
+        : public std::binary_function<const VectorType&, int, double>
     {
-        double operator()(const VectorXd& v, int index) const
+        double operator()(const VectorType& v, int index) const
         {
             if ( index >= (int)(v.size()) || index < 0)
                 return 0.0;
@@ -312,43 +335,46 @@ namespace Eigen{
         }
     };
 
+    template<class VectorType>
     struct vector_index_value_constructor
-        : public std::binary_function<int,double,const VectorXd&>
+        : public std::binary_function<int,double,const VectorType&>
     {
-        typedef const VectorXd& (Signature)( int, double );
-        mutable boost::shared_ptr< VectorXd > ptr;
+        typedef const VectorType& (Signature)( int, double );
+        mutable boost::shared_ptr< VectorType > ptr;
         vector_index_value_constructor() :
-            ptr( new VectorXd ){}
-        const VectorXd& operator()(int size,double value ) const
+            ptr( new VectorType ){}
+        const VectorType& operator()(int size,double value ) const
         {
             ptr->resize(size);
-            (*ptr)=Eigen::VectorXd::Constant(size,value);
+            (*ptr)=VectorType::Constant(size,value);
             return *(ptr);
         }
     };
 
+    template<class VectorType>
     struct vector_index_array_constructor
-        : public std::unary_function<std::vector<double >,const VectorXd&>
+        : public std::unary_function<std::vector<double >,const VectorType&>
     {
-        typedef const VectorXd& (Signature)( std::vector<double > );
-        mutable boost::shared_ptr< VectorXd > ptr;
+        typedef const VectorType& (Signature)( std::vector<double > );
+        mutable boost::shared_ptr< VectorType > ptr;
         vector_index_array_constructor() :
-            ptr( new VectorXd ){}
-        const VectorXd& operator()(std::vector<double > values) const
+            ptr( new VectorType ){}
+        const VectorType& operator()(std::vector<double > values) const
         {
-            (*ptr)=VectorXd::Map(&values[0],values.size());
+            (*ptr)=VectorType::Map(&values[0],values.size());
             return *(ptr);
         }
     };
 
+    template<class VectorType>
     struct vector_index_constructor
-        : public std::unary_function<int,const VectorXd&>
+        : public std::unary_function<int,const VectorType&>
     {
-        typedef const VectorXd& (Signature)( int );
-        mutable boost::shared_ptr< VectorXd > ptr;
+        typedef const VectorType& (Signature)( int );
+        mutable boost::shared_ptr< VectorType > ptr;
         vector_index_constructor() :
-            ptr( new VectorXd() ){}
-        const VectorXd& operator()(int size ) const
+            ptr( new VectorType() ){}
+        const VectorType& operator()(int size ) const
         {
             ptr->resize(size);
             return *(ptr);
@@ -387,23 +413,38 @@ namespace Eigen{
 
     bool EigenTypekitPlugin::loadTypes()
     {
-        RTT::types::TypeInfoRepository::Instance()->addType( new VectorTypeInfo() );
+        RTT::types::TypeInfoRepository::Instance()->addType( new VectorTypeInfo<VectorXd>("eigen_vector") );
+        RTT::types::TypeInfoRepository::Instance()->addType( new VectorTypeInfo<Vector2d>("eigen_vector2") );
+        RTT::types::TypeInfoRepository::Instance()->addType( new VectorTypeInfo<Vector3d>("eigen_vector3") );
+        RTT::types::TypeInfoRepository::Instance()->addType( new VectorTypeInfo<Vector4d>("eigen_vector4") );
         RTT::types::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo() );
         return true;
     }
 
     bool EigenTypekitPlugin::loadConstructors()
     {
-        RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_constructor()));
-        RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_value_constructor()));
-        RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_array_constructor()));
+        RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_constructor<VectorXd>()));
+        RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_index_constructor<Vector2d>()));
+        RTT::types::Types()->type("eigen_vector3")->addConstructor(types::newConstructor(vector_index_constructor<Vector3d>()));
+        RTT::types::Types()->type("eigen_vector4")->addConstructor(types::newConstructor(vector_index_constructor<Vector4d>()));
+        RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_value_constructor<VectorXd>()));
+        RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_array_constructor<VectorXd>()));
+        RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_index_value_constructor<Vector2d>()));
+        RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_index_array_constructor<Vector2d>()));
+        RTT::types::Types()->type("eigen_vector3")->addConstructor(types::newConstructor(vector_index_value_constructor<Vector3d>()));
+        RTT::types::Types()->type("eigen_vector3")->addConstructor(types::newConstructor(vector_index_array_constructor<Vector3d>()));
+        RTT::types::Types()->type("eigen_vector4")->addConstructor(types::newConstructor(vector_index_value_constructor<Vector4d>()));
+        RTT::types::Types()->type("eigen_vector4")->addConstructor(types::newConstructor(vector_index_array_constructor<Vector4d>()));
         RTT::types::Types()->type("eigen_matrix")->addConstructor(types::newConstructor(matrix_i_j_constructor()));
         return true;
     }
 
     bool EigenTypekitPlugin::loadOperators()
     {
-        RTT::types::OperatorRepository::Instance()->add( newBinaryOperator( "[]", vector_index() ) );
+        RTT::types::OperatorRepository::Instance()->add( newBinaryOperator( "[]", vector_index<VectorXd>() ) );
+        RTT::types::OperatorRepository::Instance()->add( newBinaryOperator( "[]", vector_index<Vector2d>() ) );
+        RTT::types::OperatorRepository::Instance()->add( newBinaryOperator( "[]", vector_index<Vector3d>() ) );
+        RTT::types::OperatorRepository::Instance()->add( newBinaryOperator( "[]", vector_index<Vector4d>() ) );
         //RTT::types::OperatorRepository::Instance()->add( newDotOperator( "size", get_size() ) );
         //RTT::types::OperatorRepository::Instance()->add( newTernaryOperator( "[,]", matrix_index() ) );
         return true;
diff --git a/eigen_typekit/include/orocos/eigen_typekit/eigen_typekit.hpp b/eigen_typekit/include/orocos/eigen_typekit/eigen_typekit.hpp
index a2c2938..b51c23f 100644
--- a/eigen_typekit/include/orocos/eigen_typekit/eigen_typekit.hpp
+++ b/eigen_typekit/include/orocos/eigen_typekit/eigen_typekit.hpp
@@ -37,54 +37,115 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
 	virtual bool loadOperators();
 };
 }
+
 #ifdef CORELIB_DATASOURCE_HPP
-    extern template class RTT::internal::DataSourceTypeInfo< Eigen::VectorXd >;
-    extern template class RTT::internal::DataSource< Eigen::VectorXd >;
-    extern template class RTT::internal::AssignableDataSource< Eigen::VectorXd >;
-    extern template class RTT::internal::AssignCommand< Eigen::VectorXd >;
+#define DECLARE_VECTOR_CORELIB_DATASOURCE_HPP(VectorType) \
+    extern template class RTT::internal::DataSourceTypeInfo< VectorType >; \
+    extern template class RTT::internal::DataSource< VectorType >; \
+    extern template class RTT::internal::AssignableDataSource< VectorType >; \
+    extern template class RTT::internal::AssignCommand< VectorType >;
+
+    DECLARE_VECTOR_CORELIB_DATASOURCE_HPP(Eigen::VectorXd)
+    DECLARE_VECTOR_CORELIB_DATASOURCE_HPP(Eigen::Vector2d)
+    DECLARE_VECTOR_CORELIB_DATASOURCE_HPP(Eigen::Vector3d)
+    DECLARE_VECTOR_CORELIB_DATASOURCE_HPP(Eigen::Vector4d)
 #endif
+
 #ifdef ORO_CORELIB_DATASOURCES_HPP
-    extern template class RTT::internal::ValueDataSource< Eigen::VectorXd >;
-    extern template class RTT::internal::ConstantDataSource< Eigen::VectorXd >;
-    extern template class RTT::internal::ReferenceDataSource< Eigen::VectorXd >;
+#define DECLARE_VECTOR_ORO_CORELIB_DATASOURCES_HPP(VectorType) \
+    extern template class RTT::internal::ValueDataSource< VectorType >; \
+    extern template class RTT::internal::ConstantDataSource< VectorType >; \
+    extern template class RTT::internal::ReferenceDataSource< VectorType >;
+    
+    DECLARE_VECTOR_ORO_CORELIB_DATASOURCES_HPP(Eigen::VectorXd)
+    DECLARE_VECTOR_ORO_CORELIB_DATASOURCES_HPP(Eigen::Vector2d)
+    DECLARE_VECTOR_ORO_CORELIB_DATASOURCES_HPP(Eigen::Vector3d)
+    DECLARE_VECTOR_ORO_CORELIB_DATASOURCES_HPP(Eigen::Vector4d)
 #endif
+
 #ifdef ORO_OUTPUT_PORT_HPP
     extern template class RTT::OutputPort< Eigen::VectorXd >;
+    extern template class RTT::OutputPort< Eigen::Vector2d >;
+    extern template class RTT::OutputPort< Eigen::Vector3d >;
+    extern template class RTT::OutputPort< Eigen::Vector4d >;
 #endif
+
 #ifdef ORO_INPUT_PORT_HPP
     extern template class RTT::InputPort< Eigen::VectorXd >;
+    extern template class RTT::InputPort< Eigen::Vector2d >;
+    extern template class RTT::InputPort< Eigen::Vector3d >;
+    extern template class RTT::InputPort< Eigen::Vector4d >;
 #endif
+
 #ifdef ORO_PROPERTY_HPP
     extern template class RTT::Property< Eigen::VectorXd >;
+    extern template class RTT::Property< Eigen::Vector2d >;
+    extern template class RTT::Property< Eigen::Vector3d >;
+    extern template class RTT::Property< Eigen::Vector4d >;
 #endif
+
 #ifdef ORO_CORELIB_ATTRIBUTE_HPP
-    extern template class RTT::Attribute< Eigen::VectorXd >;
-    extern template class RTT::Constant< Eigen::VectorXd >;
+#define DECLARE_VECTOR_ORO_CORELIB_ATTRIBUTE_HPP(VectorType) \
+    extern template class RTT::Attribute< VectorType >; \
+    extern template class RTT::Constant< VectorType >;
+    
+    DECLARE_VECTOR_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::VectorXd)
+    DECLARE_VECTOR_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Vector2d)
+    DECLARE_VECTOR_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Vector3d)
+    DECLARE_VECTOR_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Vector4d)
 #endif
 
+
 #ifdef CORELIB_DATASOURCE_HPP
-    extern template class RTT::internal::DataSourceTypeInfo< Eigen::MatrixXd >;
-    extern template class RTT::internal::DataSource< Eigen::MatrixXd >;
-    extern template class RTT::internal::AssignableDataSource< Eigen::MatrixXd >;
-    extern template class RTT::internal::AssignCommand< Eigen::MatrixXd >;
+#define DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(MatrixType) \
+    extern template class RTT::internal::DataSourceTypeInfo< MatrixType >; \
+    extern template class RTT::internal::DataSource< MatrixType >; \
+    extern template class RTT::internal::AssignableDataSource< MatrixType >; \
+    extern template class RTT::internal::AssignCommand< MatrixType >;
+    
+    DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::MatrixXd)
+    DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix2d)
+    DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix3d)
+    DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix4d)
 #endif
 #ifdef ORO_CORELIB_DATASOURCES_HPP
-    extern template class RTT::internal::ValueDataSource< Eigen::MatrixXd >;
-    extern template class RTT::internal::ConstantDataSource< Eigen::MatrixXd >;
-    extern template class RTT::internal::ReferenceDataSource< Eigen::MatrixXd >;
+#define DECLARE_MATRIX_ORO_CORELIB_DATASOURCES_HPP(MatrixType) \
+    extern template class RTT::internal::ValueDataSource< MatrixType >; \
+    extern template class RTT::internal::ConstantDataSource< MatrixType >; \
+    extern template class RTT::internal::ReferenceDataSource< MatrixType >;
+    
+    DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::MatrixXd)
+    DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix2d)
+    DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix3d)
+    DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix4d)
 #endif
 #ifdef ORO_OUTPUT_PORT_HPP
     extern template class RTT::OutputPort< Eigen::MatrixXd >;
+    extern template class RTT::OutputPort< Eigen::Matrix2d >;
+    extern template class RTT::OutputPort< Eigen::Matrix3d >;
+    extern template class RTT::OutputPort< Eigen::Matrix4d >;
 #endif
 #ifdef ORO_INPUT_PORT_HPP
     extern template class RTT::InputPort< Eigen::MatrixXd >;
+    extern template class RTT::InputPort< Eigen::Matrix2d >;
+    extern template class RTT::InputPort< Eigen::Matrix3d >;
+    extern template class RTT::InputPort< Eigen::Matrix4d >;
 #endif
 #ifdef ORO_PROPERTY_HPP
     extern template class RTT::Property< Eigen::MatrixXd >;
+    extern template class RTT::Property< Eigen::Matrix2d >;
+    extern template class RTT::Property< Eigen::Matrix3d >;
+    extern template class RTT::Property< Eigen::Matrix4d >;
 #endif
 #ifdef ORO_CORELIB_ATTRIBUTE_HPP
-    extern template class RTT::Attribute< Eigen::MatrixXd >;
-    extern template class RTT::Constant< Eigen::MatrixXd >;
+#define DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(MatrixType) \
+    extern template class RTT::Attribute< MatrixType >; \
+    extern template class RTT::Constant< MatrixType >;
+    
+    DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::MatrixXd)
+    DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Matrix2d)
+    DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Matrix3d)
+    DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Matrix4d)
 #endif
 
 #endif // ifndef EIGEN_TYPEKIT_HPP

From 7e1a5bcbeefb6fb04c506127ccb06ff343fffd92 Mon Sep 17 00:00:00 2001
From: Antoine Hoarau <hoarau.robotics@gmail.com>
Date: Fri, 1 Dec 2017 16:32:25 +0100
Subject: [PATCH 02/10] add fixed size matrices

+ Vector6d/Matrix6d
---
 eigen_typekit/eigen_typekit.cpp               | 167 ++++++++++--------
 .../orocos/eigen_typekit/eigen_typekit.hpp    |  15 ++
 2 files changed, 106 insertions(+), 76 deletions(-)

diff --git a/eigen_typekit/eigen_typekit.cpp b/eigen_typekit/eigen_typekit.cpp
index 0c2d042..2f76c83 100644
--- a/eigen_typekit/eigen_typekit.cpp
+++ b/eigen_typekit/eigen_typekit.cpp
@@ -34,7 +34,7 @@
 #include <rtt/internal/DataSourceGenerator.hpp>
 #include <boost/lexical_cast.hpp>
 
-#define DECLARE_RTT_VECTOR_EXPORTS( VectorType) \
+#define DECLARE_RTT_VECTOR_EXPORTS( VectorType ) \
 template class RTT_EXPORT RTT::internal::DataSourceTypeInfo< VectorType >; \
 template class RTT_EXPORT RTT::internal::DataSource< VectorType >; \
 template class RTT_EXPORT RTT::internal::AssignableDataSource< VectorType >; \
@@ -52,20 +52,27 @@ DECLARE_RTT_VECTOR_EXPORTS( Eigen::VectorXd )
 DECLARE_RTT_VECTOR_EXPORTS( Eigen::Vector2d )
 DECLARE_RTT_VECTOR_EXPORTS( Eigen::Vector3d )
 DECLARE_RTT_VECTOR_EXPORTS( Eigen::Vector4d )
-
-template class RTT_EXPORT RTT::internal::DataSourceTypeInfo< Eigen::MatrixXd >;
-template class RTT_EXPORT RTT::internal::DataSource< Eigen::MatrixXd >;
-template class RTT_EXPORT RTT::internal::AssignableDataSource< Eigen::MatrixXd >;
-template class RTT_EXPORT RTT::internal::AssignCommand< Eigen::MatrixXd >;
-template class RTT_EXPORT RTT::internal::ValueDataSource< Eigen::MatrixXd >;
-template class RTT_EXPORT RTT::internal::ConstantDataSource< Eigen::MatrixXd >;
-template class RTT_EXPORT RTT::internal::ReferenceDataSource< Eigen::MatrixXd >;
-template class RTT_EXPORT RTT::OutputPort< Eigen::MatrixXd >;
-template class RTT_EXPORT RTT::InputPort< Eigen::MatrixXd >;
-template class RTT_EXPORT RTT::Property< Eigen::MatrixXd >;
-template class RTT_EXPORT RTT::Attribute< Eigen::MatrixXd >;
-template class RTT_EXPORT RTT::Constant< Eigen::MatrixXd >;
-
+DECLARE_RTT_VECTOR_EXPORTS( Eigen::Vector6d )
+
+#define DECLARE_RTT_MATRIX_EXPORTS( MatrixType ) \
+template class RTT_EXPORT RTT::internal::DataSourceTypeInfo< MatrixType >; \
+template class RTT_EXPORT RTT::internal::DataSource< MatrixType >; \
+template class RTT_EXPORT RTT::internal::AssignableDataSource< MatrixType >; \
+template class RTT_EXPORT RTT::internal::AssignCommand< MatrixType >; \
+template class RTT_EXPORT RTT::internal::ValueDataSource< MatrixType >; \
+template class RTT_EXPORT RTT::internal::ConstantDataSource< MatrixType >; \
+template class RTT_EXPORT RTT::internal::ReferenceDataSource< MatrixType >; \
+template class RTT_EXPORT RTT::OutputPort< MatrixType >; \
+template class RTT_EXPORT RTT::InputPort< MatrixType >; \
+template class RTT_EXPORT RTT::Property< MatrixType >; \
+template class RTT_EXPORT RTT::Attribute< MatrixType >; \
+template class RTT_EXPORT RTT::Constant< MatrixType >;
+
+DECLARE_RTT_MATRIX_EXPORTS( Eigen::MatrixXd )
+DECLARE_RTT_MATRIX_EXPORTS( Eigen::Matrix2d )
+DECLARE_RTT_MATRIX_EXPORTS( Eigen::Matrix3d )
+DECLARE_RTT_MATRIX_EXPORTS( Eigen::Matrix4d )
+DECLARE_RTT_MATRIX_EXPORTS( Eigen::Matrix6d )
 
 #include <Eigen/Core>
 namespace Eigen{
@@ -74,36 +81,32 @@ namespace Eigen{
     using namespace RTT::detail;
     using namespace RTT::types;
 
-    std::istream& operator>>(std::istream &is,MatrixXd& v){
+    template<typename Derived>
+    std::istream& operator>>(std::istream &is, EigenBase<Derived>& v){
       return is;
     }
-
-#define DECLARE_VECTOR_GET_FUNCTIONS(VectorType) \
-    std::istream& operator>>(std::istream &is,VectorType& v){ \
-      return is; \
-    } \
-    double& get_item(VectorType& v, int index) \
-    { \
-        if (index >= (int) (v.size()) || index < 0) \
-            return RTT::internal::NA<double&>::na(); \
-        return v[index]; \
-    } \
-    double get_item_copy(const VectorType& v, int index) \
-    { \
-        if (index >= (int) (v.size()) || index < 0) \
-            return RTT::internal::NA<double>::na(); \
-        return v[index]; \
-    } \
-    int get_size(const VectorType& v) \
-    { \
-        return v.size(); \
+    
+    template<typename VectorType>
+    double& get_item(VectorType& v, int index)
+    {
+        if (index >= (int) (v.size()) || index < 0)
+            return RTT::internal::NA<double&>::na();
+        return v[index];
     }
     
-    DECLARE_VECTOR_GET_FUNCTIONS( Eigen::VectorXd )
-    DECLARE_VECTOR_GET_FUNCTIONS( Eigen::Vector2d )
-    DECLARE_VECTOR_GET_FUNCTIONS( Eigen::Vector3d )
-    DECLARE_VECTOR_GET_FUNCTIONS( Eigen::Vector4d )
+    template<typename VectorType>
+    double get_item_copy(const VectorType& v, int index)
+    {
+        if (index >= (int) (v.size()) || index < 0)
+            return RTT::internal::NA<double>::na();
+        return v[index];
+    }
     
+    template<typename VectorType>
+    int get_size(const VectorType& v)
+    {
+        return v.size();
+    }
 
     template<class VectorType>
     struct VectorTypeInfo : public types::TemplateTypeInfo<VectorType,true>
@@ -111,12 +114,10 @@ namespace Eigen{
                           , public MemberFactory
 #endif
     {
-        VectorTypeInfo(const std::string& type_name):TemplateTypeInfo<VectorType, true >(type_name), type_name_(type_name)
+        VectorTypeInfo(const std::string& type_name):TemplateTypeInfo<VectorType, true >(type_name)
         {
         };
-        
-        const std::string type_name_; 
-        
+                
 #if (RTT_VERSION_MAJOR*100+RTT_VERSION_MINOR) >= 206
         bool installTypeInfoObject(TypeInfo* ti) {
             // aquire a shared reference to the this object
@@ -136,8 +137,8 @@ namespace Eigen{
         bool resize(base::DataSourceBase::shared_ptr arg, int size) const
         {
             if (arg->isAssignable()) {
-	        typedef typename RTT::internal::AssignableDataSource<VectorType>::shared_ptr sh_ptr;
-            sh_ptr asarg = RTT::internal::AssignableDataSource<VectorType>::narrow( arg.get() );
+	        typedef typename RTT::internal::AssignableDataSource<VectorType >::shared_ptr sh_ptr;
+            sh_ptr asarg = RTT::internal::AssignableDataSource<VectorType >::narrow( arg.get() );
                 asarg->set().resize( size );
                 asarg->updated();
                 return true;
@@ -172,8 +173,7 @@ namespace Eigen{
             if ( id_name ) {
                 if ( id_name->get() == "size" || id_name->get() == "capacity") {
                     try {
-                        int (*get_size_pf)(const VectorType&) = get_size;
-                        return RTT::internal::newFunctorDataSource(get_size_pf, RTT::internal::GenerateDataSource()(item.get()) );
+                        return RTT::internal::newFunctorDataSource(&get_size<VectorType>, RTT::internal::GenerateDataSource()(item.get()) );
                     } catch(...) {}
                 }
             }
@@ -182,13 +182,11 @@ namespace Eigen{
                 try {
                     if ( item->isAssignable() )
                     {
-                        double& (*get_item_pf)(VectorType&,int) = get_item;
-                        return RTT::internal::newFunctorDataSource(get_item_pf, RTT::internal::GenerateDataSource()(item.get(), id_indx.get() ) );
+                        return RTT::internal::newFunctorDataSource(get_item<VectorType>, RTT::internal::GenerateDataSource()(item.get(), id_indx.get() ) );
                     }
                     else
                     {
-                        double (*get_item_copy_pf)(const VectorType&,int) = get_item_copy;
-                        return RTT::internal::newFunctorDataSource(get_item_copy_pf, RTT::internal::GenerateDataSource()(item.get(), id_indx.get() ) );
+                        return RTT::internal::newFunctorDataSource(get_item_copy<VectorType>, RTT::internal::GenerateDataSource()(item.get(), id_indx.get() ) );
                     }
                 } catch(...) {}
             }
@@ -206,7 +204,7 @@ namespace Eigen{
 
         virtual bool decomposeTypeImpl(const VectorType& vec, PropertyBag& targetbag) const
         {
-            targetbag.setType(type_name_);
+            targetbag.setType(this->getTypeName());
             int dimension = vec.rows();
             std::string str;
 
@@ -225,7 +223,7 @@ namespace Eigen{
 
       virtual bool composeTypeImpl(const PropertyBag& bag, VectorType& result) const{
 
-            if ( bag.getType() == type_name_ ) {
+            if ( bag.getType() == this->getTypeName() ) {
                 int dimension = bag.size();
                 result.resize( dimension );
 
@@ -242,7 +240,7 @@ namespace Eigen{
                     }
                 }
             }else{
-                log(Error) << "Composing Property< VectorType > :"
+                log(Error) << "Composing Property< " << this->getTypeName() << " > :"
                            << " type mismatch, got type '"<< bag.getType()
                            << "', expected type "<<"eigen_vector."<<endlog();
                 return false;
@@ -251,13 +249,14 @@ namespace Eigen{
         };
     };
 
-    struct MatrixTypeInfo : public types::TemplateTypeInfo<MatrixXd,true>{
-        MatrixTypeInfo():TemplateTypeInfo<MatrixXd, true >("eigen_matrix"){
+    template<typename MatrixType>
+    struct MatrixTypeInfo : public types::TemplateTypeInfo<MatrixType,true>{
+        MatrixTypeInfo(const std::string& type_name):TemplateTypeInfo<MatrixType, true >(type_name){
         };
 
 
-        bool decomposeTypeImpl(const MatrixXd& mat, PropertyBag& targetbag) const{
-            targetbag.setType("eigen_matrix");
+        bool decomposeTypeImpl(const MatrixType& mat, PropertyBag& targetbag) const{
+            targetbag.setType(this->getTypeName());
             unsigned int dimension = mat.rows();
             if(!targetbag.empty())
                 return false;
@@ -271,8 +270,8 @@ namespace Eigen{
             return true;
         };
 
-        bool composeTypeImpl(const PropertyBag& bag, MatrixXd& result) const{
-            if ( bag.getType() == "eigen_matrix" ) {
+        bool composeTypeImpl(const PropertyBag& bag, MatrixType& result) const{
+            if ( bag.getType() == this->getTypeName() ) {
                 unsigned int rows = bag.size();
                 unsigned int cols = 0;
                 // Get values
@@ -305,7 +304,7 @@ namespace Eigen{
                     }
                 }
             }else {
-                log(Error) << "Composing Property< MatrixXd > :"
+                log(Error) << "Composing Property< " << this->getTypeName() << " > :"
                            << " type mismatch, got type '"<< bag.getType()
                            << "', expected type "<<"ublas_matrix."<<endlog();
                 return false;
@@ -325,11 +324,12 @@ namespace Eigen{
             return v(index);
         }
     };
-
-    struct get_size
-        : public std::unary_function<const VectorXd&, int>
+    
+    template<class VectorType>
+    struct get_size_functor
+        : public std::unary_function<const VectorType&, int>
     {
-        int operator()(const VectorXd& cont ) const
+        int operator()(const VectorType& cont ) const
         {
             return cont.rows();
         }
@@ -381,24 +381,26 @@ namespace Eigen{
         }
     };
 
-    struct matrix_index
-        : public std::ternary_function<const MatrixXd&, int, int, double>
+    template<typename MatrixType>
+    struct matrix_index_functor
+        : public std::ternary_function<const MatrixType&, int, int, double>
     {
-        double operator()(const MatrixXd& m, int i, int j) const{
+        double operator()(const MatrixType& m, int i, int j) const{
             if ( i >= (int)(m.rows()) || i < 0 || j<0 || j>= (int)(m.cols()))
                 return 0.0;
             return m(i,j);
         }
     };
 
+    template<typename MatrixType>
     struct matrix_i_j_constructor
-        : public std::binary_function<int,int,const MatrixXd&>
+        : public std::binary_function<int,int,const MatrixType&>
     {
-        typedef const MatrixXd& (Signature)( int, int );
-        mutable boost::shared_ptr< MatrixXd > ptr;
+        typedef const MatrixType& (Signature)( int, int );
+        mutable boost::shared_ptr< MatrixType > ptr;
         matrix_i_j_constructor() :
-            ptr( new MatrixXd() ){}
-        const MatrixXd& operator()(int size1,int size2) const
+            ptr( new MatrixType() ){}
+        const MatrixType& operator()(int size1,int size2) const
         {
             ptr->resize(size1,size2);
             return *(ptr);
@@ -417,7 +419,12 @@ namespace Eigen{
         RTT::types::TypeInfoRepository::Instance()->addType( new VectorTypeInfo<Vector2d>("eigen_vector2") );
         RTT::types::TypeInfoRepository::Instance()->addType( new VectorTypeInfo<Vector3d>("eigen_vector3") );
         RTT::types::TypeInfoRepository::Instance()->addType( new VectorTypeInfo<Vector4d>("eigen_vector4") );
-        RTT::types::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo() );
+        RTT::types::TypeInfoRepository::Instance()->addType( new VectorTypeInfo<Vector6d>("eigen_vector6") );
+        RTT::types::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo<MatrixXd>("eigen_matrix") );
+        RTT::types::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo<Matrix2d>("eigen_matrix2") );
+        RTT::types::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo<Matrix3d>("eigen_matrix3") );
+        RTT::types::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo<Matrix4d>("eigen_matrix4") );
+        RTT::types::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo<Matrix6d>("eigen_matrix6") );
         return true;
     }
 
@@ -427,6 +434,7 @@ namespace Eigen{
         RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_index_constructor<Vector2d>()));
         RTT::types::Types()->type("eigen_vector3")->addConstructor(types::newConstructor(vector_index_constructor<Vector3d>()));
         RTT::types::Types()->type("eigen_vector4")->addConstructor(types::newConstructor(vector_index_constructor<Vector4d>()));
+        RTT::types::Types()->type("eigen_vector6")->addConstructor(types::newConstructor(vector_index_constructor<Vector6d>()));
         RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_value_constructor<VectorXd>()));
         RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_array_constructor<VectorXd>()));
         RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_index_value_constructor<Vector2d>()));
@@ -435,7 +443,13 @@ namespace Eigen{
         RTT::types::Types()->type("eigen_vector3")->addConstructor(types::newConstructor(vector_index_array_constructor<Vector3d>()));
         RTT::types::Types()->type("eigen_vector4")->addConstructor(types::newConstructor(vector_index_value_constructor<Vector4d>()));
         RTT::types::Types()->type("eigen_vector4")->addConstructor(types::newConstructor(vector_index_array_constructor<Vector4d>()));
-        RTT::types::Types()->type("eigen_matrix")->addConstructor(types::newConstructor(matrix_i_j_constructor()));
+        RTT::types::Types()->type("eigen_vector6")->addConstructor(types::newConstructor(vector_index_value_constructor<Vector6d>()));
+        RTT::types::Types()->type("eigen_vector6")->addConstructor(types::newConstructor(vector_index_array_constructor<Vector6d>()));
+        RTT::types::Types()->type("eigen_matrix")->addConstructor(types::newConstructor(matrix_i_j_constructor<MatrixXd>()));
+        RTT::types::Types()->type("eigen_matrix2")->addConstructor(types::newConstructor(matrix_i_j_constructor<Matrix2d>()));
+        RTT::types::Types()->type("eigen_matrix3")->addConstructor(types::newConstructor(matrix_i_j_constructor<Matrix3d>()));
+        RTT::types::Types()->type("eigen_matrix4")->addConstructor(types::newConstructor(matrix_i_j_constructor<Matrix4d>()));
+        RTT::types::Types()->type("eigen_matrix6")->addConstructor(types::newConstructor(matrix_i_j_constructor<Matrix6d>()));
         return true;
     }
 
@@ -445,6 +459,7 @@ namespace Eigen{
         RTT::types::OperatorRepository::Instance()->add( newBinaryOperator( "[]", vector_index<Vector2d>() ) );
         RTT::types::OperatorRepository::Instance()->add( newBinaryOperator( "[]", vector_index<Vector3d>() ) );
         RTT::types::OperatorRepository::Instance()->add( newBinaryOperator( "[]", vector_index<Vector4d>() ) );
+        RTT::types::OperatorRepository::Instance()->add( newBinaryOperator( "[]", vector_index<Vector6d>() ) );
         //RTT::types::OperatorRepository::Instance()->add( newDotOperator( "size", get_size() ) );
         //RTT::types::OperatorRepository::Instance()->add( newTernaryOperator( "[,]", matrix_index() ) );
         return true;
diff --git a/eigen_typekit/include/orocos/eigen_typekit/eigen_typekit.hpp b/eigen_typekit/include/orocos/eigen_typekit/eigen_typekit.hpp
index b51c23f..a692d91 100644
--- a/eigen_typekit/include/orocos/eigen_typekit/eigen_typekit.hpp
+++ b/eigen_typekit/include/orocos/eigen_typekit/eigen_typekit.hpp
@@ -28,6 +28,9 @@
 // import most common Eigen types 
 namespace Eigen {
 
+typedef Matrix<double,6,1> Vector6d;
+typedef Matrix<double,6,6> Matrix6d;
+
 class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
 public:
 	virtual std::string getName();
@@ -49,6 +52,7 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
     DECLARE_VECTOR_CORELIB_DATASOURCE_HPP(Eigen::Vector2d)
     DECLARE_VECTOR_CORELIB_DATASOURCE_HPP(Eigen::Vector3d)
     DECLARE_VECTOR_CORELIB_DATASOURCE_HPP(Eigen::Vector4d)
+    DECLARE_VECTOR_CORELIB_DATASOURCE_HPP(Eigen::Vector6d)
 #endif
 
 #ifdef ORO_CORELIB_DATASOURCES_HPP
@@ -61,6 +65,7 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
     DECLARE_VECTOR_ORO_CORELIB_DATASOURCES_HPP(Eigen::Vector2d)
     DECLARE_VECTOR_ORO_CORELIB_DATASOURCES_HPP(Eigen::Vector3d)
     DECLARE_VECTOR_ORO_CORELIB_DATASOURCES_HPP(Eigen::Vector4d)
+    DECLARE_VECTOR_ORO_CORELIB_DATASOURCES_HPP(Eigen::Vector6d)
 #endif
 
 #ifdef ORO_OUTPUT_PORT_HPP
@@ -68,6 +73,7 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
     extern template class RTT::OutputPort< Eigen::Vector2d >;
     extern template class RTT::OutputPort< Eigen::Vector3d >;
     extern template class RTT::OutputPort< Eigen::Vector4d >;
+    extern template class RTT::OutputPort< Eigen::Vector6d >;
 #endif
 
 #ifdef ORO_INPUT_PORT_HPP
@@ -75,6 +81,7 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
     extern template class RTT::InputPort< Eigen::Vector2d >;
     extern template class RTT::InputPort< Eigen::Vector3d >;
     extern template class RTT::InputPort< Eigen::Vector4d >;
+    extern template class RTT::InputPort< Eigen::Vector6d >;
 #endif
 
 #ifdef ORO_PROPERTY_HPP
@@ -82,6 +89,7 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
     extern template class RTT::Property< Eigen::Vector2d >;
     extern template class RTT::Property< Eigen::Vector3d >;
     extern template class RTT::Property< Eigen::Vector4d >;
+    extern template class RTT::Property< Eigen::Vector6d >;
 #endif
 
 #ifdef ORO_CORELIB_ATTRIBUTE_HPP
@@ -93,6 +101,7 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
     DECLARE_VECTOR_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Vector2d)
     DECLARE_VECTOR_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Vector3d)
     DECLARE_VECTOR_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Vector4d)
+    DECLARE_VECTOR_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Vector6d)
 #endif
 
 
@@ -107,6 +116,7 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
     DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix2d)
     DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix3d)
     DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix4d)
+    DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix6d)
 #endif
 #ifdef ORO_CORELIB_DATASOURCES_HPP
 #define DECLARE_MATRIX_ORO_CORELIB_DATASOURCES_HPP(MatrixType) \
@@ -118,24 +128,28 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
     DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix2d)
     DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix3d)
     DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix4d)
+    DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix6d)
 #endif
 #ifdef ORO_OUTPUT_PORT_HPP
     extern template class RTT::OutputPort< Eigen::MatrixXd >;
     extern template class RTT::OutputPort< Eigen::Matrix2d >;
     extern template class RTT::OutputPort< Eigen::Matrix3d >;
     extern template class RTT::OutputPort< Eigen::Matrix4d >;
+    extern template class RTT::OutputPort< Eigen::Matrix6d >;
 #endif
 #ifdef ORO_INPUT_PORT_HPP
     extern template class RTT::InputPort< Eigen::MatrixXd >;
     extern template class RTT::InputPort< Eigen::Matrix2d >;
     extern template class RTT::InputPort< Eigen::Matrix3d >;
     extern template class RTT::InputPort< Eigen::Matrix4d >;
+    extern template class RTT::InputPort< Eigen::Matrix6d >;
 #endif
 #ifdef ORO_PROPERTY_HPP
     extern template class RTT::Property< Eigen::MatrixXd >;
     extern template class RTT::Property< Eigen::Matrix2d >;
     extern template class RTT::Property< Eigen::Matrix3d >;
     extern template class RTT::Property< Eigen::Matrix4d >;
+    extern template class RTT::Property< Eigen::Matrix6d >;
 #endif
 #ifdef ORO_CORELIB_ATTRIBUTE_HPP
 #define DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(MatrixType) \
@@ -146,6 +160,7 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
     DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Matrix2d)
     DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Matrix3d)
     DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Matrix4d)
+    DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Matrix6d)
 #endif
 
 #endif // ifndef EIGEN_TYPEKIT_HPP

From f34be80de2cf4a3954382299d89b801c8ddb8644 Mon Sep 17 00:00:00 2001
From: Antoine Hoarau <antoine@fuzzylogicrobotics.com>
Date: Wed, 3 Jul 2019 11:27:48 +0200
Subject: [PATCH 03/10] rename vector index to vector size

---
 eigen_typekit/eigen_typekit.cpp | 10 +++-------
 1 file changed, 3 insertions(+), 7 deletions(-)

diff --git a/eigen_typekit/eigen_typekit.cpp b/eigen_typekit/eigen_typekit.cpp
index 2f76c83..5ce7c01 100644
--- a/eigen_typekit/eigen_typekit.cpp
+++ b/eigen_typekit/eigen_typekit.cpp
@@ -367,12 +367,12 @@ namespace Eigen{
     };
 
     template<class VectorType>
-    struct vector_index_constructor
+    struct vector_size_constructor
         : public std::unary_function<int,const VectorType&>
     {
         typedef const VectorType& (Signature)( int );
         mutable boost::shared_ptr< VectorType > ptr;
-        vector_index_constructor() :
+        vector_size_constructor() :
             ptr( new VectorType() ){}
         const VectorType& operator()(int size ) const
         {
@@ -430,11 +430,7 @@ namespace Eigen{
 
     bool EigenTypekitPlugin::loadConstructors()
     {
-        RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_constructor<VectorXd>()));
-        RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_index_constructor<Vector2d>()));
-        RTT::types::Types()->type("eigen_vector3")->addConstructor(types::newConstructor(vector_index_constructor<Vector3d>()));
-        RTT::types::Types()->type("eigen_vector4")->addConstructor(types::newConstructor(vector_index_constructor<Vector4d>()));
-        RTT::types::Types()->type("eigen_vector6")->addConstructor(types::newConstructor(vector_index_constructor<Vector6d>()));
+        RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_size_constructor<VectorXd>()));
         RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_value_constructor<VectorXd>()));
         RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_array_constructor<VectorXd>()));
         RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_index_value_constructor<Vector2d>()));

From 3342bd0bd58cead3f63fbc69a6f89b2ec56a7edb Mon Sep 17 00:00:00 2001
From: Antoine Hoarau <antoine@fuzzylogicrobotics.com>
Date: Wed, 3 Jul 2019 11:31:00 +0200
Subject: [PATCH 04/10] remove matrix6

---
 eigen_typekit/eigen_typekit.cpp                            | 3 ---
 .../include/orocos/eigen_typekit/eigen_typekit.hpp         | 7 -------
 2 files changed, 10 deletions(-)

diff --git a/eigen_typekit/eigen_typekit.cpp b/eigen_typekit/eigen_typekit.cpp
index 5ce7c01..ef85c42 100644
--- a/eigen_typekit/eigen_typekit.cpp
+++ b/eigen_typekit/eigen_typekit.cpp
@@ -72,7 +72,6 @@ DECLARE_RTT_MATRIX_EXPORTS( Eigen::MatrixXd )
 DECLARE_RTT_MATRIX_EXPORTS( Eigen::Matrix2d )
 DECLARE_RTT_MATRIX_EXPORTS( Eigen::Matrix3d )
 DECLARE_RTT_MATRIX_EXPORTS( Eigen::Matrix4d )
-DECLARE_RTT_MATRIX_EXPORTS( Eigen::Matrix6d )
 
 #include <Eigen/Core>
 namespace Eigen{
@@ -424,7 +423,6 @@ namespace Eigen{
         RTT::types::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo<Matrix2d>("eigen_matrix2") );
         RTT::types::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo<Matrix3d>("eigen_matrix3") );
         RTT::types::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo<Matrix4d>("eigen_matrix4") );
-        RTT::types::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo<Matrix6d>("eigen_matrix6") );
         return true;
     }
 
@@ -445,7 +443,6 @@ namespace Eigen{
         RTT::types::Types()->type("eigen_matrix2")->addConstructor(types::newConstructor(matrix_i_j_constructor<Matrix2d>()));
         RTT::types::Types()->type("eigen_matrix3")->addConstructor(types::newConstructor(matrix_i_j_constructor<Matrix3d>()));
         RTT::types::Types()->type("eigen_matrix4")->addConstructor(types::newConstructor(matrix_i_j_constructor<Matrix4d>()));
-        RTT::types::Types()->type("eigen_matrix6")->addConstructor(types::newConstructor(matrix_i_j_constructor<Matrix6d>()));
         return true;
     }
 
diff --git a/eigen_typekit/include/orocos/eigen_typekit/eigen_typekit.hpp b/eigen_typekit/include/orocos/eigen_typekit/eigen_typekit.hpp
index a692d91..7ffe046 100644
--- a/eigen_typekit/include/orocos/eigen_typekit/eigen_typekit.hpp
+++ b/eigen_typekit/include/orocos/eigen_typekit/eigen_typekit.hpp
@@ -29,7 +29,6 @@
 namespace Eigen {
 
 typedef Matrix<double,6,1> Vector6d;
-typedef Matrix<double,6,6> Matrix6d;
 
 class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
 public:
@@ -116,7 +115,6 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
     DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix2d)
     DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix3d)
     DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix4d)
-    DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix6d)
 #endif
 #ifdef ORO_CORELIB_DATASOURCES_HPP
 #define DECLARE_MATRIX_ORO_CORELIB_DATASOURCES_HPP(MatrixType) \
@@ -128,28 +126,24 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
     DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix2d)
     DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix3d)
     DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix4d)
-    DECLARE_MATRIX_CORELIB_DATASOURCE_HPP(Eigen::Matrix6d)
 #endif
 #ifdef ORO_OUTPUT_PORT_HPP
     extern template class RTT::OutputPort< Eigen::MatrixXd >;
     extern template class RTT::OutputPort< Eigen::Matrix2d >;
     extern template class RTT::OutputPort< Eigen::Matrix3d >;
     extern template class RTT::OutputPort< Eigen::Matrix4d >;
-    extern template class RTT::OutputPort< Eigen::Matrix6d >;
 #endif
 #ifdef ORO_INPUT_PORT_HPP
     extern template class RTT::InputPort< Eigen::MatrixXd >;
     extern template class RTT::InputPort< Eigen::Matrix2d >;
     extern template class RTT::InputPort< Eigen::Matrix3d >;
     extern template class RTT::InputPort< Eigen::Matrix4d >;
-    extern template class RTT::InputPort< Eigen::Matrix6d >;
 #endif
 #ifdef ORO_PROPERTY_HPP
     extern template class RTT::Property< Eigen::MatrixXd >;
     extern template class RTT::Property< Eigen::Matrix2d >;
     extern template class RTT::Property< Eigen::Matrix3d >;
     extern template class RTT::Property< Eigen::Matrix4d >;
-    extern template class RTT::Property< Eigen::Matrix6d >;
 #endif
 #ifdef ORO_CORELIB_ATTRIBUTE_HPP
 #define DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(MatrixType) \
@@ -160,7 +154,6 @@ class EigenTypekitPlugin: public RTT::types::TypekitPlugin {
     DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Matrix2d)
     DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Matrix3d)
     DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Matrix4d)
-    DECLARE_MATRIX_ORO_CORELIB_ATTRIBUTE_HPP(Eigen::Matrix6d)
 #endif
 
 #endif // ifndef EIGEN_TYPEKIT_HPP

From 7fe79e427e62dee55aab360e04354fb99381f967 Mon Sep 17 00:00:00 2001
From: Antoine Hoarau <antoine@fuzzylogicrobotics.com>
Date: Wed, 3 Jul 2019 13:34:35 +0200
Subject: [PATCH 05/10] fix fixed sized constructors

---
 eigen_typekit/eigen_typekit.cpp | 93 ++++++++++++++++++++++++---------
 1 file changed, 68 insertions(+), 25 deletions(-)

diff --git a/eigen_typekit/eigen_typekit.cpp b/eigen_typekit/eigen_typekit.cpp
index ef85c42..60223b3 100644
--- a/eigen_typekit/eigen_typekit.cpp
+++ b/eigen_typekit/eigen_typekit.cpp
@@ -138,7 +138,7 @@ namespace Eigen{
             if (arg->isAssignable()) {
 	        typedef typename RTT::internal::AssignableDataSource<VectorType >::shared_ptr sh_ptr;
             sh_ptr asarg = RTT::internal::AssignableDataSource<VectorType >::narrow( arg.get() );
-                asarg->set().resize( size );
+                asarg->set().conservativeResizeLike(VectorType::Zero(size));
                 asarg->updated();
                 return true;
             }
@@ -224,7 +224,7 @@ namespace Eigen{
 
             if ( bag.getType() == this->getTypeName() ) {
                 int dimension = bag.size();
-                result.resize( dimension );
+                result.conservativeResizeLike(VectorType::Zero(dimension));
 
                 // Get values
                 for (int i = 0; i < dimension ; i++) {
@@ -335,36 +335,79 @@ namespace Eigen{
     };
 
     template<class VectorType>
-    struct vector_index_value_constructor
+    struct vector_size_value_constructor
         : public std::binary_function<int,double,const VectorType&>
     {
         typedef const VectorType& (Signature)( int, double );
         mutable boost::shared_ptr< VectorType > ptr;
-        vector_index_value_constructor() :
+        vector_size_value_constructor() :
             ptr( new VectorType ){}
         const VectorType& operator()(int size,double value ) const
         {
-            ptr->resize(size);
+            ptr->conservativeResizeLike(VectorType::Zero(size));
             (*ptr)=VectorType::Constant(size,value);
             return *(ptr);
         }
     };
 
     template<class VectorType>
-    struct vector_index_array_constructor
-        : public std::unary_function<std::vector<double >,const VectorType&>
+    struct vector_fixed_value_constructor
+        : public std::unary_function<double,const VectorType&>
     {
-        typedef const VectorType& (Signature)( std::vector<double > );
+        typedef const VectorType& (Signature)( double );
         mutable boost::shared_ptr< VectorType > ptr;
-        vector_index_array_constructor() :
+        vector_fixed_value_constructor() :
             ptr( new VectorType ){}
-        const VectorType& operator()(std::vector<double > values) const
+        const VectorType& operator()(double value) const
         {
+            ptr->setConstant(value);
+            return *(ptr);
+        }
+    };
+
+    template<class VectorType>
+    struct vector_array_constructor
+        : public std::unary_function<std::vector<double>,const VectorType&>
+    {
+        typedef const VectorType& (Signature)( std::vector<double> );
+        mutable boost::shared_ptr< VectorType > ptr;
+        vector_array_constructor() :
+            ptr( new VectorType ){}
+        const VectorType& operator()(std::vector<double> values) const
+        {
+            // Explicitely resize rather than use aliasing
+            if(ptr->size() != values.size() && VectorType::RowsAtCompileTime == Eigen::Dynamic)
+            {
+                ptr->conservativeResize(values.size());
+            }
+
             (*ptr)=VectorType::Map(&values[0],values.size());
             return *(ptr);
         }
     };
 
+    template<class VectorType>
+    struct vector_fixed_array_constructor
+        : public std::unary_function<std::vector<double>,const VectorType&>
+    {
+        typedef const VectorType& (Signature)( std::vector<double> );
+        mutable boost::shared_ptr< VectorType > ptr;
+        vector_fixed_array_constructor() :
+            ptr( new VectorType ){}
+        const VectorType& operator()(std::vector<double> values) const
+        {
+            if(ptr->size() != values.size())
+            {
+                log(Error) << "Cannot copy an std vector of size " << values.size()
+                           << " into a fixed size vector of size " << ptr->size()
+                           << endlog();
+                return *(ptr);
+            }
+            (*ptr)=VectorType::Map(&values[0],ptr->size());
+            return *(ptr);
+        }
+    };
+
     template<class VectorType>
     struct vector_size_constructor
         : public std::unary_function<int,const VectorType&>
@@ -375,7 +418,7 @@ namespace Eigen{
             ptr( new VectorType() ){}
         const VectorType& operator()(int size ) const
         {
-            ptr->resize(size);
+            ptr->conservativeResizeLike(VectorType::Zero(size));
             return *(ptr);
         }
     };
@@ -402,11 +445,11 @@ namespace Eigen{
         const MatrixType& operator()(int size1,int size2) const
         {
             ptr->resize(size1,size2);
+            ptr->setZero();
             return *(ptr);
         }
     };
 
-
     std::string EigenTypekitPlugin::getName()
     {
         return "Eigen";
@@ -429,20 +472,20 @@ namespace Eigen{
     bool EigenTypekitPlugin::loadConstructors()
     {
         RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_size_constructor<VectorXd>()));
-        RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_value_constructor<VectorXd>()));
-        RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_array_constructor<VectorXd>()));
-        RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_index_value_constructor<Vector2d>()));
-        RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_index_array_constructor<Vector2d>()));
-        RTT::types::Types()->type("eigen_vector3")->addConstructor(types::newConstructor(vector_index_value_constructor<Vector3d>()));
-        RTT::types::Types()->type("eigen_vector3")->addConstructor(types::newConstructor(vector_index_array_constructor<Vector3d>()));
-        RTT::types::Types()->type("eigen_vector4")->addConstructor(types::newConstructor(vector_index_value_constructor<Vector4d>()));
-        RTT::types::Types()->type("eigen_vector4")->addConstructor(types::newConstructor(vector_index_array_constructor<Vector4d>()));
-        RTT::types::Types()->type("eigen_vector6")->addConstructor(types::newConstructor(vector_index_value_constructor<Vector6d>()));
-        RTT::types::Types()->type("eigen_vector6")->addConstructor(types::newConstructor(vector_index_array_constructor<Vector6d>()));
+        RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_size_value_constructor<VectorXd>()));
+        
+        RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_fixed_value_constructor<Vector2d>()));
+        RTT::types::Types()->type("eigen_vector3")->addConstructor(types::newConstructor(vector_fixed_value_constructor<Vector3d>()));
+        RTT::types::Types()->type("eigen_vector4")->addConstructor(types::newConstructor(vector_fixed_value_constructor<Vector4d>()));
+        RTT::types::Types()->type("eigen_vector6")->addConstructor(types::newConstructor(vector_fixed_value_constructor<Vector6d>()));
+        
+        RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_array_constructor<VectorXd>(),true));
+        RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_fixed_array_constructor<Vector2d>(),true));
+        RTT::types::Types()->type("eigen_vector3")->addConstructor(types::newConstructor(vector_fixed_array_constructor<Vector3d>(),true));
+        RTT::types::Types()->type("eigen_vector4")->addConstructor(types::newConstructor(vector_fixed_array_constructor<Vector4d>(),true));
+        RTT::types::Types()->type("eigen_vector6")->addConstructor(types::newConstructor(vector_fixed_array_constructor<Vector6d>(),true));
+        
         RTT::types::Types()->type("eigen_matrix")->addConstructor(types::newConstructor(matrix_i_j_constructor<MatrixXd>()));
-        RTT::types::Types()->type("eigen_matrix2")->addConstructor(types::newConstructor(matrix_i_j_constructor<Matrix2d>()));
-        RTT::types::Types()->type("eigen_matrix3")->addConstructor(types::newConstructor(matrix_i_j_constructor<Matrix3d>()));
-        RTT::types::Types()->type("eigen_matrix4")->addConstructor(types::newConstructor(matrix_i_j_constructor<Matrix4d>()));
         return true;
     }
 

From dfaa3ec5536d0d8d68863c02b20226e59685ff8c Mon Sep 17 00:00:00 2001
From: Antoine Hoarau <antoine@fuzzylogicrobotics.com>
Date: Fri, 5 Jul 2019 11:02:28 +0200
Subject: [PATCH 06/10] taking care of @meyerj comments

---
 eigen_typekit/eigen_typekit.cpp | 30 +++++++++++++++---------------
 1 file changed, 15 insertions(+), 15 deletions(-)

diff --git a/eigen_typekit/eigen_typekit.cpp b/eigen_typekit/eigen_typekit.cpp
index 60223b3..f4ede1e 100644
--- a/eigen_typekit/eigen_typekit.cpp
+++ b/eigen_typekit/eigen_typekit.cpp
@@ -345,8 +345,8 @@ namespace Eigen{
         const VectorType& operator()(int size,double value ) const
         {
             ptr->conservativeResizeLike(VectorType::Zero(size));
-            (*ptr)=VectorType::Constant(size,value);
-            return *(ptr);
+            *ptr=VectorType::Constant(size,value);
+            return *ptr;
         }
     };
 
@@ -375,14 +375,14 @@ namespace Eigen{
             ptr( new VectorType ){}
         const VectorType& operator()(std::vector<double> values) const
         {
-            // Explicitely resize rather than use aliasing
+            // Explicitely resize rather than use aliasing implicitely
             if(ptr->size() != values.size() && VectorType::RowsAtCompileTime == Eigen::Dynamic)
             {
-                ptr->conservativeResize(values.size());
+                ptr->resize(values.size());
             }
 
-            (*ptr)=VectorType::Map(&values[0],values.size());
-            return *(ptr);
+            *ptr = VectorType::Map(values.data(),values.size());
+            return *ptr;
         }
     };
 
@@ -398,13 +398,14 @@ namespace Eigen{
         {
             if(ptr->size() != values.size())
             {
-                log(Error) << "Cannot copy an std vector of size " << values.size()
+                log(Debug) << "Cannot copy an std vector of size " << values.size()
                            << " into a fixed size vector of size " << ptr->size()
                            << endlog();
-                return *(ptr);
+                *ptr = VectorType::Constant(values.size(), RTT::internal::NA<double&>::na());
+                return *ptr;
             }
-            (*ptr)=VectorType::Map(&values[0],ptr->size());
-            return *(ptr);
+            *ptr = VectorType::Map(values.data(),ptr->size());
+            return *ptr;
         }
     };
 
@@ -418,8 +419,8 @@ namespace Eigen{
             ptr( new VectorType() ){}
         const VectorType& operator()(int size ) const
         {
-            ptr->conservativeResizeLike(VectorType::Zero(size));
-            return *(ptr);
+            *ptr = VectorType::Zero(size);
+            return *ptr;
         }
     };
 
@@ -444,9 +445,8 @@ namespace Eigen{
             ptr( new MatrixType() ){}
         const MatrixType& operator()(int size1,int size2) const
         {
-            ptr->resize(size1,size2);
-            ptr->setZero();
-            return *(ptr);
+            *ptr = MatrixType::Zero(size1,size2);
+            return *ptr;
         }
     };
 

From d416a43ed445a2eb031c4e42f89a1a0cbef9372b Mon Sep 17 00:00:00 2001
From: Antoine Hoarau <antoine@fuzzylogicrobotics.com>
Date: Fri, 5 Jul 2019 11:04:01 +0200
Subject: [PATCH 07/10] remove unecessary function

---
 eigen_typekit/eigen_typekit.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/eigen_typekit/eigen_typekit.cpp b/eigen_typekit/eigen_typekit.cpp
index f4ede1e..5bd4ddd 100644
--- a/eigen_typekit/eigen_typekit.cpp
+++ b/eigen_typekit/eigen_typekit.cpp
@@ -344,7 +344,6 @@ namespace Eigen{
             ptr( new VectorType ){}
         const VectorType& operator()(int size,double value ) const
         {
-            ptr->conservativeResizeLike(VectorType::Zero(size));
             *ptr=VectorType::Constant(size,value);
             return *ptr;
         }

From b8e97bc5203075b39890645a83ce385394d5eacb Mon Sep 17 00:00:00 2001
From: Antoine Hoarau <antoine@fuzzylogicrobotics.com>
Date: Fri, 5 Jul 2019 11:14:05 +0200
Subject: [PATCH 08/10] addressing #34
 https://github.com/orocos/rtt_geometry/issues/34

---
 eigen_typekit/eigen_typekit.cpp | 56 +++++++++++++++------------------
 1 file changed, 25 insertions(+), 31 deletions(-)

diff --git a/eigen_typekit/eigen_typekit.cpp b/eigen_typekit/eigen_typekit.cpp
index 5bd4ddd..003f1bb 100644
--- a/eigen_typekit/eigen_typekit.cpp
+++ b/eigen_typekit/eigen_typekit.cpp
@@ -336,43 +336,42 @@ namespace Eigen{
 
     template<class VectorType>
     struct vector_size_value_constructor
-        : public std::binary_function<int,double,const VectorType&>
+        : public std::binary_function<int,double,VectorType>
     {
-        typedef const VectorType& (Signature)( int, double );
+        typedef VectorType (Signature)( int, double );
         mutable boost::shared_ptr< VectorType > ptr;
         vector_size_value_constructor() :
             ptr( new VectorType ){}
-        const VectorType& operator()(int size,double value ) const
+        VectorType operator()(int size,double value ) const
         {
-            *ptr=VectorType::Constant(size,value);
-            return *ptr;
+            return VectorType::Constant(size,value);
         }
     };
 
     template<class VectorType>
     struct vector_fixed_value_constructor
-        : public std::unary_function<double,const VectorType&>
+        : public std::unary_function<double,VectorType>
     {
-        typedef const VectorType& (Signature)( double );
+        typedef VectorType (Signature)( double );
         mutable boost::shared_ptr< VectorType > ptr;
         vector_fixed_value_constructor() :
             ptr( new VectorType ){}
-        const VectorType& operator()(double value) const
+        VectorType operator()(double value) const
         {
             ptr->setConstant(value);
-            return *(ptr);
+            return *ptr;
         }
     };
 
     template<class VectorType>
     struct vector_array_constructor
-        : public std::unary_function<std::vector<double>,const VectorType&>
+        : public std::unary_function<std::vector<double>,VectorType>
     {
         typedef const VectorType& (Signature)( std::vector<double> );
         mutable boost::shared_ptr< VectorType > ptr;
         vector_array_constructor() :
             ptr( new VectorType ){}
-        const VectorType& operator()(std::vector<double> values) const
+        VectorType operator()(std::vector<double> values) const
         {
             // Explicitely resize rather than use aliasing implicitely
             if(ptr->size() != values.size() && VectorType::RowsAtCompileTime == Eigen::Dynamic)
@@ -380,46 +379,42 @@ namespace Eigen{
                 ptr->resize(values.size());
             }
 
-            *ptr = VectorType::Map(values.data(),values.size());
-            return *ptr;
+            return VectorType::Map(values.data(),values.size());
         }
     };
 
     template<class VectorType>
     struct vector_fixed_array_constructor
-        : public std::unary_function<std::vector<double>,const VectorType&>
+        : public std::unary_function<std::vector<double>,VectorType>
     {
-        typedef const VectorType& (Signature)( std::vector<double> );
+        typedef VectorType (Signature)( std::vector<double> );
         mutable boost::shared_ptr< VectorType > ptr;
         vector_fixed_array_constructor() :
             ptr( new VectorType ){}
-        const VectorType& operator()(std::vector<double> values) const
+        VectorType operator()(std::vector<double> values) const
         {
             if(ptr->size() != values.size())
             {
                 log(Debug) << "Cannot copy an std vector of size " << values.size()
-                           << " into a fixed size vector of size " << ptr->size()
+                           << " into an eigen vector of (fixed) size " << ptr->size()
                            << endlog();
-                *ptr = VectorType::Constant(values.size(), RTT::internal::NA<double&>::na());
-                return *ptr;
+                return VectorType::Constant(values.size(), RTT::internal::NA<double&>::na());
             }
-            *ptr = VectorType::Map(values.data(),ptr->size());
-            return *ptr;
+            return VectorType::Map(values.data(),ptr->size());
         }
     };
 
     template<class VectorType>
     struct vector_size_constructor
-        : public std::unary_function<int,const VectorType&>
+        : public std::unary_function<int,VectorType>
     {
-        typedef const VectorType& (Signature)( int );
+        typedef VectorType (Signature)( int );
         mutable boost::shared_ptr< VectorType > ptr;
         vector_size_constructor() :
             ptr( new VectorType() ){}
-        const VectorType& operator()(int size ) const
+        VectorType operator()(int size ) const
         {
-            *ptr = VectorType::Zero(size);
-            return *ptr;
+            return VectorType::Zero(size);
         }
     };
 
@@ -436,16 +431,15 @@ namespace Eigen{
 
     template<typename MatrixType>
     struct matrix_i_j_constructor
-        : public std::binary_function<int,int,const MatrixType&>
+        : public std::binary_function<int,int,MatrixType>
     {
-        typedef const MatrixType& (Signature)( int, int );
+        typedef MatrixType (Signature)( int, int );
         mutable boost::shared_ptr< MatrixType > ptr;
         matrix_i_j_constructor() :
             ptr( new MatrixType() ){}
-        const MatrixType& operator()(int size1,int size2) const
+        MatrixType operator()(int size1,int size2) const
         {
-            *ptr = MatrixType::Zero(size1,size2);
-            return *ptr;
+            return MatrixType::Zero(size1,size2);
         }
     };
 

From c073451427df6d2223bf042a604385b43c7a362a Mon Sep 17 00:00:00 2001
From: Antoine Hoarau <antoine@fuzzylogicrobotics.com>
Date: Sat, 6 Jul 2019 01:26:03 +0200
Subject: [PATCH 09/10] fix last commit

---
 eigen_typekit/eigen_typekit.cpp | 11 +++--------
 1 file changed, 3 insertions(+), 8 deletions(-)

diff --git a/eigen_typekit/eigen_typekit.cpp b/eigen_typekit/eigen_typekit.cpp
index 003f1bb..6e1a2a0 100644
--- a/eigen_typekit/eigen_typekit.cpp
+++ b/eigen_typekit/eigen_typekit.cpp
@@ -367,19 +367,14 @@ namespace Eigen{
     struct vector_array_constructor
         : public std::unary_function<std::vector<double>,VectorType>
     {
-        typedef const VectorType& (Signature)( std::vector<double> );
+        typedef VectorType (Signature)( std::vector<double> );
         mutable boost::shared_ptr< VectorType > ptr;
         vector_array_constructor() :
             ptr( new VectorType ){}
         VectorType operator()(std::vector<double> values) const
         {
-            // Explicitely resize rather than use aliasing implicitely
-            if(ptr->size() != values.size() && VectorType::RowsAtCompileTime == Eigen::Dynamic)
-            {
-                ptr->resize(values.size());
-            }
-
-            return VectorType::Map(values.data(),values.size());
+            *ptr = VectorType::Map(values.data(),values.size());
+            return *ptr;
         }
     };
 

From 844baad1c265b92c5664c7efbc9ca1fa2d488e06 Mon Sep 17 00:00:00 2001
From: Antoine Hoarau <antoine@fuzzylogicrobotics.com>
Date: Mon, 22 Jul 2019 21:45:40 +0200
Subject: [PATCH 10/10] addressing #34 v2 - returning temporaries

---
 eigen_typekit/eigen_typekit.cpp | 50 ++++++---------------------------
 1 file changed, 8 insertions(+), 42 deletions(-)

diff --git a/eigen_typekit/eigen_typekit.cpp b/eigen_typekit/eigen_typekit.cpp
index 6e1a2a0..cdd3686 100644
--- a/eigen_typekit/eigen_typekit.cpp
+++ b/eigen_typekit/eigen_typekit.cpp
@@ -339,42 +339,20 @@ namespace Eigen{
         : public std::binary_function<int,double,VectorType>
     {
         typedef VectorType (Signature)( int, double );
-        mutable boost::shared_ptr< VectorType > ptr;
-        vector_size_value_constructor() :
-            ptr( new VectorType ){}
         VectorType operator()(int size,double value ) const
         {
             return VectorType::Constant(size,value);
         }
     };
 
-    template<class VectorType>
-    struct vector_fixed_value_constructor
-        : public std::unary_function<double,VectorType>
-    {
-        typedef VectorType (Signature)( double );
-        mutable boost::shared_ptr< VectorType > ptr;
-        vector_fixed_value_constructor() :
-            ptr( new VectorType ){}
-        VectorType operator()(double value) const
-        {
-            ptr->setConstant(value);
-            return *ptr;
-        }
-    };
-
     template<class VectorType>
     struct vector_array_constructor
         : public std::unary_function<std::vector<double>,VectorType>
     {
         typedef VectorType (Signature)( std::vector<double> );
-        mutable boost::shared_ptr< VectorType > ptr;
-        vector_array_constructor() :
-            ptr( new VectorType ){}
         VectorType operator()(std::vector<double> values) const
         {
-            *ptr = VectorType::Map(values.data(),values.size());
-            return *ptr;
+            return VectorType::Map(values.data(),values.size());
         }
     };
 
@@ -383,19 +361,18 @@ namespace Eigen{
         : public std::unary_function<std::vector<double>,VectorType>
     {
         typedef VectorType (Signature)( std::vector<double> );
-        mutable boost::shared_ptr< VectorType > ptr;
-        vector_fixed_array_constructor() :
-            ptr( new VectorType ){}
         VectorType operator()(std::vector<double> values) const
         {
-            if(ptr->size() != values.size())
+            int size = VectorType::RowsAtCompileTime;
+            if(size != Eigen::Dynamic && size != values.size())
             {
                 log(Debug) << "Cannot copy an std vector of size " << values.size()
-                           << " into an eigen vector of (fixed) size " << ptr->size()
+                           << " into an eigen vector of (fixed) size " << size
                            << endlog();
-                return VectorType::Constant(values.size(), RTT::internal::NA<double&>::na());
+                return VectorType::Constant(size, RTT::internal::NA<double&>::na());
             }
-            return VectorType::Map(values.data(),ptr->size());
+            // NOTE: this can resize the eigen_vector
+            return VectorType::Map(values.data(),values.size());
         }
     };
 
@@ -404,9 +381,6 @@ namespace Eigen{
         : public std::unary_function<int,VectorType>
     {
         typedef VectorType (Signature)( int );
-        mutable boost::shared_ptr< VectorType > ptr;
-        vector_size_constructor() :
-            ptr( new VectorType() ){}
         VectorType operator()(int size ) const
         {
             return VectorType::Zero(size);
@@ -429,9 +403,6 @@ namespace Eigen{
         : public std::binary_function<int,int,MatrixType>
     {
         typedef MatrixType (Signature)( int, int );
-        mutable boost::shared_ptr< MatrixType > ptr;
-        matrix_i_j_constructor() :
-            ptr( new MatrixType() ){}
         MatrixType operator()(int size1,int size2) const
         {
             return MatrixType::Zero(size1,size2);
@@ -461,12 +432,7 @@ namespace Eigen{
     {
         RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_size_constructor<VectorXd>()));
         RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_size_value_constructor<VectorXd>()));
-        
-        RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_fixed_value_constructor<Vector2d>()));
-        RTT::types::Types()->type("eigen_vector3")->addConstructor(types::newConstructor(vector_fixed_value_constructor<Vector3d>()));
-        RTT::types::Types()->type("eigen_vector4")->addConstructor(types::newConstructor(vector_fixed_value_constructor<Vector4d>()));
-        RTT::types::Types()->type("eigen_vector6")->addConstructor(types::newConstructor(vector_fixed_value_constructor<Vector6d>()));
-        
+
         RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_array_constructor<VectorXd>(),true));
         RTT::types::Types()->type("eigen_vector2")->addConstructor(types::newConstructor(vector_fixed_array_constructor<Vector2d>(),true));
         RTT::types::Types()->type("eigen_vector3")->addConstructor(types::newConstructor(vector_fixed_array_constructor<Vector3d>(),true));