orocos_kdl
jntspaceinertiamatrix.cpp
Go to the documentation of this file.
1 // Copyright (C) 2009 Dominick Vanthienen <dominick dot vanthienen at intermodalics dot eu>
2 
3 // Version: 1.0
4 // Author: Dominick Vanthienen <dominick dot vanthienen at intermodalics dot eu>
5 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
23 
24 namespace KDL
25 {
26  JntSpaceInertiaMatrix::JntSpaceInertiaMatrix()
27  {
28  }
29 
30  JntSpaceInertiaMatrix::JntSpaceInertiaMatrix(int _size):
31  data(_size, _size)
32  {
33  data.setZero();
34  }
35 
36 
37  JntSpaceInertiaMatrix::JntSpaceInertiaMatrix(const JntSpaceInertiaMatrix& arg):
38  data(arg.data)
39  {
40  }
41 
42  JntSpaceInertiaMatrix& JntSpaceInertiaMatrix::operator = (const JntSpaceInertiaMatrix& arg)
43  {
44  data=arg.data;
45  return *this;
46  }
47 
48 
49  JntSpaceInertiaMatrix::~JntSpaceInertiaMatrix()
50  {
51  }
52 
53  void JntSpaceInertiaMatrix::resize(unsigned int newSize)
54  {
55  data.resize(newSize,newSize);
56  }
57 
58  double JntSpaceInertiaMatrix::operator()(unsigned int i,unsigned int j)const
59  {
60  return data(i, j);
61  }
62 
63  double& JntSpaceInertiaMatrix::operator()(unsigned int i,unsigned int j)
64  {
65  return data(i, j);
66  }
67 
68  unsigned int JntSpaceInertiaMatrix::rows()const
69  {
70  return static_cast<unsigned int>(data.rows());
71  }
72 
73  unsigned int JntSpaceInertiaMatrix::columns()const
74  {
75  return static_cast<unsigned int>(data.cols());
76  }
77 
78 
79  void Add(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest)
80  {
81  dest.data=src1.data+src2.data;
82  }
83 
84  void Subtract(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest)
85  {
86  dest.data=src1.data-src2.data;
87  }
88 
89  void Multiply(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest)
90  {
91  dest.data=factor*src.data;
92  }
93 
94  void Divide(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest)
95  {
96  dest.data=src.data/factor;
97  }
98 
99  void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest)
100  {
101  dest.data=src.data.lazyProduct(vec.data);
102  }
103 
104  void SetToZero(JntSpaceInertiaMatrix& mat)
105  {
106  mat.data.setZero();
107  }
108 
109  bool Equal(const JntSpaceInertiaMatrix& src1, const JntSpaceInertiaMatrix& src2,double eps)
110  {
111  if(src1.rows()!=src2.rows()||src1.columns()!=src2.columns())
112  return false;
113  return (src1.data-src2.data).isZero(eps);
114  }
115 
116  bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}
117  //bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}
118 }
119 
120 
jntspaceinertiamatrix.hpp
KDL::JntArray
Definition: jntarray.hpp:69
KDL::Subtract
void Subtract(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:85
KDL::Add
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:80
KDL::Multiply
void Multiply(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:90
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::operator==
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:455
KDL::Divide
void Divide(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:95
KDL::JntArray::data
Eigen::VectorXd data
Definition: jntarray.hpp:72
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:79
std::data
T data(T... args)