orocos_kdl
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 
KDL::JntArrayAcc::value
JntArray value() const
Definition: jntarrayacc.cpp:52
KDL::JntArray
Definition: jntarray.hpp:69
KDL::JntArrayAcc::qdot
JntArray qdot
Definition: jntarrayacc.hpp:53
KDL::Subtract
void Subtract(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:85
KDL::Rall2d::dd
V dd
2nd derivative
Definition: rall2d.h:103
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
jntarrayacc.hpp
KDL::JntArrayAcc::JntArrayAcc
JntArrayAcc()
Definition: jntarrayacc.hpp:56
KDL::Rall1d::grad
V grad
gradient
Definition: rall1d.h:101
KDL::Multiply
void Multiply(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:90
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::JntArrayAcc::dderiv
JntArray dderiv() const
Definition: jntarrayacc.cpp:61
KDL::JntArrayAcc::qdotdot
JntArray qdotdot
Definition: jntarrayacc.hpp:54
KDL::JntArrayAcc::deriv
JntArray deriv() const
Definition: jntarrayacc.cpp:57
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::Rall1d::t
T t
value
Definition: rall1d.h:100
KDL::Rall2d
Definition: rall2d.h:76
KDL::JntArrayVel
Definition: jntarrayvel.hpp:45
KDL::Rall2d::t
T t
value
Definition: rall2d.h:101
KDL::Rall1d
Definition: rall1d.h:71
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::JntArrayVel::qdot
JntArray qdot
Definition: jntarrayvel.hpp:49
KDL::Rall2d::d
V d
1st derivative
Definition: rall2d.h:102
KDL::JntArrayAcc::q
JntArray q
Definition: jntarrayacc.hpp:52
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:79
KDL::JntArrayVel::q
JntArray q
Definition: jntarrayvel.hpp:48
KDL::JntArrayAcc::resize
void resize(unsigned int newSize)
Definition: jntarrayacc.cpp:45
KDL::JntArrayAcc
Definition: jntarrayacc.hpp:49