jntarrayacc.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 "jntarrayacc.hpp"
23 
24 namespace KDL
25 {
26  JntArrayAcc::JntArrayAcc(unsigned int size):
27  q(size),qdot(size),qdotdot(size)
28  {
29  }
30  JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin,const JntArray& qdotdotin):
31  q(qin),qdot(qdotin),qdotdot(qdotdotin)
32  {
33  assert(q.rows()==qdot.rows()&&qdot.rows()==qdotdot.rows());
34  }
35  JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin):
36  q(qin),qdot(qdotin),qdotdot(q.rows())
37  {
38  assert(q.rows()==qdot.rows());
39  }
41  q(qin),qdot(q.rows()),qdotdot(q.rows())
42  {
43  }
44 
45  void JntArrayAcc::resize(unsigned int newSize)
46  {
47  q.resize(newSize);
48  qdot.resize(newSize);
49  qdotdot.resize(newSize);
50  }
51 
53  {
54  return q;
55  }
56 
58  {
59  return qdot;
60  }
62  {
63  return qdotdot;
64  }
65 
66  void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest)
67  {
68  Add(src1.q,src2.q,dest.q);
69  Add(src1.qdot,src2.qdot,dest.qdot);
70  Add(src1.qdotdot,src2.qdotdot,dest.qdotdot);
71  }
72  void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest)
73  {
74  Add(src1.q,src2.q,dest.q);
75  Add(src1.qdot,src2.qdot,dest.qdot);
76  dest.qdotdot=src1.qdotdot;
77  }
78  void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest)
79  {
80  Add(src1.q,src2,dest.q);
81  dest.qdot=src1.qdot;
82  dest.qdotdot=src1.qdotdot;
83  }
84 
85  void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest)
86  {
87  Subtract(src1.q,src2.q,dest.q);
88  Subtract(src1.qdot,src2.qdot,dest.qdot);
89  Subtract(src1.qdotdot,src2.qdotdot,dest.qdotdot);
90  }
91  void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest)
92  {
93  Subtract(src1.q,src2.q,dest.q);
94  Subtract(src1.qdot,src2.qdot,dest.qdot);
95  dest.qdotdot=src1.qdotdot;
96  }
97  void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest)
98  {
99  Subtract(src1.q,src2,dest.q);
100  dest.qdot=src1.qdot;
101  dest.qdotdot=src1.qdotdot;
102  }
103 
104  void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest)
105  {
106  Multiply(src.q,factor,dest.q);
107  Multiply(src.qdot,factor,dest.qdot);
108  Multiply(src.qdotdot,factor,dest.qdotdot);
109  }
110  void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest)
111  {
112  Multiply(src.qdot,factor.grad*2,dest.qdot);
113  Multiply(src.qdotdot,factor.t,dest.qdotdot);
114  Add(dest.qdot,dest.qdotdot,dest.qdotdot);
115  Multiply(src.q,factor.grad,dest.q);
116  Multiply(src.qdot,factor.t,dest.qdot);
117  Add(dest.qdot,dest.q,dest.qdot);
118  Multiply(src.q,factor.t,dest.q);
119  }
120  void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest)
121  {
122  Multiply(src.q,factor.dd,dest.q);
123  Multiply(src.qdot,factor.d*2,dest.qdot);
124  Multiply(src.qdotdot,factor.t,dest.qdotdot);
125  Add(dest.qdotdot,dest.qdot,dest.qdotdot);
126  Add(dest.qdotdot,dest.q,dest.qdotdot);
127  Multiply(src.q,factor.d,dest.q);
128  Multiply(src.qdot,factor.t,dest.qdot);
129  Add(dest.qdot,dest.q,dest.qdot);
130  Multiply(src.q,factor.t,dest.q);
131  }
132 
133  void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest)
134  {
135  Divide(src.q,factor,dest.q);
136  Divide(src.qdot,factor,dest.qdot);
137  Divide(src.qdotdot,factor,dest.qdotdot);
138  }
139  void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest)
140  {
141  Multiply(src.q,(2*factor.grad*factor.grad)/(factor.t*factor.t*factor.t),dest.q);
142  Multiply(src.qdot,(2*factor.grad)/(factor.t*factor.t),dest.qdot);
143  Divide(src.qdotdot,factor.t,dest.qdotdot);
144  Subtract(dest.qdotdot,dest.qdot,dest.qdotdot);
145  Add(dest.qdotdot,dest.q,dest.qdotdot);
146  Multiply(src.q,factor.grad/(factor.t*factor.t),dest.q);
147  Divide(src.qdot,factor.t,dest.qdot);
148  Subtract(dest.qdot,dest.q,dest.qdot);
149  Divide(src.q,factor.t,dest.q);
150  }
151  void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest)
152  {
153  Multiply(src.q,(2*factor.d*factor.d)/(factor.t*factor.t*factor.t)-factor.dd/(factor.t*factor.t),dest.q);
154  Multiply(src.qdot,(2*factor.d)/(factor.t*factor.t),dest.qdot);
155  Divide(src.qdotdot,factor.t,dest.qdotdot);
156  Subtract(dest.qdotdot,dest.qdot,dest.qdotdot);
157  Add(dest.qdotdot,dest.q,dest.qdotdot);
158  Multiply(src.q,factor.d/(factor.t*factor.t),dest.q);
159  Divide(src.qdot,factor.t,dest.qdot);
160  Subtract(dest.qdot,dest.q,dest.qdot);
161  Divide(src.q,factor.t,dest.q);
162  }
163 
164  void SetToZero(JntArrayAcc& array)
165  {
166  SetToZero(array.q);
167  SetToZero(array.qdot);
168  SetToZero(array.qdotdot);
169  }
170 
171  bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps)
172  {
173  return (Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps)&&Equal(src1.qdotdot,src2.qdotdot,eps));
174  }
175 }
176 
177 
JntArray dderiv() const
Definition: jntarrayacc.cpp:61
friend void Add(const JntArrayAcc &src1, const JntArrayAcc &src2, JntArrayAcc &dest)
Definition: jntarrayacc.cpp:66
void resize(unsigned int newSize)
Definition: jntarrayacc.cpp:45
unsigned int rows() const
Definition: jntarray.cpp:72
JntArray value() const
Definition: jntarrayacc.cpp:52
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
friend void Subtract(const JntArrayAcc &src1, const JntArrayAcc &src2, JntArrayAcc &dest)
Definition: jntarrayacc.cpp:85
friend bool Equal(const JntArrayAcc &src1, const JntArrayAcc &src2, double eps)
friend void SetToZero(JntArrayAcc &array)
void resize(unsigned int newSize)
Definition: jntarray.cpp:55
V grad
gradient
Definition: rall1d.h:57
friend void Divide(const JntArrayAcc &src, const double &factor, JntArrayAcc &dest)
friend void Multiply(const JntArrayAcc &src, const double &factor, JntArrayAcc &dest)
T t
value
Definition: rall1d.h:56
V d
1st derivative
Definition: rall2d.h:58
JntArray deriv() const
Definition: jntarrayacc.cpp:57
T t
value
Definition: rall2d.h:57
V dd
2nd derivative
Definition: rall2d.h:59


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