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  using namespace Eigen;
27 
28  JntSpaceInertiaMatrix::JntSpaceInertiaMatrix()
29  {
30  }
31 
32  JntSpaceInertiaMatrix::JntSpaceInertiaMatrix(int _size):
33  data(_size, _size)
34  {
35  data.setZero();
36  }
37 
38 
39  JntSpaceInertiaMatrix::JntSpaceInertiaMatrix(const JntSpaceInertiaMatrix& arg):
40  data(arg.data)
41  {
42  }
43 
44  JntSpaceInertiaMatrix& JntSpaceInertiaMatrix::operator = (const JntSpaceInertiaMatrix& arg)
45  {
46  data=arg.data;
47  return *this;
48  }
49 
50 
51  JntSpaceInertiaMatrix::~JntSpaceInertiaMatrix()
52  {
53  }
54 
55  void JntSpaceInertiaMatrix::resize(unsigned int newSize)
56  {
57  data.resize(newSize,newSize);
58  }
59 
60  double JntSpaceInertiaMatrix::operator()(unsigned int i,unsigned int j)const
61  {
62  return data(i, j);
63  }
64 
65  double& JntSpaceInertiaMatrix::operator()(unsigned int i,unsigned int j)
66  {
67  return data(i, j);
68  }
69 
70  unsigned int JntSpaceInertiaMatrix::rows()const
71  {
72  return data.rows();
73  }
74 
75  unsigned int JntSpaceInertiaMatrix::columns()const
76  {
77  return data.cols();
78  }
79 
80 
81  void Add(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest)
82  {
83  dest.data=src1.data+src2.data;
84  }
85 
86  void Subtract(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest)
87  {
88  dest.data=src1.data-src2.data;
89  }
90 
91  void Multiply(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest)
92  {
93  dest.data=factor*src.data;
94  }
95 
96  void Divide(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest)
97  {
98  dest.data=src.data/factor;
99  }
100 
101  void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest)
102  {
103  dest.data=src.data.lazyProduct(vec.data);
104  }
105 
106  void SetToZero(JntSpaceInertiaMatrix& mat)
107  {
108  mat.data.setZero();
109  }
110 
111  bool Equal(const JntSpaceInertiaMatrix& src1, const JntSpaceInertiaMatrix& src2,double eps)
112  {
113  if(src1.rows()!=src2.rows()||src1.columns()!=src2.columns())
114  return false;
115  return (src1.data-src2.data).isZero(eps);
116  }
117 
118  bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}
119  //bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}
120 
121 }
122 
123 
friend void SetToZero(JntArrayVel &array)
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
friend void Divide(const JntArrayVel &src, const double &factor, JntArrayVel &dest)
Definition: jntarrayvel.cpp:92
friend void Multiply(const JntArrayVel &src, const double &factor, JntArrayVel &dest)
Definition: jntarrayvel.cpp:79
friend void Subtract(const JntArrayVel &src1, const JntArrayVel &src2, JntArrayVel &dest)
Definition: jntarrayvel.cpp:68
Eigen::VectorXd data
Definition: jntarray.hpp:72
friend bool Equal(const JntArrayVel &src1, const JntArrayVel &src2, double eps)
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:430
friend void Add(const JntArrayVel &src1, const JntArrayVel &src2, JntArrayVel &dest)
Definition: jntarrayvel.cpp:57


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:43