orocos_kdl
jntarray.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
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 
22 #include "jntarray.hpp"
23 
24 namespace KDL
25 {
27  {
28  }
29 
30  JntArray::JntArray(unsigned int _size):
31  data(_size)
32  {
33  data.setZero();
34  }
35 
36 
38  data(arg.data)
39  {
40  }
41 
43  {
44  data=arg.data;
45  return *this;
46  }
47 
48 
50  {
51  }
52 
53  void JntArray::resize(unsigned int newSize)
54  {
55  data.conservativeResizeLike(Eigen::VectorXd::Zero(newSize));
56  }
57 
58  double JntArray::operator()(unsigned int i,unsigned int j)const
59  {
60  assert(j==0);
61  return data(i);
62  }
63 
64  double& JntArray::operator()(unsigned int i,unsigned int j)
65  {
66  assert(j==0);
67  return data(i);
68  }
69 
70  unsigned int JntArray::rows()const
71  {
72  return static_cast<unsigned int>(data.rows());
73  }
74 
75  unsigned int JntArray::columns()const
76  {
77  return static_cast<unsigned int>(data.cols());
78  }
79 
80  void Add(const JntArray& src1,const JntArray& src2,JntArray& dest)
81  {
82  dest.data=src1.data+src2.data;
83  }
84 
85  void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest)
86  {
87  dest.data=src1.data-src2.data;
88  }
89 
90  void Multiply(const JntArray& src,const double& factor,JntArray& dest)
91  {
92  dest.data=factor*src.data;
93  }
94 
95  void Divide(const JntArray& src,const double& factor,JntArray& dest)
96  {
97  dest.data=src.data/factor;
98  }
99 
100  void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest)
101  {
102  Eigen::Matrix<double,6,1> t=jac.data.lazyProduct(src.data);
103  dest=Twist(Vector(t(0),t(1),t(2)),Vector(t(3),t(4),t(5)));
104  }
105 
106  void SetToZero(JntArray& array)
107  {
108  array.data.setZero();
109  }
110 
111  bool Equal(const JntArray& src1, const JntArray& src2,double eps)
112  {
113  if(src1.rows()!=src2.rows())
114  return false;
115  return (src1.data-src2.data).isZero(eps);
116  }
117 
118  bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}
119  //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}
120 
121 }
122 
123 
KDL::JntArray::columns
unsigned int columns() const
Definition: jntarray.cpp:75
KDL::JntArray
Definition: jntarray.hpp:69
KDL::Subtract
void Subtract(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:85
jntarray.hpp
KDL::JntArray::resize
void resize(unsigned int newSize)
Definition: jntarray.cpp:53
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::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::operator==
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:455
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::JntArray::operator()
double operator()(unsigned int i, unsigned int j=0) const
Definition: jntarray.cpp:58
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:70
KDL::Divide
void Divide(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:95
KDL::JntArray::JntArray
JntArray()
Definition: jntarray.cpp:26
KDL::JntArray::~JntArray
~JntArray()
Definition: jntarray.cpp:49
KDL::JntArray::data
Eigen::VectorXd data
Definition: jntarray.hpp:72
KDL::Jacobian::data
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
Definition: jacobian.hpp:41
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::JntArray::operator=
JntArray & operator=(const JntArray &arg)
Definition: jntarray.cpp:42
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:79
KDL::MultiplyJacobian
void MultiplyJacobian(const Jacobian &jac, const JntArray &src, Twist &dest)
Definition: jntarray.cpp:100
KDL::Jacobian
Definition: jacobian.hpp:36