cob_powercube_chain: Joint.h Source File

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