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

ROBOOP v1.21a
Generated Tue Nov 23 16:35:51 2004 by Doxygen 1.3.9.1