00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include "eigen_typekit.hpp"
00021 #include <rtt/Property.hpp>
00022 #include <rtt/PropertyBag.hpp>
00023 #include <rtt/types/TemplateTypeInfo.hpp>
00024 #include <rtt/types/Operators.hpp>
00025 #include <rtt/types/OperatorTypes.hpp>
00026 #include <rtt/types/TemplateConstructor.hpp>
00027 #include <rtt/types/Types.hpp>
00028 #include <rtt/Logger.hpp>
00029 #include <rtt/internal/DataSources.hpp>
00030 #include <rtt/internal/mystd.hpp>
00031 #include <rtt/os/StartStopManager.hpp>
00032 #include <rtt/types/TypekitRepository.hpp>
00033 #include <rtt/internal/FusedFunctorDataSource.hpp>
00034 #include <rtt/internal/DataSourceGenerator.hpp>
00035 #include <boost/lexical_cast.hpp>
00036
00037
00038 template class RTT_EXPORT RTT::internal::DataSourceTypeInfo< Eigen::VectorXd >;
00039 template class RTT_EXPORT RTT::internal::DataSource< Eigen::VectorXd >;
00040 template class RTT_EXPORT RTT::internal::AssignableDataSource< Eigen::VectorXd >;
00041 template class RTT_EXPORT RTT::internal::AssignCommand< Eigen::VectorXd >;
00042 template class RTT_EXPORT RTT::internal::ValueDataSource< Eigen::VectorXd >;
00043 template class RTT_EXPORT RTT::internal::ConstantDataSource< Eigen::VectorXd >;
00044 template class RTT_EXPORT RTT::internal::ReferenceDataSource< Eigen::VectorXd >;
00045 template class RTT_EXPORT RTT::OutputPort< Eigen::VectorXd >;
00046 template class RTT_EXPORT RTT::InputPort< Eigen::VectorXd >;
00047 template class RTT_EXPORT RTT::Property< Eigen::VectorXd >;
00048 template class RTT_EXPORT RTT::Attribute< Eigen::VectorXd >;
00049 template class RTT_EXPORT RTT::Constant< Eigen::VectorXd >;
00050
00051 template class RTT_EXPORT RTT::internal::DataSourceTypeInfo< Eigen::MatrixXd >;
00052 template class RTT_EXPORT RTT::internal::DataSource< Eigen::MatrixXd >;
00053 template class RTT_EXPORT RTT::internal::AssignableDataSource< Eigen::MatrixXd >;
00054 template class RTT_EXPORT RTT::internal::AssignCommand< Eigen::MatrixXd >;
00055 template class RTT_EXPORT RTT::internal::ValueDataSource< Eigen::MatrixXd >;
00056 template class RTT_EXPORT RTT::internal::ConstantDataSource< Eigen::MatrixXd >;
00057 template class RTT_EXPORT RTT::internal::ReferenceDataSource< Eigen::MatrixXd >;
00058 template class RTT_EXPORT RTT::OutputPort< Eigen::MatrixXd >;
00059 template class RTT_EXPORT RTT::InputPort< Eigen::MatrixXd >;
00060 template class RTT_EXPORT RTT::Property< Eigen::MatrixXd >;
00061 template class RTT_EXPORT RTT::Attribute< Eigen::MatrixXd >;
00062 template class RTT_EXPORT RTT::Constant< Eigen::MatrixXd >;
00063
00064
00065 #include <Eigen/Core>
00066 namespace Eigen{
00067
00068 using namespace RTT;
00069 using namespace RTT::detail;
00070 using namespace RTT::types;
00071
00072 std::istream& operator>>(std::istream &is,MatrixXd& v){
00073 return is;
00074 }
00075 std::istream& operator>>(std::istream &is,VectorXd& v){
00076 return is;
00077 }
00078
00079 double& get_item(VectorXd& v, int index)
00080 {
00081 if (index >= (int) (v.size()) || index < 0)
00082 return RTT::internal::NA<double&>::na();
00083 return v[index];
00084 }
00085
00086 double get_item_copy(const VectorXd& v, int index)
00087 {
00088 if (index >= (int) (v.size()) || index < 0)
00089 return RTT::internal::NA<double>::na();
00090 return v[index];
00091 }
00092
00093
00094 int get_size(const VectorXd& v)
00095 {
00096 return v.size();
00097 }
00098
00099 struct VectorTypeInfo : public types::TemplateTypeInfo<VectorXd,true>
00100 #if (RTT_VERSION_MAJOR*100+RTT_VERSION_MINOR) >= 206
00101 , public MemberFactory
00102 #endif
00103 {
00104 VectorTypeInfo():TemplateTypeInfo<VectorXd, true >("eigen_vector")
00105 {
00106 };
00107
00108 #if (RTT_VERSION_MAJOR*100+RTT_VERSION_MINOR) >= 206
00109 bool installTypeInfoObject(TypeInfo* ti) {
00110
00111 boost::shared_ptr< VectorTypeInfo > mthis = boost::dynamic_pointer_cast<VectorTypeInfo >( this->getSharedPtr() );
00112 assert(mthis);
00113
00114 TemplateTypeInfo<VectorXd,true>::installTypeInfoObject(ti);
00115
00116 ti->setMemberFactory( mthis );
00117
00118 return false;
00119 }
00120 #endif
00121
00122 bool resize(base::DataSourceBase::shared_ptr arg, int size) const
00123 {
00124 if (arg->isAssignable()) {
00125 RTT::internal::AssignableDataSource<VectorXd>::shared_ptr asarg = RTT::internal::AssignableDataSource<VectorXd>::narrow( arg.get() );
00126 asarg->set().resize( size );
00127 asarg->updated();
00128 return true;
00129 }
00130 return false;
00131 }
00132
00133 virtual std::vector<std::string> getMemberNames() const {
00134
00135 std::vector<std::string> result;
00136 result.push_back("size");
00137 result.push_back("capacity");
00138 return result;
00139 }
00140
00141 base::DataSourceBase::shared_ptr getMember(base::DataSourceBase::shared_ptr item, const std::string& name) const {
00142
00143 try {
00144 unsigned int indx = boost::lexical_cast<unsigned int>(name);
00145
00146 return getMember( item, new RTT::internal::ConstantDataSource<int>(indx));
00147 } catch(...) {}
00148
00149 return getMember( item, new RTT::internal::ConstantDataSource<std::string>(name) );
00150 }
00151
00152 base::DataSourceBase::shared_ptr getMember(base::DataSourceBase::shared_ptr item,
00153 base::DataSourceBase::shared_ptr id) const {
00154
00155 RTT::internal::DataSource<int>::shared_ptr id_indx = RTT::internal::DataSource<int>::narrow( RTT::internal::DataSourceTypeInfo<int>::getTypeInfo()->convert(id).get() );
00156 RTT::internal::DataSource<std::string>::shared_ptr id_name = RTT::internal::DataSource<std::string>::narrow( id.get() );
00157 if ( id_name ) {
00158 if ( id_name->get() == "size" || id_name->get() == "capacity") {
00159 try {
00160 return RTT::internal::newFunctorDataSource(&get_size, RTT::internal::GenerateDataSource()(item.get()) );
00161 } catch(...) {}
00162 }
00163 }
00164
00165 if ( id_indx ) {
00166 try {
00167 if ( item->isAssignable() )
00168 return RTT::internal::newFunctorDataSource(&get_item, RTT::internal::GenerateDataSource()(item.get(), id_indx.get() ) );
00169 else
00170 return RTT::internal::newFunctorDataSource(&get_item_copy, RTT::internal::GenerateDataSource()(item.get(), id_indx.get() ) );
00171 } catch(...) {}
00172 }
00173 if (id_name) {
00174 log(Error) << "EigenVectorTypeInfo: No such member : " << id_name->get() << endlog();
00175 }
00176 if (id_indx) {
00177 log(Error) << "EigenVectorTypeInfo: Invalid index : " << id_indx->get() <<":"<< id_indx->getTypeName() << endlog();
00178 }
00179 if ( !id_name && ! id_indx)
00180 log(Error) << "EigenVectorTypeInfo: Not a member or index : " << id <<":"<< id->getTypeName() << endlog();
00181 return base::DataSourceBase::shared_ptr();
00182 }
00183
00184
00185 virtual bool decomposeTypeImpl(const VectorXd& vec, PropertyBag& targetbag) const
00186 {
00187 targetbag.setType("eigen_vector");
00188 int dimension = vec.rows();
00189 std::string str;
00190
00191 if(!targetbag.empty())
00192 return false;
00193
00194 for ( int i=0; i < dimension ; i++){
00195 std::stringstream out;
00196 out << i+1;
00197 str = out.str();
00198 targetbag.add( new Property<double>(str, str +"th element of vector",vec(i)) );
00199 }
00200
00201 return true;
00202 };
00203
00204 virtual bool composeTypeImpl(const PropertyBag& bag, VectorXd& result) const{
00205
00206 if ( bag.getType() == "eigen_vector" ) {
00207 int dimension = bag.size();
00208 result.resize( dimension );
00209
00210
00211 for (int i = 0; i < dimension ; i++) {
00212 std::stringstream out;
00213 out << i+1;
00214 Property<double> elem = bag.getProperty(out.str());
00215 if(elem.ready())
00216 result(i) = elem.get();
00217 else{
00218 log(Error)<<"Could not read element "<<i+1<<endlog();
00219 return false;
00220 }
00221 }
00222 }else{
00223 log(Error) << "Composing Property< VectorXd > :"
00224 << " type mismatch, got type '"<< bag.getType()
00225 << "', expected type "<<"eigen_vector."<<endlog();
00226 return false;
00227 }
00228 return true;
00229 };
00230 };
00231
00232 struct MatrixTypeInfo : public types::TemplateTypeInfo<MatrixXd,true>{
00233 MatrixTypeInfo():TemplateTypeInfo<MatrixXd, true >("eigen_matrix"){
00234 };
00235
00236
00237 bool decomposeTypeImpl(const MatrixXd& mat, PropertyBag& targetbag) const{
00238 targetbag.setType("eigen_matrix");
00239 unsigned int dimension = mat.rows();
00240 if(!targetbag.empty())
00241 return false;
00242
00243 for ( unsigned int i=0; i < dimension ; i++){
00244 std::stringstream out;
00245 out << i+1;
00246 targetbag.add( new Property<VectorXd >(out.str(), out.str() +"th row of matrix",mat.row(i) ));
00247 }
00248
00249 return true;
00250 };
00251
00252 bool composeTypeImpl(const PropertyBag& bag, MatrixXd& result) const{
00253 if ( bag.getType() == "eigen_matrix" ) {
00254 unsigned int rows = bag.size();
00255 unsigned int cols = 0;
00256
00257 for (unsigned int i = 0; i < rows ; i++) {
00258 std::stringstream out;
00259 out << i+1;
00260 Property<PropertyBag> row_bag = bag.getProperty(out.str());
00261 if(!row_bag.ready()){
00262 log(Error)<<"Could not read row "<<i+1<<endlog();
00263 return false;
00264 }
00265 Property<VectorXd > row_p(row_bag.getName(),row_bag.getDescription());
00266 if(!(row_p.compose(row_bag))){
00267 log(Error)<<"Could not compose row "<<i+1<<endlog();
00268 return false;
00269 }
00270 if(row_p.ready()){
00271 if(i==0){
00272 cols = row_p.get().size();
00273 result.resize(rows,cols);
00274 } else
00275 if(row_p.get().rows()!=(int)cols){
00276 log(Error)<<"Row "<<i+1<<" size does not match matrix columns"<<endlog();
00277 return false;
00278 }
00279 result.row(i)=row_p.get();
00280 }else{
00281 log(Error)<<"Property of Row "<<i+1<<"was not ready for use"<<endlog();
00282 return false;
00283 }
00284 }
00285 }else {
00286 log(Error) << "Composing Property< MatrixXd > :"
00287 << " type mismatch, got type '"<< bag.getType()
00288 << "', expected type "<<"ublas_matrix."<<endlog();
00289 return false;
00290 }
00291 return true;
00292 };
00293 };
00294
00295 struct vector_index
00296 : public std::binary_function<const VectorXd&, int, double>
00297 {
00298 double operator()(const VectorXd& v, int index) const
00299 {
00300 if ( index >= (int)(v.size()) || index < 0)
00301 return 0.0;
00302 return v(index);
00303 }
00304 };
00305
00306 struct get_size
00307 : public std::unary_function<const VectorXd&, int>
00308 {
00309 int operator()(const VectorXd& cont ) const
00310 {
00311 return cont.rows();
00312 }
00313 };
00314
00315 struct vector_index_value_constructor
00316 : public std::binary_function<int,double,const VectorXd&>
00317 {
00318 typedef const VectorXd& (Signature)( int, double );
00319 mutable boost::shared_ptr< VectorXd > ptr;
00320 vector_index_value_constructor() :
00321 ptr( new VectorXd ){}
00322 const VectorXd& operator()(int size,double value ) const
00323 {
00324 ptr->resize(size);
00325 (*ptr)=Eigen::VectorXd::Constant(size,value);
00326 return *(ptr);
00327 }
00328 };
00329
00330 struct vector_index_array_constructor
00331 : public std::unary_function<std::vector<double >,const VectorXd&>
00332 {
00333 typedef const VectorXd& (Signature)( std::vector<double > );
00334 mutable boost::shared_ptr< VectorXd > ptr;
00335 vector_index_array_constructor() :
00336 ptr( new VectorXd ){}
00337 const VectorXd& operator()(std::vector<double > values) const
00338 {
00339 (*ptr)=VectorXd::Map(&values[0],values.size());
00340 return *(ptr);
00341 }
00342 };
00343
00344 struct vector_index_constructor
00345 : public std::unary_function<int,const VectorXd&>
00346 {
00347 typedef const VectorXd& (Signature)( int );
00348 mutable boost::shared_ptr< VectorXd > ptr;
00349 vector_index_constructor() :
00350 ptr( new VectorXd() ){}
00351 const VectorXd& operator()(int size ) const
00352 {
00353 ptr->resize(size);
00354 return *(ptr);
00355 }
00356 };
00357
00358 struct matrix_index
00359 : public std::ternary_function<const MatrixXd&, int, int, double>
00360 {
00361 double operator()(const MatrixXd& m, int i, int j) const{
00362 if ( i >= (int)(m.rows()) || i < 0 || j<0 || j>= (int)(m.cols()))
00363 return 0.0;
00364 return m(i,j);
00365 }
00366 };
00367
00368 struct matrix_i_j_constructor
00369 : public std::binary_function<int,int,const MatrixXd&>
00370 {
00371 typedef const MatrixXd& (Signature)( int, int );
00372 mutable boost::shared_ptr< MatrixXd > ptr;
00373 matrix_i_j_constructor() :
00374 ptr( new MatrixXd() ){}
00375 const MatrixXd& operator()(int size1,int size2) const
00376 {
00377 ptr->resize(size1,size2);
00378 return *(ptr);
00379 }
00380 };
00381
00382
00383 std::string EigenTypekitPlugin::getName()
00384 {
00385 return "Eigen";
00386 }
00387
00388 bool EigenTypekitPlugin::loadTypes()
00389 {
00390 RTT::types::TypeInfoRepository::Instance()->addType( new VectorTypeInfo() );
00391 RTT::types::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo() );
00392 return true;
00393 }
00394
00395 bool EigenTypekitPlugin::loadConstructors()
00396 {
00397 RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_constructor()));
00398 RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_value_constructor()));
00399 RTT::types::Types()->type("eigen_vector")->addConstructor(types::newConstructor(vector_index_array_constructor()));
00400 RTT::types::Types()->type("eigen_matrix")->addConstructor(types::newConstructor(matrix_i_j_constructor()));
00401 return true;
00402 }
00403
00404 bool EigenTypekitPlugin::loadOperators()
00405 {
00406 RTT::types::OperatorRepository::Instance()->add( newBinaryOperator( "[]", vector_index() ) );
00407
00408
00409 return true;
00410 }
00411 }
00412
00413 ORO_TYPEKIT_PLUGIN(Eigen::EigenTypekitPlugin)