Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / kdl / jntarray.hpp
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 #ifndef KDL_JNTARRAY_HPP
23 #define KDL_JNTARRAY_HPP
24
25 #include "frames.hpp"
26 #include "jacobian.hpp"
27
28 namespace KDL
29 {
30     /**
31      * @brief This class represents an fixed size array containing
32      * joint values of a KDL::Chain.
33      *
34      * \warning An object constructed with the default constructor provides
35      * a valid, but inert, object. Many of the member functions will do
36      * the correct thing and have no affect on this object, but some 
37      * member functions can _NOT_ deal with an inert/empty object. These 
38      * functions will assert() and exit the program instead. The intended use 
39      * case for the default constructor (in an RTT/OCL setting) is outlined in
40      * code below - the default constructor plus the resize() function allow
41      * use of JntArray objects whose size is set within a configureHook() call
42      * (typically based on a size determined from a property).
43          
44 \code
45 class MyTask : public RTT::TaskContext
46 {
47    JntArray             j;
48    MyTask() 
49    {}                   // invokes j's default constructor
50
51    bool configureHook()
52    {
53        unsigned int size = some_property.rvalue();
54            j.resize(size)
55            ...
56    }
57
58    void updateHook()
59    {
60    ** use j here
61    }
62 };
63 /endcode
64
65      */ 
66
67     class JntArray
68     {
69     private:
70         unsigned int size;
71         double* data;
72     public:
73         /** Construct with _no_ data array
74          * @post NULL == data
75          * @post 0 == rows()
76          * @warning use of an object constructed like this, without
77          * a resize() first, may result in program exit! See class
78          * documentation.
79          */
80         JntArray();
81         /**
82          * Constructor of the joint array
83          *
84          * @param size size of the array, this cannot be changed
85          * afterwards.
86          * @pre 0 < size
87          * @post NULL != data
88          * @post 0 < rows()
89          * @post all elements in data have 0 value
90          */
91         JntArray(unsigned int size);
92         /** Copy constructor 
93          * @note Will correctly copy an empty object
94          */
95         JntArray(const JntArray& arg);
96         ~JntArray();
97         /** Resize the array 
98          * @warning This causes a dynamic allocation (and potentially   
99          * also a dynamic deallocation). This _will_ negatively affect
100          * real-time performance! 
101          *
102          * @post newSize == rows()
103          * @post NULL != data
104          * @post all elements in data have 0 value
105          */
106         void resize(unsigned int newSize);
107                 
108         JntArray& operator = ( const JntArray& arg);
109         /**
110          * get_item operator for the joint array, if a second value is
111          * given it should be zero, since a JntArray resembles a column.
112          *
113          *
114          * @return the joint value at position i, starting from 0
115          * @pre 0 != size (ie non-default constructor or resize() called)
116          */
117         double operator()(unsigned int i,unsigned int j=0)const;
118         /**
119          * set_item operator, again if a second value is given it
120          *should be zero.
121          *
122          * @return reference to the joint value at position i,starting
123          *from zero.
124          * @pre 0 != size (ie non-default constructor or resize() called)
125          */
126         double& operator()(unsigned int i,unsigned int j=0);
127         /**
128          * Returns the number of rows (size) of the array
129          *
130          */
131         unsigned int rows()const;
132         /**
133          * Returns the number of columns of the array, always 1.
134          */
135         unsigned int columns()const;
136
137         /**
138          * Function to add two joint arrays, all the arguments must
139          * have the same size: A + B = C. This function is
140          * aliasing-safe, A or B can be the same array as C.
141          *
142          * @param src1 A
143          * @param src2 B
144          * @param dest C
145          */
146         friend void Add(const JntArray& src1,const JntArray& src2,JntArray& dest);
147         /**
148          * Function to subtract two joint arrays, all the arguments must
149          * have the same size: A - B = C. This function is
150          * aliasing-safe, A or B can be the same array as C.
151          *
152          * @param src1 A
153          * @param src2 B
154          * @param dest C
155          */
156         friend void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest);
157         /**
158          * Function to multiply all the array values with a scalar
159          * factor: A*b=C. This function is aliasing-safe, A can be the
160          * same array as C.
161          *
162          * @param src A
163          * @param factor b
164          * @param dest C
165          */
166         friend void Multiply(const JntArray& src,const double& factor,JntArray& dest);
167         /**
168          * Function to divide all the array values with a scalar
169          * factor: A/b=C. This function is aliasing-safe, A can be the
170          * same array as C.
171          *
172          * @param src A
173          * @param factor b
174          * @param dest C
175          */
176         friend void Divide(const JntArray& src,const double& factor,JntArray& dest);
177         /**
178          * Function to multiply a KDL::Jacobian with a KDL::JntArray
179          * to get a KDL::Twist, it should not be used to calculate the
180          * forward velocity kinematics, the solver classes are built
181          * for this purpose.
182          * J*q = t
183          *
184          * @param jac J
185          * @param src q
186          * @param dest t
187          * @post dest==Twist::Zero() if 0==src.rows() (ie src is empty)
188          */
189         friend void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest);
190         /**
191          * Function to set all the values of the array to 0
192          *
193          * @param array
194          */
195         friend void SetToZero(JntArray& array);
196         /**
197          * Function to check if two arrays are the same with a
198          *precision of eps
199          *
200          * @param src1
201          * @param src2
202          * @param eps default: epsilon
203          * @return true if each element of src1 is within eps of the same
204                  * element in src2, or if both src1 and src2 have no data (ie 0==rows())
205          */
206         friend bool Equal(const JntArray& src1,const JntArray& src2,double eps=epsilon);
207
208         friend bool operator==(const JntArray& src1,const JntArray& src2);
209         //friend bool operator!=(const JntArray& src1,const JntArray& src2);
210         };
211
212     bool operator==(const JntArray& src1,const JntArray& src2);
213     //bool operator!=(const JntArray& src1,const JntArray& src2);
214
215 }
216
217 #endif