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
00041 #ifdef use_namespace
00042 namespace ROBOOP {
00043 using namespace NEWMAT;
00044 #endif
00045
00046
00047 static const char rcsid[] __UNUSED__ = "$Id: trajectory.cpp,v 1.5 2005/07/26 03:22:08 ejt Exp $";
00048
00049
00050 Spl_cubic::Spl_cubic(const Matrix & pts)
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
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);
00091
00092 for(int i = 1; i <= N-1; i++)
00093 {
00094 delta(i) = pts(1,i+1) - pts(1,i);
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);
00103 }
00104
00105 Matrix A(N-2, N-2); A = 0;
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;
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;
00125
00126 ColumnVector dd_s(N);
00127 dd_s(1) = dd_s(N) = 0;
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
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
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
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
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
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
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
00246
00247 Spl_path::Spl_path(const string & filename)
00248
00249
00250
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 const char *ptr_filename = filename.c_str();
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) == "#") {
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
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
00399
00400
00401 Spl_path::Spl_path(const Matrix & pts): Spl_cubic(pts)
00402 {
00403 }
00404
00405
00406
00407
00408
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
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
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
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
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
00478
00479
00480 Spl_Quaternion::Spl_Quaternion(const string & filename)
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506 {
00507 const char *ptr_filename = filename.c_str();
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
00602 {
00603 quat_data = quat;
00604 }
00605
00606
00607 Spl_Quaternion::Spl_Quaternion(const Spl_Quaternion & x)
00608
00609 {
00610 quat_data = x.quat_data;
00611 }
00612
00613
00614 Spl_Quaternion & Spl_Quaternion::operator=(const Spl_Quaternion & x)
00615
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
00626
00627
00628
00629
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
00643
00644
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
00656
00657
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,
00667 qn = (++iter1)->second,
00668 qn_1 = (++iter1)->second,
00669 qn_2 = (++iter1)->second,
00670 q_tmp;
00671 ----iter1;
00672
00673
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
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
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
00710
00711
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
00725
00726
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,
00736 qn = (++iter1)->second,
00737 qn_1 = (++iter1)->second,
00738 qn_2 = (++iter1)->second,
00739 q_tmp;
00740 ----iter1;
00741
00742
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
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
00774 {
00775 type = NONE;
00776 quaternion_active = false;
00777 }
00778
00779
00780 Trajectory_Select::Trajectory_Select(const string & filename)
00781
00782
00783
00784
00785
00786
00787
00788 {
00789 set_trajectory(filename);
00790 }
00791
00792
00793 Trajectory_Select & Trajectory_Select::operator=(const Trajectory_Select & x)
00794
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
00811 {
00812 const char *ptr_filename = filename.c_str();
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) == "#") {
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