schunk_powercube_chain: schunk_powercube_chain: Joint.h Source File

00001 
00060 #ifndef _Joint_H
00061 #define _Joint_H
00062 
00063 /*******************************************************************************
00064  *  includes & SWIG support                                                    *
00065  *******************************************************************************/
00066 
00067 #include <math.h>
00068 #include <string.h>
00069 #include <iostream>
00070 #include <vector>
00071 #include <stdlib.h>
00072 #include <cstdio>
00073 #include <stdexcept> // stdexcept header file contains runtime_error
00074 #include <limits>
00075 
00076 
00077 
00078 
00079 /*******************************************************************************
00080  *  defines:                                                                   *
00081  *******************************************************************************/
00082 
00083 /* if length(Joint1 - Joint2) < JOINT_EPSILON, they are considered equal: */
00084 #ifndef JOINT_EPSILON
00085 #define JOINT_EPSILON .01 / 360.0
00086 #endif
00087 
00088 //#define PI 3.141592654
00089 #define DEGS_PER_RAD 57.29577951
00090 #define RADS_PER_DEG 0.017453292
00091 
00092 /* if for some reason (faster performance) you want this class to not perform 
00093    rangechecking, define JOINT_NO_RANGECHECK in your config or makefile!       */
00094    
00095 // #define JOINT_NO_RANGECHECK
00096 
00097 /*******************************************************************************
00098  *  Exception class (definition & implementation)                              *
00099  *******************************************************************************/
00100 
00101 /* In addition to the standard runtime class, this one takes file and Line number arguments. */
00102 class Joint_Exception : public std::runtime_error
00103 {
00104         public:
00105         Joint_Exception(const char* _file, int _line, const char* _msg) :
00106                 std::runtime_error( _msg ),
00107                 file(_file), line(_line) { msg = _msg; }
00108         const char* getFile() const { return file; }
00109         int getLine() const { return line; }
00110         const char* getMsg() const { return msg; }
00111         private:
00112         const char* file;
00113         int line;
00114         const char* msg;
00115 };
00116 
00117 /* operator<< so you can use an exception e like: "cout << e" */
00118 inline std::ostream& operator<<(std::ostream& _os, const Joint_Exception& e)
00119 {
00120         _os << "Error in " << e.getFile();
00121         _os << " at line " << e.getLine() << ":" << std::endl;
00122         _os << " " << e.getMsg() << std::endl;
00123         return _os;
00124 }
00125 
00126 /*******************************************************************************
00127  *  class definition of template class Joint                                   *
00128  *******************************************************************************/
00129 
00130 template <class Real>
00131 class Joint
00132 {
00133 public:
00134 
00135         /* constructors: */
00136         
00137         /* empty Joint, no elements: */
00138     Joint();
00139     /* joint of size NrJoints, zero=false: uninitialized, zero=true -> all elements zero: */
00140         Joint(unsigned int NrJoints, bool zero=false);
00141         /* copy constructor: */
00142         Joint(const Joint& joints);
00143         /* size NrJoints, content copied from d: */
00144         Joint(unsigned int NrJoints, const Real d[]);
00145     
00146     /* destructor: */
00147     virtual ~Joint();
00148     
00149     /* adjust the size of the Joint: */
00150         void setNrJoints(unsigned int NrJoints);
00151         /* make all elements zero: */
00152         void zero();
00153 
00154     /* element access (write): */
00155     
00156         /* set element i to value d: */
00157         void set(unsigned int i,  Real d);
00158         Real& operator[](unsigned int i);
00159         /* set Joint to size NrJoints and copy contents from d: */
00160         void set(unsigned int NrJoints, Real *d);
00161         /* copy constructor: */
00162         //void set(unsigned int const Joint& _rhs);
00163         
00164         /* element access (read): */
00165     
00166     /* return element i: */
00167         Real get(unsigned int i) const;
00168         Real operator[](unsigned int i) const;
00169         /* copy Joint values to array d (d must have enough allocated memory!): */
00170         void get(unsigned int NrJoints, Real* d) const;
00171         /* return number of elements: */
00172         unsigned int size() const;
00173         
00174         /* Return value of the largest/smallest component: */
00175         Real getMax() const;
00176         Real getMin() const;
00177         
00178         /* Return Index of largest/smallest component: */
00179         unsigned int getMaxInd() const;
00180         unsigned int getMinInd() const;
00181     
00182     /* calculate this + (j2-this)*f: */
00183         Joint interpolate( const Joint& j2, Real f ) const;
00184         /* set this joint to j1 + (j2-j1)*f: */
00185         static Joint interpolate( const Joint& j1, const Joint& j2, Real f );
00186         
00187         /* convert the Joint from Deg to Rad or vice versa: */
00188         void toRad();
00189         void toDeg();
00190         
00191         /* define various operators: */
00192 
00193         /* operator= */
00194         Joint& operator=(const Joint& joint);
00195         
00196         /* if length(this - rhs) < JOINT_EPSILON, they are considered equal: */
00197         bool operator==(const Joint& rhs) const;
00198     
00199     /* operators +, -, *, / */
00200         Joint operator-(const Joint& rhs) const;
00201         Joint operator+(const Joint& rhs) const;
00202         Joint operator*(Real s) const;
00203         Joint operator/(Real s) const;
00204         
00205         void operator+=(const Joint& rhs);
00206         void operator-=(const Joint& rhs);
00207         void operator*=(Real s);
00208         void operator/=(Real s);
00209         
00210         /* get euklidian norm (length), squared or normal */
00211         /* (not taking the root saves computation time) */
00212         Real lengthSqr() const;
00213         Real length() const;
00214     
00215     /* return string which looks like: (0.000, 1.000, 2.000, ...) */
00216     /* if convert is true, the values will be converted to degrees */ 
00217         std::string toString(bool convert = false) const;
00218         /* read in the joint from a c-string, format as above: */
00219         void fromString(unsigned int nrjoints, const char* str);
00220         void fromString(const char* str);
00221 
00222         
00223         /* input / output */
00224         
00225         /* operator<< is not a member function and defined below the class definition */
00226         /* same for operator>> */
00227         
00228         /* if for some reason printf should be used rather than operator<< use this: */
00229         void print();
00230         
00231         /* for compatibility with older code: */
00232         unsigned int getNrJoints() const { return size(); }
00233 
00234 
00235 private:
00236         unsigned int m_NrJoints;
00237         Real* m_Joints;
00238         
00239 };
00240 
00241 typedef Joint<double> Jointd;
00242 typedef Joint<float> Jointf;
00243 
00244 
00245 /*******************************************************************************
00246  * prototypes for std input output                                             *
00247  *******************************************************************************/
00248 
00249 template <class Real>
00250 std::ostream& operator<<(std::ostream& _os, const Joint<Real>& joint);
00251 
00252 template <class Real>
00253 std::istream& operator>>(std::istream&, Joint<Real>& joint);
00254 
00255 
00256 /*******************************************************************************
00257  * function for spline compatibility                                           *
00258  *******************************************************************************/
00259 
00260 template <class Real>
00261 inline double Distance(const Joint<Real>& j1, const Joint<Real>& j2)
00262 {
00263         return (j1-j2).length();
00264 }
00265 
00266 /*******************************************************************************
00267  *  Inline function implementation:                                            *
00268  *******************************************************************************/
00269 
00270 /* empty Joint, no elements: */
00271 template <class Real>
00272 inline Joint<Real>::Joint()
00273 {
00274         m_NrJoints = 0;
00275         m_Joints = NULL;
00276 }
00277 
00278 /* joint of size NrJoints, zero=false: uninitialized, zero=true -> all elements zero: */
00279 template <class Real>
00280 inline Joint<Real>::Joint(unsigned int NrJoints, bool _zero)
00281 {
00282         m_NrJoints = NrJoints;
00283         m_Joints = new Real[m_NrJoints];
00284         
00285         if (_zero)
00286                 zero();
00287 }
00288 
00289 /* copy constructor: */
00290 template <class Real>
00291 inline Joint<Real>::Joint(const Joint<Real>& rhs)
00292 {
00293         m_NrJoints = rhs.m_NrJoints;
00294         m_Joints = new Real[rhs.m_NrJoints];
00295         
00296         for (unsigned int i = 0; i < m_NrJoints; i++)
00297                 m_Joints[i] = rhs.m_Joints[i];
00298 }
00299 
00300 /* size NrJoints, content copied from d: */
00301 template <class Real>
00302 inline Joint<Real>::Joint(unsigned int NrJoints, const Real d[])
00303 {
00304         m_NrJoints = NrJoints;
00305         m_Joints = new Real[m_NrJoints];
00306 
00307         for (unsigned int i = 0; i < m_NrJoints; i++)
00308                 m_Joints[i] = d[i];
00309 }
00310 
00311 /* destructor: */
00312 template <class Real>
00313 inline Joint<Real>::~Joint()
00314 {
00315         if (m_Joints)
00316                 delete[] m_Joints;
00317 }
00318     
00319 /* adjust the size of the Joint: */
00320 template <class Real>
00321 inline void Joint<Real>::setNrJoints(unsigned int NrJoints)
00322 {
00323         /* keep old values, if smaller than before cutoff, if larger fill with zeros: */
00324 
00325         Real* old_m_Joints = m_Joints;  
00326         m_Joints = new Real[NrJoints];
00327         
00328         for (unsigned int i = 0; i < NrJoints; i++)
00329         {
00330                 if (i < m_NrJoints)
00331                         m_Joints[i] = old_m_Joints[i];
00332                 else
00333                         m_Joints[i] = 0.0;
00334         }
00335 
00336         if (old_m_Joints)
00337           delete[] old_m_Joints;
00338 
00339         m_NrJoints = NrJoints;
00340 }
00341 
00342 /* make all elements zero: */
00343 template <class Real>
00344 inline void Joint<Real>::zero()
00345 {
00346         for (unsigned int i=0; i < m_NrJoints; i++)
00347                 m_Joints[i] = 0.0;
00348 }
00349 
00350 
00351 
00352 /* element access (write): */
00353 
00354 /* set element i to value d: */
00355 template <class Real>
00356 inline void Joint<Real>::set(unsigned int i, Real d)
00357 {
00358         #ifndef JOINT_NO_RANGECHECK
00359         if ( (m_Joints == NULL) || (i >= m_NrJoints) )
00360                 throw Joint_Exception(__FILE__, __LINE__, "tried to acces an element out of Joint range!");
00361         #endif
00362         m_Joints[i] = d;
00363 }
00364 
00365 template <class Real>
00366 inline Real& Joint<Real>::operator[](unsigned int i)
00367 {
00368         #ifndef JOINT_NO_RANGECHECK
00369         if ( (m_Joints == NULL) || (i >= m_NrJoints) )
00370                 throw Joint_Exception(__FILE__, __LINE__, "tried to acces an element out of Joint range!");
00371         #endif
00372         return m_Joints[i];
00373 }
00374 
00375 /* set Joint to size NrJoints and copy contents from d: */
00376 template <class Real>
00377 inline void Joint<Real>::set(unsigned int NrJoints, Real* d)
00378 {
00379         #ifndef JOINT_NO_RANGECHECK
00380         if ((!m_Joints) || (NrJoints != m_NrJoints) )
00381           setNrJoints(NrJoints);
00382         #endif
00383         
00384         for (unsigned int i = 0; i < m_NrJoints; i++)
00385                 m_Joints[i] = d[i];
00386 }
00387 
00388 
00389 /* element access (read): */
00390 
00391 /* return element i: */
00392 template <class Real>
00393 inline Real Joint<Real>::get(unsigned int i) const
00394 {
00395         #ifndef JOINT_NO_RANGECHECK
00396         if ((m_Joints==NULL) || (i >= m_NrJoints) ) 
00397                 throw Joint_Exception(__FILE__, __LINE__, "tried to acces an element out of Joint range!");
00398         #endif
00399 
00400         return  m_Joints[i];
00401 }
00402 
00403 template <class Real>
00404 inline Real Joint<Real>::operator[](unsigned int i) const
00405 {
00406         return get(i);
00407 }
00408 
00409 
00410 /* copy Joint values to array d (d must have enough allocated memory!): */
00411 template <class Real>
00412 inline void Joint<Real>::get(unsigned int NrJoints, Real* d) const
00413 {
00414         #ifndef JOINT_NO_RANGECHECK
00415         if ((m_Joints==NULL) || (NrJoints > m_NrJoints) ) 
00416                 throw Joint_Exception(__FILE__, __LINE__, "tried to acces an element out of Joint range!");
00417         #endif
00418 
00419         for (unsigned int i = 0; i < m_NrJoints; i++)
00420                 d[i] = m_Joints[i];
00421 }
00422 
00423 /* return number of elements: */
00424 template <class Real>
00425 inline unsigned int Joint<Real>::size() const
00426 {
00427         return m_NrJoints;
00428 }
00429         
00430 /* Return value of the largest/smallest component: */
00431 template <class Real>
00432 inline Real Joint<Real>::getMax() const
00433 {
00434         #ifndef JOINT_NO_RANGECHECK
00435         if (m_NrJoints==0)
00436                 throw Joint_Exception(__FILE__, __LINE__, "tried to call getMax on empty Joint!");
00437         #endif
00438         
00439         Real max = m_Joints[0];
00440         for (unsigned int i=1; i<m_NrJoints; i++)
00441                 max = (m_Joints[i]>max)?m_Joints[i]:max;
00442         return max;
00443 }
00444 
00445 template <class Real>
00446 inline Real Joint<Real>::getMin() const
00447 {
00448         #ifndef JOINT_NO_RANGECHECK
00449         if (m_NrJoints==0)
00450                 throw Joint_Exception(__FILE__, __LINE__, "tried to call getMin on empty Joint!");
00451         #endif
00452         
00453         Real min = m_Joints[0];
00454         for (unsigned int i=1; i<m_NrJoints; i++)
00455                 min = (m_Joints[i]<min)?m_Joints[i]:min;
00456         return min;
00457 }
00458 
00459 
00460         
00461 /* Return Index of largest/smallest component: */
00462 template <class Real>
00463 inline unsigned int Joint<Real>::getMaxInd() const
00464 {
00465         #ifndef JOINT_NO_RANGECHECK
00466         if (m_NrJoints==0)
00467                 throw Joint_Exception(__FILE__, __LINE__, "tried to call getMaxInd on empty Joint!");
00468         #endif
00469         
00470         Real max = m_Joints[0];
00471         unsigned int maxI = 0;
00472         
00473         for (unsigned int i=1; i<m_NrJoints; i++)
00474         {
00475                 if (m_Joints[i]>max)
00476                 {
00477                         max = m_Joints[i];
00478                         maxI = i;
00479                 }
00480         }
00481         return maxI;
00482 }
00483 
00484 template <class Real>
00485 inline unsigned int Joint<Real>::getMinInd() const
00486 {
00487         #ifndef JOINT_NO_RANGECHECK
00488         if (m_NrJoints==0)
00489                 throw Joint_Exception(__FILE__, __LINE__, "tried to call getMinInd on empty Joint!");
00490         #endif
00491         
00492         Real min = m_Joints[0];
00493         unsigned int minI = 0;
00494         
00495         for (unsigned int i=1; i<m_NrJoints; i++)
00496         {
00497                 if (m_Joints[i]<min)
00498                 {
00499                         min = m_Joints[i];
00500                         minI = i;
00501                 }
00502         }
00503         return minI;
00504 }
00505 
00506 /* calculate this + (j2-this)*f: */
00507 template <class Real>
00508 inline Joint<Real> Joint<Real>::interpolate( const Joint& j2, Real f ) const
00509 {
00510         #ifndef JOINT_NO_RANGECHECK
00511         if ((m_Joints == NULL) || (j2.size() != m_NrJoints) )
00512                 throw Joint_Exception(__FILE__, __LINE__, "Joint dimensions mismatch in interpolate!");
00513         #endif
00514         
00515         Joint<Real> result(m_NrJoints);
00516         
00517         for (unsigned int i = 0; i < m_NrJoints; i++)
00518                 result[i] = m_Joints[i] + ( j2[i] - m_Joints[i] ) * f;
00519         
00520         return result;
00521 }
00522 
00523 /* calculate j1 + (j2-j1)*f: */
00524 template <class Real>
00525 inline Joint<Real> Joint<Real>::interpolate( const Joint& j1, const Joint& j2, Real f )
00526 {
00527         #ifndef JOINT_NO_RANGECHECK
00528         if ( j1.size() != j2.size() )
00529                 throw Joint_Exception(__FILE__, __LINE__, "Joint dimensions mismatch in interpolate!");
00530         #endif
00531         
00532         Joint<Real> result(j1.size());
00533         
00534         for (unsigned int i = 0; i < j1.size(); i++)
00535                 result[i] = j1[i] + f * (j2[i] - j1[i]);
00536         
00537         return result;
00538 }
00539 
00540 /* convert the Joint from Deg to Rad or vice versa: */
00541 template <class Real>
00542 inline void Joint<Real>::toRad()
00543 {
00544         for (unsigned int i=0; i < m_NrJoints; i++)
00545           m_Joints[i] *= RADS_PER_DEG; //0.017453292;
00546 }
00547 
00548 template <class Real>
00549 inline void Joint<Real>::toDeg()
00550 {
00551         for (unsigned int i=0; i < m_NrJoints; i++)
00552       m_Joints[i] *= DEGS_PER_RAD; //57.29577951;
00553       // 57.29577951 = 180 / Pi
00554 }
00555 
00556 
00557 /* define various operators: */
00558 
00559 /* operator= */
00560 template <class Real>
00561 inline Joint<Real>& Joint<Real>::operator=(const Joint<Real>& joint)
00562 {
00563         if ( joint.size() != m_NrJoints )
00564                 setNrJoints(joint.size());
00565 
00566         for (unsigned int i = 0; i < m_NrJoints; i++)
00567                 m_Joints[i] = joint[i];
00568                 
00569         return *this;
00570 }
00571 
00572 /* if length(this - rhs) < JOINT_EPSILON, they are considered equal: */
00573 template <class Real>
00574 inline bool Joint<Real>::operator==(const Joint& joint2) const
00575 {
00576         #ifndef JOINT_NO_RANGECHECK
00577         if (m_NrJoints != joint2.size())
00578                 return false;
00579         else {
00580         #endif
00581         
00582                 // Wenn betragsquadrat(joint1-joint2) < epsilon sind joints "gleich"
00583                 Joint<Real> diff = *this - joint2;
00584                 if (diff.lengthSqr() < JOINT_EPSILON)
00585                         return true;
00586                 else
00587                         return false;
00588 
00589         #ifndef JOINT_NO_RANGECHECK
00590         }
00591         #endif
00592 }
00593 
00594 /* operators +, -, *, / */
00595 template <class Real>
00596 inline Joint<Real> Joint<Real>::operator+(const Joint<Real>& rhs) const
00597 {
00598         #ifndef JOINT_NO_RANGECHECK
00599         if (m_NrJoints != rhs.size())
00600                 throw Joint_Exception(__FILE__, __LINE__, "Joint dimensions mismatch in operator+ !");
00601         #endif
00602 
00603     Joint<Real> result(*this);
00604     for (unsigned int i=0; i < m_NrJoints; i++) {
00605         result.m_Joints[i] +=  rhs[i];
00606     }
00607     return result;
00608 }
00609 
00610 
00611 template <class Real>
00612 inline Joint<Real> Joint<Real>::operator-(const Joint<Real>& rhs) const
00613 {
00614         #ifndef JOINT_NO_RANGECHECK
00615         if (m_NrJoints != rhs.size())
00616                 throw Joint_Exception(__FILE__, __LINE__, "Joint dimensions mismatch in operator- !");
00617         #endif
00618 
00619     Joint<Real> result(*this);
00620     for (unsigned int i=0; i < m_NrJoints; i++) {
00621         result.m_Joints[i] -=  rhs[i];
00622     }
00623     return result;
00624 }
00625 
00626 template <class Real>
00627 inline Joint<Real> Joint<Real>::operator*(Real s) const
00628 {
00629         Joint<Real> result(*this);
00630         for (unsigned int i=0; i < m_NrJoints; i++)
00631                 result.m_Joints[i] *= s;
00632         return result;
00633 }
00634 
00635 template <class Real>
00636 inline Joint<Real> Joint<Real>::operator/(Real s) const
00637 {
00638         Joint<Real> result(*this);
00639         if (fabs(s) > std::numeric_limits<Real>::epsilon())
00640                 for (unsigned int i=0; i < m_NrJoints; i++)
00641                         result.m_Joints[i] /= s;
00642         else 
00643                 throw Joint_Exception(__FILE__, __LINE__, "Attempt to divide by zero in operator/ !");
00644                                 
00645         return result;
00646 }
00647 
00648 
00649 
00650 /* operators +=, -=, *=, /= */
00651 template <class Real>
00652 inline void Joint<Real>::operator+=(const Joint<Real>& rhs)
00653 {
00654         #ifndef JOINT_NO_RANGECHECK
00655         if (m_NrJoints != rhs.size())
00656                 throw Joint_Exception(__FILE__, __LINE__, "Joint dimensions mismatch in operator+= !");
00657         #endif
00658 
00659     for (unsigned int i=0; i < m_NrJoints; i++) {
00660         m_Joints[i] +=  rhs[i];
00661     }
00662 }
00663 
00664 template <class Real>
00665 inline void Joint<Real>::operator-=(const Joint<Real>& rhs)
00666 {
00667         #ifndef JOINT_NO_RANGECHECK
00668         if (m_NrJoints != rhs.size())
00669                 throw Joint_Exception(__FILE__, __LINE__, "Joint dimensions mismatch in operator-= !");
00670         #endif
00671 
00672     for (unsigned int i=0; i < m_NrJoints; i++) {
00673         m_Joints[i] -=  rhs[i];
00674     }
00675 }
00676 
00677 template <class Real>
00678 inline void Joint<Real>::operator*=(Real s)
00679 {
00680         for (unsigned int i=0; i < m_NrJoints; i++)
00681                 m_Joints[i] *= s;
00682 }
00683 
00684 template <class Real>
00685 inline void Joint<Real>::operator/=(Real s)
00686 {
00687         if (fabs(s) > std::numeric_limits<Real>::epsilon())
00688                 for (unsigned int i=0; i < m_NrJoints; i++)
00689                         m_Joints[i] /= s;
00690         else 
00691                 throw Joint_Exception(__FILE__, __LINE__, "Attempt to divide by zero in operator/= !");
00692 }
00693 
00694 
00695 
00696 /* get euklidian norm (length), squared or normal */
00697 /* (not taking the root saves computation time) */
00698 template <class Real>
00699 inline Real Joint<Real>::lengthSqr() const
00700 {
00701     Real l = 0;
00702     for (unsigned int i = 0; i < getNrJoints(); i++) {
00703         l += m_Joints[i] * m_Joints[i];
00704     }
00705     return l;
00706 }
00707 
00708 template <class Real>
00709 inline Real Joint<Real>::length() const
00710 {
00711         return sqrt( lengthSqr() );
00712 }
00713     
00714 /* return string which looks like: (0.000, 1.000, 2.000, ...) */
00715 /* if convert is true, the values will be converted to degrees */ 
00716 template <class Real>
00717 inline std::string Joint<Real>::toString(bool convert) const
00718 {
00719         char out[10];
00720         std::string str("(");
00721         for (unsigned int i = 0; i < m_NrJoints; i++) 
00722         {
00723                 if (i != 0)     str+=",";
00724                 if (convert)
00725                         sprintf(out,"%3.3lf", m_Joints[i] * DEGS_PER_RAD );
00726                 else    
00727                         sprintf(out,"%3.6lf", m_Joints[i] );
00728                 str+=std::string(out);
00729         }               
00730         str+=")";
00731         return str;
00732 }
00733 
00734 /* read in the joint from a c-string, format as above: */
00735 template <class Real>
00736 inline void Joint<Real>::fromString(unsigned int nrjoints, const char* str)
00737 {
00738         /* resize the joint if necessary */
00739     if ( nrjoints != m_NrJoints )
00740         setNrJoints(nrjoints);
00741     
00742     const char * start = strrchr(str, '(');
00743     const char * end = strrchr(str, ')');
00744     if (end > start) {
00745         int n = end - start;
00746         char * numbers = new char[n];
00747         strncpy(numbers, start+1, n-1);
00748         char * pch = strtok (numbers,",");
00749         unsigned int i = 0;
00750         while (pch != NULL && i < nrjoints)
00751         {
00752             //cout << pch << endl;
00753             set(i, atof(pch));
00754             i++;
00755             pch = strtok (NULL, ",");
00756         }
00757         delete numbers;
00758     }
00759 }
00760 
00761 /* read in the joint from a c-string, format as above: */
00762 template <class Real>
00763 inline void Joint<Real>::fromString(const char* str)
00764 {
00765         std::vector<Real> vec;
00766             
00767     const char * start = strrchr(str, '(');
00768     const char * end = strrchr(str, ')');
00769     if (end > start) {
00770         int n = end - start;
00771         char * numbers = new char[n];
00772         strncpy(numbers, start+1, n-1);
00773         char * pch = strtok (numbers,",");
00774         //int i = 0;
00775         while (pch != NULL)
00776         {
00777             //cout << pch << endl;
00778             vec.push_back( atof(pch) );
00779             pch = strtok (NULL, ",");
00780         }
00781         }
00782         
00783         setNrJoints( vec.size() );
00784         for (unsigned int i=0; i < m_NrJoints; i++)
00785                 m_Joints[i] = vec[i];
00786 }
00787 
00788 /* if for some reason printf should be used rather than operator<< use this: */
00789 template <class Real>
00790 inline void Joint<Real>::print()
00791 {
00792         for (unsigned int i = 0; i < m_NrJoints; i++)
00793                         printf("%f ",get(i));
00794         printf("\n");
00795 }
00796 
00797 
00798 
00799 
00800 template <class Real>
00801 inline std::ostream& operator<<(std::ostream& _os, const Joint<Real>& joint)
00802 
00803 {
00804         std::string str = joint.toString();
00805         _os << str;
00806         return _os;
00807 }
00808 
00809 template <class Real>
00810 inline std::istream& operator>>(std::istream& _is, Joint<Real>& joint)
00811 {
00812         char c_str[255];
00813         _is.get(c_str, 255 );
00814         joint.fromString(c_str);
00815         return _is;
00816 }
00817         
00818 
00819 #endif
00820 
00821 
00822 #ifdef SWIG
00823 %module Util
00824 %include Source/Manipulation/ManipUtil/datastructsManipulator.h
00825 %include Source/Manipulation/ManipUtil/Trajectory.h
00826 %{
00827         #include "Joint.h"
00828 %}
00829 %include "std_vector.i"
00830 %template(Jointd) Joint<double>;
00831 #endif
00832