Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

trajectory.cpp

Go to the documentation of this file.
00001 /*
00002 Copyright (C) 2002-2004  Etienne Lachance
00003 
00004 This library is free software; you can redistribute it and/or modify
00005 it under the terms of the GNU Lesser General Public License as
00006 published by the Free Software Foundation; either version 2.1 of the
00007 License, or (at your option) any later version.
00008 
00009 This library is distributed in the hope that it will be useful,
00010 but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 GNU Lesser General Public License for more details.
00013 
00014 You should have received a copy of the GNU Lesser General Public
00015 License along with this library; if not, write to the Free Software
00016 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00017 
00018 
00019 Report problems and direct all questions to:
00020 
00021 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
00022 
00023 -------------------------------------------------------------------------------
00024 Revision_history:
00025 
00026 2004/06/02: Etienne Lachance
00027     -Added class Trajectory_Select.
00028 
00029 2004/07/01: Ethan Tira-Thompson
00030     -Added support for newmat's use_namespace #define, using ROBOOP namespace.
00031 -------------------------------------------------------------------------------
00032 */
00033 
00034 /*! 
00035   @file trajectory.cpp
00036   @brief Trajectory member functions.
00037 */
00038 
00039 #include "trajectory.h"
00040 #include <fstream>
00041 
00042 using namespace std;
00043 
00044 #ifdef use_namespace
00045 namespace ROBOOP {
00046   using namespace NEWMAT;
00047 #endif
00048 
00049 //! @brief RCS/CVS version.
00050 static const char rcsid[] __UNUSED__ = "$Id: trajectory.cpp,v 1.7 2007/11/11 23:57:25 ejt Exp $";
00051 
00052 
00053 Spl_cubic::Spl_cubic(const Matrix & pts)
00054 /*!
00055   @brief Constructor
00056   @param pts: Matrix containing the spline data.
00057 
00058   The first line of the Matrix contain the sampling time
00059   Second line contain data (sk) to create spline i.
00060   Third             "                       "    i+1.
00061   on Nth line                                   i+N.
00062 
00063   The spline has the following form:
00064   \f[
00065     s = A_k(t-t_k)^3 + B_k(t-t_k)^2 + C_k(t-t_k) + D_k
00066   \f]
00067 */
00068 {
00069    int N = pts.Ncols();
00070    bad_data = false;
00071    nb_path = pts.Nrows()-1;
00072 
00073    if(!N)
00074    {
00075       bad_data = true;
00076       cerr << "Spl_cubic::Spl_cubic: size of time vector is zero." << endl;
00077       return;
00078    }
00079    if(N <= 3)
00080    {
00081       bad_data = true;
00082       cerr << "Spl_cubic::Spl_cubic: need at least 4 points to produce a cubic spline." << endl;
00083       return;
00084    }
00085    if(!nb_path)
00086    {
00087       bad_data = true;
00088       cerr << "Spl_cubic::Spl_cubic: No data for each points." << endl;
00089       return;
00090    }
00091 
00092    ColumnVector delta(N-1), beta(N-1);
00093    tk = pts.SubMatrix(1,1,1,N);           // time at sampling point
00094 
00095    for(int i = 1; i <= N-1; i++)
00096    {
00097       delta(i) = pts(1,i+1) - pts(1,i);  // eq 5.58d Angeles
00098 
00099       if(!delta(i))
00100       {
00101          bad_data = true;
00102          cerr << "Spl_cubic::Spl_cubic: time between input points is zero" << endl;
00103          return;
00104       }
00105       beta(i) = 1/delta(i);              // eq 5.58e Angeles
00106    }
00107 
00108    Matrix A(N-2, N-2); A = 0;             // eq 5.59 Angeles
00109    A(1,1) = 2*(delta(1)+delta(2));
00110    A(1,2) = delta(2);
00111    for(int j = 2; j <= N-2; j++)
00112    {
00113       A(j,j-1) = delta(j);
00114       A(j,j)   = 2*(delta(j)+delta(j+1));
00115       if( (j+1) <= A.Ncols())
00116          A(j,j+1) = delta(j+1);
00117    }
00118 
00119    Matrix C(N-2, N);   C = 0;             // eq 5.58c Angeles
00120    for(int k = 1; k <= N-2; k++)
00121    {
00122       C(k,k) = beta(k);
00123       C(k,k+1) = -(beta(k)+beta(k+1));
00124       C(k,k+2) = beta(k+1);
00125    }
00126 
00127    Matrix _6AiC = 6*A.i()*C;        // eq 5.58a Angeles
00128 
00129    ColumnVector dd_s(N);   // second spline derivative at sampling points
00130    dd_s(1) = dd_s(N) = 0;  // second der is 0 on first and last point of natural splines.
00131 
00132    Ak = Matrix(nb_path, N-1);
00133    Bk = Matrix(nb_path, N-1);
00134    Ck = Matrix(nb_path, N-1);
00135    Dk = Matrix(nb_path, N-1);
00136 
00137    for(int ii = 2; ii <= nb_path+1; ii++)
00138    {
00139       dd_s.SubMatrix(2,N-1,1,1) = _6AiC*pts.SubMatrix(ii,ii,1,N).t();
00140 
00141       for(int jj = 1; jj < N; jj++)
00142       {
00143          // eq 5.55a - 5.55d Angeles
00144          Ak(ii-1,jj) = 1/(6.0*delta(jj))*(dd_s(jj+1) - dd_s(jj));
00145          Bk(ii-1,jj) = 1/2.0*dd_s(jj);
00146          Ck(ii-1,jj) = (pts(ii,jj+1)-pts(ii,jj))/delta(jj) -
00147                        1/6.0*delta(jj)*(dd_s(jj+1) + 2*dd_s(jj));
00148          Dk(ii-1,jj) = pts(ii,jj);
00149       }
00150    }
00151 }
00152 
00153 Spl_cubic::Spl_cubic(const Spl_cubic & x)
00154 //!  @brief Copy constructor.
00155 {
00156    bad_data = x.bad_data;
00157    nb_path = x.nb_path;
00158    Ak = x.Ak;
00159    Bk = x.Bk;
00160    Ck = x.Ck;
00161    Dk = x.Dk;
00162    tk = x.tk;
00163 }
00164 
00165 
00166 Spl_cubic & Spl_cubic::operator=(const Spl_cubic & x)
00167 //!  @brief Overload = operator.
00168 {
00169    bad_data = x.bad_data;
00170    nb_path = x.nb_path;
00171    Ak = x.Ak;
00172    Bk = x.Bk;
00173    Ck = x.Ck;
00174    Dk = x.Dk;
00175    tk = x.tk;
00176    return *this;
00177 }
00178 
00179 
00180 short Spl_cubic::interpolating(const Real t, ColumnVector & s)
00181 //!  @brief Interpolating the spline at time t. Extrapolating is not allowed.
00182 {
00183    if(bad_data)
00184    {
00185       cerr << "Spl_cubic::interpolating: data is not good. Problems occur in constructor." << endl;
00186       return BAD_DATA;
00187    }
00188 
00189    for(int i = 1; i < tk.Ncols(); i++)
00190       if( (t >= tk(i)) && (t < tk(i+1)) )
00191       {
00192          s = Ak.SubMatrix(1,nb_path,i,i)*pow(t - tk(i), 3) +
00193              Bk.SubMatrix(1,nb_path,i,i)*pow(t - tk(i),2) +
00194              Ck.SubMatrix(1,nb_path,i,i)*(t - tk(i)) +
00195              Dk.SubMatrix(1,nb_path,i,i);
00196          return 0;
00197       }
00198 
00199    cerr << "Spl_cubic::interpolating: t is out of range." << endl;
00200    return NOT_IN_RANGE;
00201 }
00202 
00203 
00204 short Spl_cubic::first_derivative(const Real t, ColumnVector & ds)
00205 //!  @brief Spline first derivative at time t.
00206 {
00207    if(bad_data)
00208    {
00209       cerr << "Spl_cubic::first_derivative: data is not good. Problems occur in constructor." << endl;
00210       return BAD_DATA;
00211    }
00212 
00213    for(int i = 1; i < tk.Ncols(); i++)
00214       if( (t >= tk(i)) && (t < tk(i+1)) )
00215       {
00216          ds = 3*Ak.SubMatrix(1,nb_path,i,i)*pow(t - tk(i), 2) +
00217               2*Bk.SubMatrix(1,nb_path,i,i)*(t - tk(i)) +
00218               Ck.SubMatrix(1,nb_path,i,i);
00219          return 0;
00220       }
00221 
00222    cerr << "Spl_cubic::first_derivative: t not in range." << endl;
00223    return NOT_IN_RANGE;
00224 }
00225 
00226 
00227 short Spl_cubic::second_derivative(const Real t, ColumnVector & ds)
00228 //!  @brief Spline second derivative at time t.
00229 {
00230    if(bad_data)
00231    {
00232       cerr << "Spl_cubic::second_derivative: data is not good. Problems occur in constructor." << endl;
00233       return BAD_DATA;
00234    }
00235 
00236    for(int i = 1; i < tk.Ncols(); i++)
00237       if( (t >= tk(i)) && (t < tk(i+1)) )
00238       {
00239          ds = 6*Ak.SubMatrix(1,nb_path,i,i)*(t - tk(i)) +
00240               2*Bk.SubMatrix(1,nb_path,i,i);
00241          return 0;
00242       }
00243 
00244    cerr << "Spl_cubic::second_derivative: t not in range." << endl;
00245    return NOT_IN_RANGE;
00246 }
00247 
00248 // ----------- E N D - E F F E C T O R   P A T H   W I T H   S P L I N E S  ----------
00249 
00250 Spl_path::Spl_path(const string & filename)
00251 /*!
00252   @brief Constructor.
00253   @param filename: Spline data.
00254 
00255   Here is two examples of file. # is used as a comment, the rest of line is ignored.
00256   The first line should contain the string CARTESIAN_SPACE or JOINT_SPACE. The following
00257   number (3 in first example and 6 in the second) represents the number of points on 
00258   each data line. 0, 0.5 and 1.0 in example 1 represents the time.
00259 
00260 Example 1:
00261 \verbatim
00262 #
00263 #
00264 #
00265 CARTESIAN_SPACE
00266 3
00267 0
00268 0.452100 -0.150050 -0.731800 
00269 0.5
00270 0.561821 -0.021244 -0.537842 
00271 1.0
00272 0.577857 0.268650 -0.323976 
00273 \endverbatim
00274 
00275 Example 2:
00276 \verbatim
00277 #
00278 #
00279 #
00280 JOINT_SPACE
00281 6
00282 0
00283 0 0 0 0 0 0
00284 1.
00285 0.2 -0.4 0.1 0.2 0.3 0.0
00286 2.0
00287 0.6 -0.8 0.2 0.4 0.4 0.0
00288 \endverbatim
00289 */
00290 {
00291    const char *ptr_filename = filename.c_str(); //transform string to *char
00292 
00293    std::ifstream inpointfile(ptr_filename, std::ios::in);
00294    point_map pts_map;
00295 
00296    if(inpointfile)
00297    {
00298       double time;
00299       int nb_path;
00300       string temp;
00301       pts_map.clear();
00302 
00303 #ifdef __WATCOMC__
00304       char tempo[256];
00305       inpointfile.getline(tempo,sizeof(tempo));
00306       temp = tempo;
00307 #else
00308       getline(inpointfile, temp);
00309 #endif
00310       while(temp.substr(0,1) == "#") { // # comment
00311 #ifdef __WATCOMC__
00312          inpointfile.getline(tempo,sizeof(tempo));
00313          temp = tempo;
00314 #else
00315          getline(inpointfile, temp);
00316 #endif
00317       }
00318 
00319       if(temp == "JOINT_SPACE")
00320     type = JOINT_SPACE;
00321       else if (temp == "CARTESIAN_SPACE")
00322     type = CARTESIAN_SPACE;
00323       else
00324       {
00325     cerr << "Spl_path::Spl_path: wrong selection type. Should be joint space or cartesian space." << endl;
00326     return;
00327       }
00328         
00329 #ifdef __WATCOMC__
00330       inpointfile.getline(tempo,sizeof(tempo));
00331       temp = tempo;
00332       istrstream inputString1(tempo);
00333 #else
00334       getline(inpointfile, temp);
00335       istringstream inputString1(temp);
00336 #endif
00337       inputString1 >> nb_path;
00338       if(nb_path < 1)
00339       {
00340     cerr << "Spl_path::Spl_path: number of splines should be >= 1." << endl;
00341     return;
00342       }
00343       ColumnVector p(nb_path);
00344 
00345 
00346       while( !inpointfile.eof() )
00347       {
00348 #ifdef __WATCOMC__
00349          inpointfile.getline(tempo,sizeof(tempo));
00350          temp = tempo;
00351          istrstream inputString2(tempo);
00352 #else
00353          getline(inpointfile, temp);
00354          istringstream inputString2(temp);
00355 #endif
00356 
00357          if(temp.substr(0,1) == " ")
00358             break;
00359          inputString2 >> time;
00360    final_time = time;
00361 
00362 #ifdef __WATCOMC__
00363          inpointfile.getline(tempo,sizeof(tempo));
00364          temp = tempo;
00365          istrstream inputString3(tempo);
00366 #else
00367          getline(inpointfile, temp);
00368          istringstream inputString3(temp);
00369 #endif
00370 
00371          for(int i = 1; i <= nb_path; i++)
00372             inputString3 >> p(i);
00373 
00374          pts_map.insert(point_map::value_type(time, p));
00375       }
00376    }
00377    else
00378       cerr << "Spl_path::Spl_path: can not open file " << filename.c_str() << endl;
00379 
00380    // Spl_cubic class take as input a Matrix that contain the data.
00381    int nb_pts, nb_path;
00382    nb_pts = pts_map.size();
00383    point_map::const_iterator iter = pts_map.begin();
00384    nb_path = iter->second.Nrows();
00385 
00386    Matrix pts(nb_path+1, nb_pts); pts = 0;
00387    {
00388       int i = 1;
00389       for(iter = pts_map.begin(); iter != pts_map.end(); ++iter)
00390       {
00391          pts(1,i) = iter->first;
00392          pts.SubMatrix(2, nb_path+1, i, i) = iter->second;
00393          i++;
00394       }
00395    }
00396    Spl_cubic::operator=(Spl_cubic(pts));
00397 }
00398 
00399 
00400 /*!
00401   @fn Spl_path::Spl_path(const Matrix & pts)
00402   @brief Constructor.
00403 */
00404 Spl_path::Spl_path(const Matrix & pts): Spl_cubic(pts)
00405 {
00406 }
00407 
00408 
00409 /*!
00410   @fn Spl_path::Spl_path(const Spl_path & x)
00411   @brief Copy constructor.
00412 */
00413 Spl_path::Spl_path(const Spl_path & x): Spl_cubic(x)
00414 {
00415   type = x.type;
00416   final_time = x.final_time;
00417 }
00418 
00419 
00420 Spl_path & Spl_path::operator=(const Spl_path & x)
00421 //!  @brief Overload = operator.
00422 {
00423   type = x.type;
00424   final_time = x.final_time;
00425   Spl_cubic::operator=(x);
00426   return *this;
00427 }
00428 
00429 short Spl_path::p(const Real t, ColumnVector & p)
00430 //!  @brief Position vector at time t.
00431 {
00432    if(interpolating(t, p))
00433    {
00434       cerr << "Spl_path::p_pdot: problem with spline interpolating." << endl;
00435       return -4;
00436    }
00437 
00438    return 0;
00439 }
00440 
00441 short Spl_path::p_pdot(const Real t, ColumnVector & p, ColumnVector & pdot)
00442 //!  @brief Position and velocity vector at time t.
00443 {
00444    if(interpolating(t, p))
00445    {
00446       cerr << "Spl_path::p_pdot: problem with spline interpolating." << endl;
00447       return -4;
00448    }
00449    if(first_derivative(t, pdot))
00450    {
00451       cerr << "Spl_path::p_pdot: problem with spline first_derivative." << endl;
00452       return -4;
00453    }
00454 
00455    return 0;
00456 }
00457 
00458 short Spl_path::p_pdot_pddot(const Real t, ColumnVector & p, ColumnVector & pdot,
00459                              ColumnVector & pdotdot)
00460 //! @brief Position, velocity and acceleration vector at time t.
00461 {
00462    if(interpolating(t, p))
00463    {
00464       cerr << "Spl_path::p_pdot_pdotdot: problem with spline interpolating." << endl;
00465       return -4;
00466    }
00467    if(first_derivative(t, pdot))
00468    {
00469       cerr << "Spl_path::p_pdot_pdotdot: problem with spline first_derivative." << endl;
00470       return -4;
00471    }
00472    if(second_derivative(t, pdotdot))
00473    {
00474       cerr << "Spl_path::p_pdot_pdotdot: problem with spline first_derivative." << endl;
00475       return -4;
00476    }
00477    return 0;
00478 }
00479 
00480 // -------------------- Q U A T E R N I O N  S P L I N E S -------------------
00481 
00482 
00483 Spl_Quaternion::Spl_Quaternion(const string & filename)
00484 /*!
00485   @brief Constructor.
00486   @param filename: Quaternions spline data.
00487 
00488 Here is an exemple of file. # is used as comment, the rest of the line is
00489 ignored. 0, 0.5 and 1.0 are the time at the control points. R indicated that the 
00490 quaternion at the control point is obtained from a rotation matrix, and the data 
00491 (9 elements of the matrix) is on the next line. Instead of R one could use Q to 
00492 indicate a quaternion and the following line will contain 4 elements.
00493 
00494 \verbatim
00495 #
00496 #
00497 #
00498 0
00499 R
00500 1.000000 0.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -0.000000 -1.000000
00501 0.5
00502 R
00503 0.999943 0.008696 -0.006149 0.009042 -0.998237 0.058659 -0.005628 -0.058711 -0.998259
00504 1.0
00505 R
00506 0.961882 0.254807 0.099282 0.223474 -0.941661 0.251662 0.157616 -0.219882 -0.962709
00507 \endverbatim
00508 */
00509 {
00510    const char *ptr_filename = filename.c_str(); //transform string to *char
00511 
00512    std::ifstream inquatfile(ptr_filename, std::ios::in);
00513 
00514    if(inquatfile)
00515    {
00516       double time;
00517       string temp;
00518       Matrix R(3,3);
00519       Quaternion q;
00520       quat_data.clear();
00521 #ifdef __WATCOMC__
00522       char tempo[256];
00523 #endif
00524 
00525       while( !inquatfile.eof() )
00526       {
00527 #ifdef __WATCOMC__
00528          inquatfile.getline(tempo,sizeof(tempo));
00529          temp = tempo;
00530 #else
00531          getline(inquatfile, temp);
00532 #endif
00533          while(temp.substr(0,1) == "#") {
00534 #ifdef __WATCOMC__
00535             inquatfile.getline(tempo,sizeof(tempo));
00536             temp = tempo;
00537 #else
00538             getline(inquatfile, temp);
00539 #endif
00540          }
00541 #ifdef __WATCOMC__
00542          istrstream inputString(tempo);
00543 #else
00544          istringstream inputString(temp);
00545 #endif
00546          if(temp.substr(0,1) == " ")
00547             break;
00548          inputString >> time;
00549 
00550 #ifdef __WATCOMC__
00551          inquatfile.getline(tempo,sizeof(tempo));
00552          temp = tempo;
00553 #else
00554          getline(inquatfile, temp);
00555 #endif
00556          if( (temp.substr(0,1) == "r") || (temp.substr(0,1) == "R") )
00557          {
00558 #ifdef __WATCOMC__
00559             inquatfile.getline(tempo,sizeof(tempo));
00560             temp = tempo;
00561             istrstream inputString(tempo);
00562 #else
00563             getline(inquatfile, temp);
00564             istringstream inputString(temp);
00565 #endif
00566             inputString >> R(1,1) >> R(1,2) >> R(1,3) >> R(2,1) >> R(2,2) >>
00567             R(2,3) >> R(3,1) >> R(3,2) >> R(3,3);
00568             q = Quaternion(R);
00569          }
00570          else if( (temp.substr(0,1) == "q") || (temp.substr(0,1) == "Q") )
00571          {
00572 #ifdef __WATCOMC__
00573             inquatfile.getline(tempo,sizeof(tempo));
00574             temp = tempo;
00575             istrstream inputString(tempo);
00576 #else
00577             getline(inquatfile, temp);
00578             istringstream inputString(temp);
00579 #endif
00580             inputString >> R(1,1) >> R(1,2) >> R(1,3) >> R(2,1);
00581             q = Quaternion(R(1,1), R(1,2), R(1,3), R(2,1));
00582          }
00583          else if(temp.substr(0,1) == "")
00584          {
00585          }
00586          else
00587          {
00588             cerr << "Spl_Quaternion::Spl_Quaternion: format of input file "
00589             << filename.c_str() << " is incorrect" << endl;
00590          }
00591 
00592          quat_data.insert( quat_map::value_type(time, q));
00593       }
00594    }
00595    else
00596    {
00597       cerr << "Spl_Quaternion::Spl_Quaternion: can not open file "
00598       << filename.c_str() << endl;
00599    }
00600 }
00601 
00602 
00603 Spl_Quaternion::Spl_Quaternion(const quat_map & quat)
00604 //!  @brief Constructor.
00605 {
00606    quat_data = quat;
00607 }
00608 
00609 
00610 Spl_Quaternion::Spl_Quaternion(const Spl_Quaternion & x)
00611 //!  @brief Copy constructor.
00612 {
00613    quat_data = x.quat_data;
00614 }
00615 
00616 
00617 Spl_Quaternion & Spl_Quaternion::operator=(const Spl_Quaternion & x)
00618 //!  @brief Overload = operator.
00619 {
00620    quat_data = x.quat_data;
00621    return *this;
00622 }
00623 
00624 
00625 
00626 short Spl_Quaternion::quat(const Real t, Quaternion & s)
00627 /*!
00628   @brief Quaternion interpollation.
00629 
00630   \f[
00631     S_n(t) = Squad(q_n,a_n, a(n+1),q(n+1),t)
00632   \f]
00633 */
00634 {
00635    Real dt=0;
00636    Quaternion ds;
00637    quat_map::const_iterator iter1 = quat_data.begin(), iter2;
00638 
00639    if( t == iter1->first)
00640    {
00641       s = iter1->second;
00642       return 0;
00643    }
00644 
00645    // Point to interpolate is between the first and the second point. Use Slerp function
00646    // since there is not enough points for Squad. Use dt since Slerp and Squad functions
00647    // need t from 0 to 1.
00648    iter2 = iter1;
00649    iter2++;
00650    if( t < iter2->first)
00651    {
00652       dt = (t - iter1->first)/(iter2->first - iter1->first);
00653       s  = Slerp(iter1->second, iter2->second, dt);
00654       return 0;
00655    }
00656 
00657 
00658    // From second element to element N-2.
00659    //    for(iter1 = ++quat_data.begin(); iter1 != ----quat_data.end(); ++iter1)
00660    //    for(iter1 = ++iter1; iter1 != ----quat_data.end(); ++iter1)
00661    for(iter1 = iter2; iter1 != ----quat_data.end(); ++iter1)
00662    {
00663       iter2=iter1;
00664       iter2++;
00665       if( (t >= iter1->first) && (t < iter2->first) )
00666       {
00667          dt = (t - iter1->first)/(iter2->first - iter1->first);
00668 
00669          Quaternion qn_  = (--iter1)->second,  // q(n-1)
00670                            qn   = (++iter1)->second,  // q(n)
00671                                   qn_1 = (++iter1)->second,  // q(n+1)
00672                                          qn_2 = (++iter1)->second,  // q(n+2)
00673                                                 q_tmp;
00674          ----iter1;
00675 
00676          // Intermediate point a(n) and a(n+1)
00677          Quaternion an   = qn*(((qn.i()*qn_1).Log() + (qn.i()*qn_).Log())/-4).exp(),
00678                            an_1 = qn_1*(((qn_1.i()*qn_2).Log() + (qn_1.i()*qn).Log())/-4).exp();
00679 
00680          s  = Squad(qn, an, an_1, qn_1, dt);
00681          return 0;
00682       }
00683    }
00684 
00685    // Interpolation between last two points.
00686    iter2 = iter1; iter2++;
00687    if( (t >= iter1->first) && (t <= iter2->first) )
00688    {
00689       dt = (t - iter1->first)/(iter2->first - iter1->first);
00690       s = Slerp(iter1->second, iter2->second, dt);
00691       return 0;
00692    }
00693 
00694    cerr << "Spl_Quaternion::quat_w: t not in range." << endl;
00695    return NOT_IN_RANGE;
00696 }
00697 
00698 short Spl_Quaternion::quat_w(const Real t, Quaternion & s, ColumnVector & w)
00699 //!  @brief Quaternion interpollation and angular velocity.
00700 {
00701    Real dt=0;
00702    Quaternion ds;
00703    quat_map::const_iterator iter1 = quat_data.begin(), iter2;
00704 
00705    if( t == iter1->first)
00706    {
00707       s = iter1->second;
00708       w = ColumnVector(3); w = 0.0;
00709       return 0;
00710    }
00711 
00712    // Point to interpolate is between the first and the second point. Use Slerp function
00713    // since there is not enough points for Squad. Use dt since Slerp and Squad functions
00714    // need t from 0 to 1.
00715    iter2 = iter1;
00716    iter2++;
00717    if( t < iter2->first)
00718    {
00719       dt = (t - iter1->first)/(iter2->first - iter1->first);
00720       s  = Slerp(iter1->second, iter2->second, dt);
00721       ds = Slerp_prime(iter1->second, iter2->second, dt);
00722       w = Omega(s, ds);
00723 
00724       return 0;
00725    }
00726 
00727    // From second element to element N-2.
00728    //    for(iter1 = ++quat_data.begin(); iter1 != ----quat_data.end(); ++iter1)
00729    //    for(iter1 = ++iter1; iter1 != ----quat_data.end(); ++iter1)
00730    for(iter1 = iter2; iter1 != ----quat_data.end(); ++iter1)
00731    {
00732       iter2=iter1;
00733       iter2++;
00734       if( (t >= iter1->first) && (t < iter2->first) )
00735       {
00736          dt = (t - iter1->first)/(iter2->first - iter1->first);
00737 
00738          Quaternion qn_  = (--iter1)->second,  // q(n-1)
00739                            qn   = (++iter1)->second,  // q(n)
00740                                   qn_1 = (++iter1)->second,  // q(n+1)
00741                                          qn_2 = (++iter1)->second,  // q(n+2)
00742                                                 q_tmp;
00743          ----iter1;
00744 
00745          // Intermediate point a(n) and a(n+1)
00746          Quaternion an   = qn*(((qn.i()*qn_1).Log() + (qn.i()*qn_).Log())/-4).exp(),
00747                            an_1 = qn_1*(((qn_1.i()*qn_2).Log() + (qn_1.i()*qn).Log())/-4).exp();
00748 
00749          s  = Squad(qn, an, an_1, qn_1, dt);
00750          ds = Squad_prime(qn, an, an_1, qn_1, dt);
00751          w = Omega(s, ds);
00752          return 0;
00753       }
00754    }
00755 
00756    // Interpolation between last two points.
00757    iter2 = iter1; iter2++;
00758    if( (t >= iter1->first) && (t <= iter2->first) )
00759    {
00760       dt = (t - iter1->first)/(iter2->first - iter1->first);
00761       s = Slerp(iter1->second, iter2->second, dt);
00762       ds = Slerp_prime(iter1->second, iter2->second, dt);
00763       w = Omega(s, ds);
00764       return 0;
00765    }
00766 
00767    cerr << "Spl_Quaternion::quat_w: t not in range." << endl;
00768    return NOT_IN_RANGE;
00769 }
00770 
00771 
00772 // -----------------------------------------------------------------------------------------------
00773 
00774 
00775 Trajectory_Select::Trajectory_Select()
00776 //!  @brief Constructor.
00777 {
00778     type = NONE;
00779     quaternion_active = false;
00780 }
00781 
00782 
00783 Trajectory_Select::Trajectory_Select(const string & filename)
00784 /*!
00785   @brief Constructor.
00786   @param filename: File containing spline data. 
00787 
00788   The file can be a quaternion file (Spl_Quaternion) or path 
00789   file (Spl_path).
00790 */
00791 {
00792     set_trajectory(filename);
00793 }
00794 
00795 
00796 Trajectory_Select & Trajectory_Select::operator=(const Trajectory_Select & x)
00797 //!  @brief Overload = operator.
00798 {
00799     type = x.type;
00800     if(x.quaternion_active)
00801     {
00802   quaternion_active = x.quaternion_active;
00803   path_quat = x.path_quat;
00804     }
00805     else
00806   path = x.path;
00807 
00808     return *this;
00809 }
00810 
00811 
00812 void Trajectory_Select::set_trajectory(const string & filename)
00813 //!  @brief Trajectory selection.
00814 {
00815    const char *ptr_filename = filename.c_str(); //transform string to *char
00816 
00817    std::ifstream inpointfile(ptr_filename, std::ios::in);
00818 
00819    if(inpointfile)
00820    {
00821       string temp;
00822 
00823 #ifdef __WATCOMC__
00824       char tempo[256];
00825       inpointfile.getline(tempo,sizeof(tempo));
00826       temp = tempo;
00827 #else
00828       getline(inpointfile, temp);
00829 #endif
00830       while(temp.substr(0,1) == "#") { // # comment
00831 #ifdef __WATCOMC__
00832          inpointfile.getline(tempo,sizeof(tempo));
00833          temp = tempo;
00834 #else
00835          getline(inpointfile, temp);
00836 #endif
00837       }
00838 
00839       if( (temp == "JOINT_SPACE") || (temp == "CARTESIAN_SPACE") )
00840       {  
00841     path = Spl_path(filename);
00842     type = path.get_type();
00843     quaternion_active = false;
00844       }
00845       else
00846       {
00847     path_quat = Spl_Quaternion(filename);
00848     type = CARTESIAN_SPACE;
00849     quaternion_active = true;
00850       }
00851    }
00852 }
00853 
00854 #ifdef use_namespace
00855 }
00856 #endif
00857 
00858 
00859 
00860 
00861 
00862 
00863 

ROBOOP v1.21a
Generated Thu Nov 22 00:51:28 2007 by Doxygen 1.5.4