1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
|
/*
* 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_ */
|
