1 /*
2 * eigen_types.hpp
3 *
4 * Created on: March 6, 2009
5 * Author: benoit bolsee
6 */
7
8 #ifndef EIGEN_TYPES_HPP_
9 #define EIGEN_TYPES_HPP_
10
11 #include <Eigen/Core>
12 #include "kdl/frames.hpp"
13 #include "kdl/tree.hpp"
14 #include "kdl/chain.hpp"
15 #include "kdl/jacobian.hpp"
16 #include "kdl/jntarray.hpp"
17
18
19 namespace iTaSC{
20
21 using KDL::Twist;
22 using KDL::Frame;
23 using KDL::Joint;
24 using KDL::Inertia;
25 using KDL::SegmentMap;
26 using KDL::Tree;
27 using KDL::JntArray;
28 using KDL::Jacobian;
29 using KDL::Segment;
30 using KDL::Rotation;
31 using KDL::Vector;
32 using KDL::Vector2;
33 using KDL::Chain;
34
35 extern const Frame F_identity;
36
37 #define e_scalar double
38 #define e_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>
39 #define e_zero_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Zero
40 #define e_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>
41 #define e_matrix6 Eigen::Matrix<e_scalar,6,6>
42 #define e_identity_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Identity
43 #define e_scalar_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Constant
44 #define e_zero_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero
45 #define e_random_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Random
46 #define e_vector6 Eigen::Matrix<e_scalar,6,1>
47 #define e_vector3 Eigen::Matrix<e_scalar,3,1>
48
49 class Range {
50 public:
51 int start;
52 int count;
Range(int _start,int _count)53 Range(int _start, int _count) { start = _start; count=_count; }
Range(const Range & other)54 Range(const Range& other) { start=other.start; count=other.count; }
55 };
56
project(MatrixType & m,Range r)57 template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r)
58 {
59 return Eigen::Block<MatrixType>(m,r.start,0,r.count,1);
60 }
61
project(MatrixType & m,Range r,Range c)62 template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r, Range c)
63 {
64 return Eigen::Block<MatrixType>(m,r.start,c.start,r.count,c.count);
65 }
66
changeBase(Eigen::MatrixBase<Derived> & J,const Frame & T)67 template<typename Derived> inline static int changeBase(Eigen::MatrixBase<Derived>& J, const Frame& T) {
68
69 if (J.rows() != 6)
70 return -1;
71 for (int j = 0; j < J.cols(); ++j) {
72 typename Derived::ColXpr Jj = J.col(j);
73 Twist arg;
74 for(unsigned int i=0;i<6;++i)
75 arg(i)=Jj[i];
76 Twist tmp(T*arg);
77 for(unsigned int i=0;i<6;++i)
78 Jj[i]=e_scalar(tmp(i));
79 }
80 return 0;
81 }
82
83 }
84 #endif /* UBLAS_TYPES_HPP_ */
85