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 {
26  using namespace Eigen;
27 
29  {
30  }
31 
32  JntArray::JntArray(unsigned int _size):
33  data(_size)
34  {
35  data.setZero();
36  }
37 
38 
40  data(arg.data)
41  {
42  }
43 
45  {
46  data=arg.data;
47  return *this;
48  }
49 
50 
52  {
53  }
54 
55  void JntArray::resize(unsigned int newSize)
56  {
57  data.conservativeResizeLike(VectorXd::Zero(newSize));
58  }
59 
60  double JntArray::operator()(unsigned int i,unsigned int j)const
61  {
62  assert(j==0);
63  return data(i);
64  }
65 
66  double& JntArray::operator()(unsigned int i,unsigned int j)
67  {
68  assert(j==0);
69  return data(i);
70  }
71 
72  unsigned int JntArray::rows()const
73  {
74  return data.rows();
75  }
76 
77  unsigned int JntArray::columns()const
78  {
79  return data.cols();
80  }
81 
82  void Add(const JntArray& src1,const JntArray& src2,JntArray& dest)
83  {
84  dest.data=src1.data+src2.data;
85  }
86 
87  void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest)
88  {
89  dest.data=src1.data-src2.data;
90  }
91 
92  void Multiply(const JntArray& src,const double& factor,JntArray& dest)
93  {
94  dest.data=factor*src.data;
95  }
96 
97  void Divide(const JntArray& src,const double& factor,JntArray& dest)
98  {
99  dest.data=src.data/factor;
100  }
101 
102  void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest)
103  {
104  Eigen::Matrix<double,6,1> t=jac.data.lazyProduct(src.data);
105  dest=Twist(Vector(t(0),t(1),t(2)),Vector(t(3),t(4),t(5)));
106  }
107 
108  void SetToZero(JntArray& array)
109  {
110  array.data.setZero();
111  }
112 
113  bool Equal(const JntArray& src1, const JntArray& src2,double eps)
114  {
115  if(src1.rows()!=src2.rows())
116  return false;
117  return (src1.data-src2.data).isZero(eps);
118  }
119 
120  bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}
121  //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}
122 
123 }
124 
125 
friend void MultiplyJacobian(const Jacobian &jac, const JntArray &src, Twist &dest)
Definition: jntarray.cpp:102
unsigned int rows() const
Definition: jntarray.cpp:72
friend void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:82
friend void SetToZero(JntArray &array)
Definition: jntarray.cpp:108
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
double operator()(unsigned int i, unsigned int j=0) const
Definition: jntarray.cpp:60
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
Definition: jacobian.hpp:41
represents both translational and rotational velocities.
Definition: frames.hpp:720
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
unsigned int columns() const
Definition: jntarray.cpp:77
Eigen::VectorXd data
Definition: jntarray.hpp:72
friend void Multiply(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:92
void resize(unsigned int newSize)
Definition: jntarray.cpp:55
friend void Subtract(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:87
JntArray & operator=(const JntArray &arg)
Definition: jntarray.cpp:44
friend bool operator==(const JntArray &src1, const JntArray &src2)
Definition: jntarray.cpp:120
friend bool Equal(const JntArray &src1, const JntArray &src2, double eps)
Definition: jntarray.cpp:113
friend void Divide(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:97


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