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 2005/06/10: Etienne Lachance
00033     -Missing file warning message.
00034 
00035 2005/11/06: Etienne Lachance
00036     - No need to provide a copy constructor and the assignment operator 
00037       (operator=) for Spl_Quaternion, Spl_Cubic and Spl_path classes. Instead we 
00038       use the one provide by the compiler.
00039 -------------------------------------------------------------------------------
00040 */
00041 
00047 
00048 static const char rcsid[] = "$Id: trajectory.cpp,v 1.8 2006/05/16 19:24:26 gourdeau Exp $";
00049 
00050 #include "trajectory.h"
00051 
00052 #ifdef use_namespace
00053 namespace ROBOOP {
00054   using namespace NEWMAT;
00055 #endif
00056 
00057 
00058 Spl_cubic::Spl_cubic(const Matrix & pts)
00073 {
00074    int N = pts.Ncols();
00075    bad_data = false;
00076    nb_path = pts.Nrows()-1;
00077 
00078    if(!N)
00079    {
00080       bad_data = true;
00081       cerr << "Spl_cubic::Spl_cubic: size of time vector is zero." << endl;
00082       return;
00083    }
00084    if(N <= 3)
00085    {
00086       bad_data = true;
00087       cerr << "Spl_cubic::Spl_cubic: need at least 4 points to produce a cubic spline." << endl;
00088       return;
00089    }
00090    if(!nb_path)
00091    {
00092       bad_data = true;
00093       cerr << "Spl_cubic::Spl_cubic: No data for each points." << endl;
00094       return;
00095    }
00096 
00097    ColumnVector delta(N-1), beta(N-1);
00098    tk = pts.SubMatrix(1,1,1,N);           // time at sampling point
00099 
00100    for(int i = 1; i <= N-1; i++)
00101    {
00102       delta(i) = pts(1,i+1) - pts(1,i);  // eq 5.58d Angeles
00103 
00104       if(!delta(i))
00105       {
00106          bad_data = true;
00107          cerr << "Spl_cubic::Spl_cubic: time between input points is zero" << endl;
00108          return;
00109       }
00110       beta(i) = 1/delta(i);              // eq 5.58e Angeles
00111    }
00112 
00113    Matrix A(N-2, N-2); A = 0;             // eq 5.59 Angeles
00114    A(1,1) = 2*(delta(1)+delta(2));
00115    A(1,2) = delta(2);
00116    for(int j = 2; j <= N-2; j++)
00117    {
00118       A(j,j-1) = delta(j);
00119       A(j,j)   = 2*(delta(j)+delta(j+1));
00120       if( (j+1) <= A.Ncols())
00121          A(j,j+1) = delta(j+1);
00122    }
00123 
00124    Matrix C(N-2, N);   C = 0;             // eq 5.58c Angeles
00125    for(int k = 1; k <= N-2; k++)
00126    {
00127       C(k,k) = beta(k);
00128       C(k,k+1) = -(beta(k)+beta(k+1));
00129       C(k,k+2) = beta(k+1);
00130    }
00131 
00132    Matrix _6AiC = 6*A.i()*C;        // eq 5.58a Angeles
00133 
00134    ColumnVector dd_s(N);   // second spline derivative at sampling points
00135    dd_s(1) = dd_s(N) = 0;  // second der is 0 on first and last point of natural splines.
00136 
00137    Ak = Matrix(nb_path, N-1);
00138    Bk = Matrix(nb_path, N-1);
00139    Ck = Matrix(nb_path, N-1);
00140    Dk = Matrix(nb_path, N-1);
00141 
00142    for(int ii = 2; ii <= nb_path+1; ii++)
00143    {
00144       dd_s.SubMatrix(2,N-1,1,1) = _6AiC*pts.SubMatrix(ii,ii,1,N).t();
00145 
00146       for(int jj = 1; jj < N; jj++)
00147       {
00148          // eq 5.55a - 5.55d Angeles
00149          Ak(ii-1,jj) = 1/(6.0*delta(jj))*(dd_s(jj+1) - dd_s(jj));
00150          Bk(ii-1,jj) = 1/2.0*dd_s(jj);
00151          Ck(ii-1,jj) = (pts(ii,jj+1)-pts(ii,jj))/delta(jj) -
00152                        1/6.0*delta(jj)*(dd_s(jj+1) + 2*dd_s(jj));
00153          Dk(ii-1,jj) = pts(ii,jj);
00154       }
00155    }
00156 }
00157 
00158 short Spl_cubic::interpolating(const Real t, ColumnVector & s)
00160 {
00161    if(bad_data)
00162    {
00163       cerr << "Spl_cubic::interpolating: data is not good. Problems occur in constructor." << endl;
00164       return BAD_DATA;
00165    }
00166 
00167    for(int i = 1; i < tk.Ncols(); i++)
00168       if( (t >= tk(i)) && (t < tk(i+1)) )
00169       {
00170          s = Ak.SubMatrix(1,nb_path,i,i)*pow(t - tk(i), 3) +
00171              Bk.SubMatrix(1,nb_path,i,i)*pow(t - tk(i),2) +
00172              Ck.SubMatrix(1,nb_path,i,i)*(t - tk(i)) +
00173              Dk.SubMatrix(1,nb_path,i,i);
00174          return 0;
00175       }
00176 
00177    cerr << "Spl_cubic::interpolating: t is out of range." << endl;
00178    return NOT_IN_RANGE;
00179 }
00180 
00181 
00182 short Spl_cubic::first_derivative(const Real t, ColumnVector & ds)
00184 {
00185    if(bad_data)
00186    {
00187       cerr << "Spl_cubic::first_derivative: data is not good. Problems occur in constructor." << endl;
00188       return BAD_DATA;
00189    }
00190 
00191    for(int i = 1; i < tk.Ncols(); i++)
00192       if( (t >= tk(i)) && (t < tk(i+1)) )
00193       {
00194          ds = 3*Ak.SubMatrix(1,nb_path,i,i)*pow(t - tk(i), 2) +
00195               2*Bk.SubMatrix(1,nb_path,i,i)*(t - tk(i)) +
00196               Ck.SubMatrix(1,nb_path,i,i);
00197          return 0;
00198       }
00199 
00200    cerr << "Spl_cubic::first_derivative: t not in range." << endl;
00201    return NOT_IN_RANGE;
00202 }
00203 
00204 
00205 short Spl_cubic::second_derivative(const Real t, ColumnVector & ds)
00207 {
00208    if(bad_data)
00209    {
00210       cerr << "Spl_cubic::second_derivative: data is not good. Problems occur in constructor." << endl;
00211       return BAD_DATA;
00212    }
00213 
00214    for(int i = 1; i < tk.Ncols(); i++)
00215       if( (t >= tk(i)) && (t < tk(i+1)) )
00216       {
00217          ds = 6*Ak.SubMatrix(1,nb_path,i,i)*(t - tk(i)) +
00218               2*Bk.SubMatrix(1,nb_path,i,i);
00219          return 0;
00220       }
00221 
00222    cerr << "Spl_cubic::second_derivative: t not in range." << endl;
00223    return NOT_IN_RANGE;
00224 }
00225 
00226 // ----------- 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  ----------
00227 
00228 Spl_path::Spl_path(const string & filename)
00268 {
00269    const char *ptr_filename = filename.c_str(); //transform string to *char
00270 
00271    std::ifstream inpointfile(ptr_filename, std::ios::in);
00272    point_map pts_map;
00273 
00274    if(inpointfile)
00275    {
00276       double time;
00277       int nb_path;
00278       string temp;
00279       pts_map.clear();
00280 
00281       getline(inpointfile, temp);
00282       while(temp.substr(0,1) == "#") { // # comment
00283          getline(inpointfile, temp);
00284       }
00285 
00286       if(temp == "JOINT_SPACE")
00287           type = JOINT_SPACE;
00288       else if (temp == "CARTESIAN_SPACE")
00289           type = CARTESIAN_SPACE;
00290       else
00291       {
00292           cerr << "Spl_path::Spl_path: wrong selection type. Should be joint space or cartesian space." << endl;
00293           return;
00294       }
00295               
00296       getline(inpointfile, temp);
00297       istringstream inputString1(temp);
00298       inputString1 >> nb_path;
00299       if(nb_path < 1)
00300       {
00301           cerr << "Spl_path::Spl_path: number of splines should be >= 1." << endl;
00302           return;
00303       }
00304       ColumnVector p(nb_path);
00305 
00306 
00307       while( !inpointfile.eof() )
00308       {
00309          getline(inpointfile, temp);
00310          istringstream inputString2(temp);
00311 
00312          if(temp.substr(0,1) == " ")
00313             break;
00314          inputString2 >> time;
00315          final_time = time;
00316 
00317          getline(inpointfile, temp);
00318          istringstream inputString3(temp);
00319 
00320          for(int i = 1; i <= nb_path; i++)
00321             inputString3 >> p(i);
00322 
00323          pts_map.insert(point_map::value_type(time, p));
00324       }
00325    }
00326    else
00327       cerr << "Spl_path::Spl_path: can not open file " << filename.c_str() << endl;
00328 
00329    // Spl_cubic class take as input a Matrix that contain the data.
00330    int nb_pts, nb_path;
00331    nb_pts = pts_map.size();
00332    point_map::const_iterator iter = pts_map.begin();
00333    nb_path = iter->second.Nrows();
00334 
00335    Matrix pts(nb_path+1, nb_pts); pts = 0;
00336    {
00337       int i = 1;
00338       for(iter = pts_map.begin(); iter != pts_map.end(); ++iter)
00339       {
00340          pts(1,i) = iter->first;
00341          pts.SubMatrix(2, nb_path+1, i, i) = iter->second;
00342          i++;
00343       }
00344    }
00345    Spl_cubic::operator=(Spl_cubic(pts));
00346 }
00347 
00348 
00353 Spl_path::Spl_path(const Matrix & pts): Spl_cubic(pts)
00354 {
00355 }
00356 
00357 short Spl_path::p(const Real t, ColumnVector & p)
00359 {
00360    if(interpolating(t, p))
00361    {
00362       cerr << "Spl_path::p_pdot: problem with spline interpolating." << endl;
00363       return -4;
00364    }
00365 
00366    return 0;
00367 }
00368 
00369 short Spl_path::p_pdot(const Real t, ColumnVector & p, ColumnVector & pdot)
00371 {
00372    if(interpolating(t, p))
00373    {
00374       cerr << "Spl_path::p_pdot: problem with spline interpolating." << endl;
00375       return -4;
00376    }
00377    if(first_derivative(t, pdot))
00378    {
00379       cerr << "Spl_path::p_pdot: problem with spline first_derivative." << endl;
00380       return -4;
00381    }
00382 
00383    return 0;
00384 }
00385 
00386 short Spl_path::p_pdot_pddot(const Real t, ColumnVector & p, ColumnVector & pdot,
00387                              ColumnVector & pdotdot)
00389 {
00390    if(interpolating(t, p))
00391    {
00392       cerr << "Spl_path::p_pdot_pdotdot: problem with spline interpolating." << endl;
00393       return -4;
00394    }
00395    if(first_derivative(t, pdot))
00396    {
00397       cerr << "Spl_path::p_pdot_pdotdot: problem with spline first_derivative." << endl;
00398       return -4;
00399    }
00400    if(second_derivative(t, pdotdot))
00401    {
00402       cerr << "Spl_path::p_pdot_pdotdot: problem with spline first_derivative." << endl;
00403       return -4;
00404    }
00405    return 0;
00406 }
00407 
00408 // -------------------- Q U A T E R N I O N  S P L I N E S -------------------
00409 
00410 
00411 Spl_Quaternion::Spl_Quaternion(const string & filename)
00437 {
00438    const char *ptr_filename = filename.c_str(); //transform string to *char
00439 
00440    std::ifstream inquatfile(ptr_filename, std::ios::in);
00441 
00442    if(inquatfile)
00443    {
00444       double time;
00445       string temp;
00446       Matrix R(3,3);
00447       Quaternion q;
00448       quat_data.clear();
00449 
00450       while( !inquatfile.eof() )
00451       {
00452          getline(inquatfile, temp);
00453          while(temp.substr(0,1) == "#") {
00454             getline(inquatfile, temp);
00455          }
00456          istringstream inputString(temp);
00457          if(temp.substr(0,1) == " ")
00458             break;
00459          inputString >> time;
00460 
00461          getline(inquatfile, temp);
00462          if( (temp.substr(0,1) == "r") || (temp.substr(0,1) == "R") )
00463          {
00464             getline(inquatfile, temp);
00465             istringstream inputString(temp);
00466             inputString >> R(1,1) >> R(1,2) >> R(1,3) >> R(2,1) >> R(2,2) >>
00467             R(2,3) >> R(3,1) >> R(3,2) >> R(3,3);
00468             q = Quaternion(R);
00469          }
00470          else if( (temp.substr(0,1) == "q") || (temp.substr(0,1) == "Q") )
00471          {
00472             getline(inquatfile, temp);
00473             istringstream inputString(temp);
00474             inputString >> R(1,1) >> R(1,2) >> R(1,3) >> R(2,1);
00475             q = Quaternion(R(1,1), R(1,2), R(1,3), R(2,1));
00476          }
00477          else if(temp.substr(0,1) == "")
00478          {
00479          }
00480          else
00481          {
00482             cerr << "Spl_Quaternion::Spl_Quaternion: format of input file "
00483             << filename.c_str() << " is incorrect" << endl;
00484          }
00485 
00486          quat_data.insert( quat_map::value_type(time, q));
00487       }
00488    }
00489    else
00490    {
00491       cerr << "Spl_Quaternion::Spl_Quaternion: can not open file "
00492       << filename.c_str() << endl;
00493    }
00494 }
00495 
00496 
00497 Spl_Quaternion::Spl_Quaternion(const quat_map & quat)
00499 {
00500    quat_data = quat;
00501 }
00502 
00503 short Spl_Quaternion::quat(const Real t, Quaternion & s)
00511 {
00512    Real dt=0;
00513    Quaternion ds;
00514    quat_map::const_iterator iter1 = quat_data.begin(), iter2;
00515 
00516    if( t == iter1->first)
00517    {
00518       s = iter1->second;
00519       return 0;
00520    }
00521 
00522    // Point to interpolate is between the first and the second point. Use Slerp function
00523    // since there is not enough points for Squad. Use dt since Slerp and Squad functions
00524    // need t from 0 to 1.
00525    iter2 = iter1;
00526    iter2++;
00527    if( t < iter2->first)
00528    {
00529       dt = (t - iter1->first)/(iter2->first - iter1->first);
00530       s  = Slerp(iter1->second, iter2->second, dt);
00531       return 0;
00532    }
00533 
00534 
00535    // From second element to element N-2.
00536    //    for(iter1 = ++quat_data.begin(); iter1 != ----quat_data.end(); ++iter1)
00537    //    for(iter1 = ++iter1; iter1 != ----quat_data.end(); ++iter1)
00538    for(iter1 = iter2; iter1 != ----quat_data.end(); ++iter1)
00539    {
00540       iter2=iter1;
00541       iter2++;
00542       if( (t >= iter1->first) && (t < iter2->first) )
00543       {
00544          dt = (t - iter1->first)/(iter2->first - iter1->first);
00545 
00546          Quaternion qn_  = (--iter1)->second,  // q(n-1)
00547                            qn   = (++iter1)->second,  // q(n)
00548                                   qn_1 = (++iter1)->second,  // q(n+1)
00549                                          qn_2 = (++iter1)->second,  // q(n+2)
00550                                                 q_tmp;
00551          ----iter1;
00552 
00553          // Intermediate point a(n) and a(n+1)
00554          Quaternion an   = qn*(((qn.i()*qn_1).Log() + (qn.i()*qn_).Log())/-4).exp(),
00555                            an_1 = qn_1*(((qn_1.i()*qn_2).Log() + (qn_1.i()*qn).Log())/-4).exp();
00556 
00557          s  = Squad(qn, an, an_1, qn_1, dt);
00558          return 0;
00559       }
00560    }
00561 
00562    // Interpolation between last two points.
00563    iter2 = iter1; iter2++;
00564    if( (t >= iter1->first) && (t <= iter2->first) )
00565    {
00566       dt = (t - iter1->first)/(iter2->first - iter1->first);
00567       s = Slerp(iter1->second, iter2->second, dt);
00568       return 0;
00569    }
00570 
00571    cerr << "Spl_Quaternion::quat_w: t not in range." << endl;
00572    return NOT_IN_RANGE;
00573 }
00574 
00575 short Spl_Quaternion::quat_w(const Real t, Quaternion & s, ColumnVector & w)
00577 {
00578    Real dt=0;
00579    Quaternion ds;
00580    quat_map::const_iterator iter1 = quat_data.begin(), iter2;
00581 
00582    if( t == iter1->first)
00583    {
00584       s = iter1->second;
00585       w = ColumnVector(3); w = 0.0;
00586       return 0;
00587    }
00588 
00589    // Point to interpolate is between the first and the second point. Use Slerp function
00590    // since there is not enough points for Squad. Use dt since Slerp and Squad functions
00591    // need t from 0 to 1.
00592    iter2 = iter1;
00593    iter2++;
00594    if( t < iter2->first)
00595    {
00596       dt = (t - iter1->first)/(iter2->first - iter1->first);
00597       s  = Slerp(iter1->second, iter2->second, dt);
00598       ds = Slerp_prime(iter1->second, iter2->second, dt);
00599       w = Omega(s, ds);
00600 
00601       return 0;
00602    }
00603 
00604    // From second element to element N-2.
00605    //    for(iter1 = ++quat_data.begin(); iter1 != ----quat_data.end(); ++iter1)
00606    //    for(iter1 = ++iter1; iter1 != ----quat_data.end(); ++iter1)
00607    for(iter1 = iter2; iter1 != ----quat_data.end(); ++iter1)
00608    {
00609       iter2=iter1;
00610       iter2++;
00611       if( (t >= iter1->first) && (t < iter2->first) )
00612       {
00613          dt = (t - iter1->first)/(iter2->first - iter1->first);
00614 
00615          Quaternion qn_  = (--iter1)->second,  // q(n-1)
00616                            qn   = (++iter1)->second,  // q(n)
00617                                   qn_1 = (++iter1)->second,  // q(n+1)
00618                                          qn_2 = (++iter1)->second,  // q(n+2)
00619                                                 q_tmp;
00620          ----iter1;
00621 
00622          // Intermediate point a(n) and a(n+1)
00623          Quaternion an   = qn*(((qn.i()*qn_1).Log() + (qn.i()*qn_).Log())/-4).exp(),
00624                            an_1 = qn_1*(((qn_1.i()*qn_2).Log() + (qn_1.i()*qn).Log())/-4).exp();
00625 
00626          s  = Squad(qn, an, an_1, qn_1, dt);
00627          ds = Squad_prime(qn, an, an_1, qn_1, dt);
00628          w = Omega(s, ds);
00629          return 0;
00630       }
00631    }
00632 
00633    // Interpolation between last two points.
00634    iter2 = iter1; iter2++;
00635    if( (t >= iter1->first) && (t <= iter2->first) )
00636    {
00637       dt = (t - iter1->first)/(iter2->first - iter1->first);
00638       s = Slerp(iter1->second, iter2->second, dt);
00639       ds = Slerp_prime(iter1->second, iter2->second, dt);
00640       w = Omega(s, ds);
00641       return 0;
00642    }
00643 
00644    cerr << "Spl_Quaternion::quat_w: t not in range." << endl;
00645    return NOT_IN_RANGE;
00646 }
00647 
00648 
00649 // -----------------------------------------------------------------------------------------------
00650 
00651 
00652 Trajectory_Select::Trajectory_Select()
00654 {
00655     type = NONE;
00656     quaternion_active = false;
00657 }
00658 
00659 
00660 Trajectory_Select::Trajectory_Select(const string & filename)
00668 {
00669     set_trajectory(filename);
00670 }
00671 
00672 
00673 Trajectory_Select & Trajectory_Select::operator=(const Trajectory_Select & x)
00675 {
00676     type = x.type;
00677     if(x.quaternion_active)
00678     {
00679         quaternion_active = x.quaternion_active;
00680         path_quat = x.path_quat;
00681     }
00682     else
00683         path = x.path;
00684 
00685     return *this;
00686 }
00687 
00688 
00689 void Trajectory_Select::set_trajectory(const string & filename)
00691 {
00692    const char *ptr_filename = filename.c_str(); //transform string to *char
00693 
00694    std::ifstream inpointfile(ptr_filename, std::ios::in);
00695 
00696    if(inpointfile)
00697    {
00698       string temp;
00699 
00700       getline(inpointfile, temp);
00701       while(temp.substr(0,1) == "#") { // # comment
00702          getline(inpointfile, temp);
00703       }
00704 
00705       if( (temp == "JOINT_SPACE") || (temp == "CARTESIAN_SPACE") )
00706       {  
00707           path = Spl_path(filename);
00708           type = path.get_type();
00709           quaternion_active = false;
00710       }
00711       else
00712       {
00713           path_quat = Spl_Quaternion(filename);
00714           type = CARTESIAN_SPACE;
00715           quaternion_active = true;
00716       }
00717    }
00718    else
00719        cerr << "Trajectory_Select::set_trajectory: invalid input file " << filename << endl;
00720 }
00721 
00722 #ifdef use_namespace
00723 }
00724 #endif
00725 
00726 
00727 
00728 
00729 
00730 
00731 


kni
Author(s): Martin Günther
autogenerated on Thu Jun 6 2019 21:42:34