00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
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
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
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
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);
00094
00095 for(int i = 1; i <= N-1; i++)
00096 {
00097 delta(i) = pts(1,i+1) - pts(1,i);
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);
00106 }
00107
00108 Matrix A(N-2, N-2); A = 0;
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;
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;
00128
00129 ColumnVector dd_s(N);
00130 dd_s(1) = dd_s(N) = 0;
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
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
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
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
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
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
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
00249
00250 Spl_path::Spl_path(const string & filename)
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290 {
00291 const char *ptr_filename = filename.c_str();
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) == "#") {
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
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
00402
00403
00404 Spl_path::Spl_path(const Matrix & pts): Spl_cubic(pts)
00405 {
00406 }
00407
00408
00409
00410
00411
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
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
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
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
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
00481
00482
00483 Spl_Quaternion::Spl_Quaternion(const string & filename)
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509 {
00510 const char *ptr_filename = filename.c_str();
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
00605 {
00606 quat_data = quat;
00607 }
00608
00609
00610 Spl_Quaternion::Spl_Quaternion(const Spl_Quaternion & x)
00611
00612 {
00613 quat_data = x.quat_data;
00614 }
00615
00616
00617 Spl_Quaternion & Spl_Quaternion::operator=(const Spl_Quaternion & x)
00618
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
00629
00630
00631
00632
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
00646
00647
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
00659
00660
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,
00670 qn = (++iter1)->second,
00671 qn_1 = (++iter1)->second,
00672 qn_2 = (++iter1)->second,
00673 q_tmp;
00674 ----iter1;
00675
00676
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
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
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
00713
00714
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
00728
00729
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,
00739 qn = (++iter1)->second,
00740 qn_1 = (++iter1)->second,
00741 qn_2 = (++iter1)->second,
00742 q_tmp;
00743 ----iter1;
00744
00745
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
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
00777 {
00778 type = NONE;
00779 quaternion_active = false;
00780 }
00781
00782
00783 Trajectory_Select::Trajectory_Select(const string & filename)
00784
00785
00786
00787
00788
00789
00790
00791 {
00792 set_trajectory(filename);
00793 }
00794
00795
00796 Trajectory_Select & Trajectory_Select::operator=(const Trajectory_Select & x)
00797
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
00814 {
00815 const char *ptr_filename = filename.c_str();
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) == "#") {
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