jntarrayvel.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 
23 #include "jntarrayacc.hpp"
24 
25 namespace KDL
26 {
27  JntArrayVel::JntArrayVel(unsigned int size):
28  q(size),qdot(size)
29  {
30  }
31  JntArrayVel::JntArrayVel(const JntArray& qin, const JntArray& qdotin):
32  q(qin),qdot(qdotin)
33  {
34  assert(q.rows()==qdot.rows());
35  }
37  q(qin),qdot(q.rows())
38  {
39  }
40 
41  void JntArrayVel::resize(unsigned int newSize)
42  {
43  q.resize(newSize);
44  qdot.resize(newSize);
45  }
46 
48  {
49  return q;
50  }
51 
53  {
54  return qdot;
55  }
56 
57  void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest)
58  {
59  Add(src1.q,src2.q,dest.q);
60  Add(src1.qdot,src2.qdot,dest.qdot);
61  }
62  void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest)
63  {
64  Add(src1.q,src2,dest.q);
65  dest.qdot=src1.qdot;
66  }
67 
68  void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest)
69  {
70  Subtract(src1.q,src2.q,dest.q);
71  Subtract(src1.qdot,src2.qdot,dest.qdot);
72  }
73  void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest)
74  {
75  Subtract(src1.q,src2,dest.q);
76  dest.qdot=src1.qdot;
77  }
78 
79  void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest)
80  {
81  Multiply(src.q,factor,dest.q);
82  Multiply(src.qdot,factor,dest.qdot);
83  }
84  void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest)
85  {
86  Multiply(src.q,factor.grad,dest.q);
87  Multiply(src.qdot,factor.t,dest.qdot);
88  Add(dest.qdot,dest.q,dest.qdot);
89  Multiply(src.q,factor.t,dest.q);
90  }
91 
92  void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest)
93  {
94  Divide(src.q,factor,dest.q);
95  Divide(src.qdot,factor,dest.qdot);
96  }
97  void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest)
98  {
99  Multiply(src.q,(factor.grad/factor.t/factor.t),dest.q);
100  Divide(src.qdot,factor.t,dest.qdot);
101  Subtract(dest.qdot,dest.q,dest.qdot);
102  Divide(src.q,factor.t,dest.q);
103  }
104 
105  void SetToZero(JntArrayVel& array)
106  {
107  SetToZero(array.q);
108  SetToZero(array.qdot);
109  }
110 
111  bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps)
112  {
113  return Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps);
114  }
115 }
116 
117 
friend void SetToZero(JntArrayVel &array)
unsigned int rows() const
Definition: jntarray.cpp:72
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
JntArray value() const
Definition: jntarrayvel.cpp:47
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
void resize(unsigned int newSize)
Definition: jntarrayvel.cpp:41
JntArray deriv() const
Definition: jntarrayvel.cpp:52
friend bool Equal(const JntArrayVel &src1, const JntArrayVel &src2, double eps)
void resize(unsigned int newSize)
Definition: jntarray.cpp:55
V grad
gradient
Definition: rall1d.h:57
T t
value
Definition: rall1d.h:56
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