diff options
author | Benoit Bolsee <benoit.bolsee@online.be> | 2009-09-25 01:22:24 +0400 |
---|---|---|
committer | Benoit Bolsee <benoit.bolsee@online.be> | 2009-09-25 01:22:24 +0400 |
commit | 1483fafd1372a3d3e025d08634e798adb7da512f (patch) | |
tree | 9191765749e29866339f4c31d892603f5f8b334d /intern/itasc/eigen_types.hpp | |
parent | c995c605f640d8d688e6e58e0fe247ca83f91696 (diff) | |
parent | 222fe6b1a5d49f67177cbb762f55a0e482145f5d (diff) |
Merge of itasc branch. Project files, scons and cmake should be working. Makefile updated but not tested. Comes with Eigen2 2.0.6 C++ matrix library.
Diffstat (limited to 'intern/itasc/eigen_types.hpp')
-rw-r--r-- | intern/itasc/eigen_types.hpp | 84 |
1 files changed, 84 insertions, 0 deletions
diff --git a/intern/itasc/eigen_types.hpp b/intern/itasc/eigen_types.hpp new file mode 100644 index 00000000000..fe46f8b6bb3 --- /dev/null +++ b/intern/itasc/eigen_types.hpp @@ -0,0 +1,84 @@ +/* $Id: eigen_types.hpp 19905 2009-04-23 13:29:54Z ben2610 $ + * eigen_types.hpp + * + * Created on: March 6, 2009 + * Author: benoit bolsee + */ + +#ifndef EIGEN_TYPES_HPP_ +#define EIGEN_TYPES_HPP_ + +#include <Eigen/Core> +#include "kdl/frames.hpp" +#include "kdl/tree.hpp" +#include "kdl/chain.hpp" +#include "kdl/jacobian.hpp" +#include "kdl/jntarray.hpp" + + +namespace iTaSC{ + +using KDL::Twist; +using KDL::Frame; +using KDL::Joint; +using KDL::Inertia; +using KDL::SegmentMap; +using KDL::Tree; +using KDL::JntArray; +using KDL::Jacobian; +using KDL::Segment; +using KDL::Rotation; +using KDL::Vector; +using KDL::Vector2; +using KDL::Chain; + +extern const Frame F_identity; + +#define e_scalar double +#define e_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1> +#define e_zero_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Zero +#define e_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic> +#define e_matrix6 Eigen::Matrix<e_scalar,6,6> +#define e_identity_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Identity +#define e_scalar_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Constant +#define e_zero_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero +#define e_random_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Random +#define e_vector6 Eigen::Matrix<e_scalar,6,1> +#define e_vector3 Eigen::Matrix<e_scalar,3,1> + +class Range { +public: + int start; + int count; + Range(int _start, int _count) { start = _start; count=_count; } + Range(const Range& other) { start=other.start; count=other.count; } +}; + +template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r) +{ + return Eigen::Block<MatrixType>(m,r.start,0,r.count,1); +} + +template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r, Range c) +{ + return Eigen::Block<MatrixType>(m,r.start,c.start,r.count,c.count); +} + +template<typename Derived> inline static int changeBase(Eigen::MatrixBase<Derived>& J, const Frame& T) { + + if (J.rows() != 6) + return -1; + for (int j = 0; j < J.cols(); ++j) { + typename Derived::ColXpr Jj = J.col(j); + Twist arg; + for(unsigned int i=0;i<6;++i) + arg(i)=Jj[i]; + Twist tmp(T*arg); + for(unsigned int i=0;i<6;++i) + Jj[i]=e_scalar(tmp(i)); + } + return 0; +} + +} +#endif /* UBLAS_TYPES_HPP_ */ |