|
GCOP
1.0
|
00001 #ifndef GCOP_UNIFORMSPLINETPARAM_H 00002 #define GCOP_UNIFORMSPLINETPARAM_H 00003 00004 #include "tparam.h" 00005 00006 namespace gcop { 00007 00008 using namespace std; 00009 using namespace Eigen; 00010 00023 template <typename T, 00024 int nx, 00025 int nu, 00026 int np = Dynamic> class UniformSplineTparam : public Tparam<T, nx, nu, np> { 00027 00028 typedef Matrix<double, nx, 1> Vectornd; 00029 typedef Matrix<double, nu, 1> Vectorcd; 00030 typedef Matrix<double, nx, nx> Matrixnd; 00031 typedef Matrix<double, nx, nu> Matrixncd; 00032 typedef Matrix<double, nu, nx> Matrixcnd; 00033 typedef Matrix<double, nu, nu> Matrixcd; 00034 00035 typedef Matrix<double, np, 1> Vectormd; 00036 00037 public: 00038 UniformSplineTparam(System<T, nx, nu, np> &sys, const VectorXd &tks, int degree = 2);//By default use quadratic spline 00039 00040 bool To(VectorXd &s, 00041 const vector<double> &ts, 00042 const vector<T> &xs, 00043 const vector<Vectorcd> &us, 00044 const Vectormd *p = 0); 00045 00046 bool From(vector<double> &ts, 00047 vector<T> &xs, 00048 vector<Vectorcd> &us, 00049 const VectorXd &s, 00050 Vectormd *p = 0); 00051 00052 const VectorXd &tks; 00053 int degree; //Degree of the spline // p = (m - n - 1) where m is knot vector size; n is the control vector size (tks size) 00054 00055 //Basis Function: 00056 void FindBasis(VectorXd &basis, double u) 00057 { 00058 assert((u <=1) && (u>=0)); 00059 assert(degree <= 4 && degree>=1);//I only have closed form solution for basis from Wiki http://www.wikiwand.com/en/Irwin%E2%80%93Hall_distribution#/Special_cases 00060 //Can put an assert for basis size not doing right now 00061 //In general it is easy to use deboor algorithm to find the basis for any n. 00062 double x; 00063 switch(degree) 00064 { 00065 case 1: 00066 basis[1] = u; 00067 x = 1+u; 00068 basis[0] = 2-x; 00069 break; 00070 case 2: 00071 basis[2] = 0.5*u*u; 00072 x = 1+u; 00073 basis[1] = 0.5*(-2*x*x + 6*x -3); 00074 x = 2+u; 00075 basis[0] = 0.5*(x*x - 6*x +9); 00076 break; 00077 case 3: 00078 basis[3] = (1.0/6.0)*u*u*u; 00079 x = 1+u; 00080 basis[2] = (1.0/6.0)*(-3*x*x*x + 12*x*x -12*x + 4); 00081 x = 2+u; 00082 basis[1] = (1.0/6.0)*(3*x*x*x - 24*x*x +60*x - 44); 00083 x = 3+u; 00084 basis[0] = (1.0/6.0)*(-1*x*x*x + 12*x*x -48*x +64); 00085 break; 00086 case 4: 00087 basis[4] = (1.0/24.0)*u*u*u*u; 00088 x = 1+u; 00089 basis[3] = (1.0/24.0)*(-4*x*x*x*x +20*x*x*x - 30*x*x + 20*x -5); 00090 x = 2+u; 00091 basis[2] = (1.0/24.0)*(6*x*x*x*x -60*x*x*x + 210*x*x - 300*x +155); 00092 x = 3+u; 00093 basis[1] = (1.0/24.0)*(-4*x*x*x*x +60*x*x*x - 330*x*x + 780*x -655); 00094 x = 4+u; 00095 basis[0] = (1.0/24.0)*(x*x*x*x -20*x*x*x + 150*x*x - 500*x +625); 00096 break; 00098 } 00099 } 00100 }; 00101 00102 template <typename T, int nx, int nu, int np> 00103 UniformSplineTparam<T, nx, nu, np>::UniformSplineTparam(System<T, nx, nu, np> &sys, const VectorXd &tks, int degree) : Tparam<T, nx, nu, np>(sys, (tks.size()+degree-1)*(sys.U.n)),degree(degree), tks(tks) { 00104 00105 assert(tks.size() >=2);//atleast 1 segments for which above constructor size is valid 00106 cout<<"Degree: "<<degree<<endl; 00107 00108 cout<<"tks: "<<tks.transpose()<<endl; 00109 } 00110 00111 template <typename T, int nx, int nu, int np> 00112 bool UniformSplineTparam<T, nx, nu, np>::To(VectorXd &s, 00113 const vector<double> &ts, 00114 const vector<T> &xs, 00115 const vector<Vectorcd> &us, 00116 const Vectormd *p) { 00117 //Find control points(uks) given us 00118 int nofcontrolpoints = (degree + this->tks.size() - 1); 00119 int tks_index = 0; 00120 00121 //Initialize variables for least square fit; Fitting each control dimension separately 00122 MatrixXd A(us.size(), nofcontrolpoints);//Basis Matrix 00123 MatrixXd Asquare(nofcontrolpoints, nofcontrolpoints);//Basis Matrix 00124 VectorXd c(us.size());//RHS 00125 A.setZero();//Initialize A 00126 VectorXd basis(degree+1); 00127 00128 //Find Basis Matrix to find the control points: 00129 for(int ind = 0;ind < us.size(); ind++) 00130 { 00131 //Find the region in which ts[i] lies: 00132 if(tks_index < (tks.size()-1)) 00133 while((ts[ind] - tks[tks_index+1])>1e-17) 00134 tks_index +=1; 00135 //tks_index = (ts[ind] - tks[tks_index+1])<-1e-17?tks_index:(tks_index+1);//1e-17 so that it considers numerical accuracy 00136 //Find the Basis for the normalized coordinate: 00137 //cout<<"index: "<<tks_index<<endl; 00138 double u = (ts[ind] - tks[tks_index])/(tks[tks_index+1] - tks[tks_index]); 00139 //cout<<"tks["<<(tks_index)<<"]: "<<tks[tks_index]<<"\t"<<tks[tks_index+1]<<endl; 00140 //cout<<"ts: "<<ts[ind]<<endl; 00141 //cout<<"u: "<<u<<endl; 00142 FindBasis(basis, u); 00143 A.block(ind, tks_index, 1, degree+1) = basis.transpose();//Create Basis Matrix 00144 } 00145 //Asquare = (A.transpose()*A); 00146 //cout<<"A: "<<endl<<A<<endl; 00147 //cout<<"Asquare: "<<endl<<Asquare<<endl; 00148 //getchar(); 00149 for(int ind = 0; ind < nu; ind++) 00150 { 00151 for(int uind = 0; uind < us.size(); uind++) 00152 { 00153 c(uind) = us[uind](ind); 00154 } 00155 Asquare = A.transpose()*A;//Weighted matrix 00156 VectorXd b = Asquare.ldlt().solve(A.transpose()*c); 00157 cout<<"Error: "<<(A*b - c).squaredNorm()<<endl; 00158 // cout<<"c: "<<c.transpose()<<endl; 00159 //cout<<"b: "<<b.transpose()<<endl; 00160 //Copy the elements back into vector s: 00161 for(int sind = 0; sind < nofcontrolpoints; sind++) 00162 { 00163 s(sind*nu + ind) = b(sind); 00164 } 00165 } 00166 cout<<"s: "<<s.transpose()<<endl; 00167 //getchar(); 00168 //Verify if this s is good: 00169 /* 00170 { 00171 tks_index = 0; 00172 Vectorcd usi; 00173 for (int i = 0; i < us.size(); ++i) { 00174 //Find the region in which ts[i] lies: 00175 if(tks_index < (tks.size()-1)) 00176 tks_index = (ts[i] - tks[tks_index+1])<-1e-17?tks_index:(tks_index+1); 00177 //Find the Basis for the normalized coordinate: 00178 double u = (ts[i] - tks[tks_index])/(tks[tks_index+1] - tks[tks_index]); 00179 FindBasis(basis, u); 00180 usi.setZero(); 00181 //Weight the spline using the basis: 00182 for(int degree_count = 0; degree_count <= degree; degree_count++) 00183 { 00184 usi += basis[degree_count]*s.segment((tks_index+degree_count)*nu,nu); 00185 } 00186 cout<<"Basis: "<<basis.transpose()<<endl; 00187 00188 cout<<"us_pred["<<i<<"]: "<<usi.transpose()<<"us_act: "<<us[i].transpose()<<endl; 00189 } 00190 } 00191 getchar(); 00192 */ 00193 00194 //s.setZero(); 00195 return true; 00196 } 00197 00198 template <typename T, int nx, int nu, int np> 00199 bool UniformSplineTparam<T, nx, nu, np>::From(vector<double> &ts, 00200 vector<T> &xs, 00201 vector<Vectorcd> &us, 00202 const VectorXd &s, 00203 Vectormd *p) { 00204 assert(s.size() == (degree + this->tks.size() - 1)*(this->sys.U.n)); 00205 00206 this->sys.Reset(xs[0],ts[0]); 00207 int tks_index = 0; 00208 VectorXd basis(degree+1); 00209 Vectorcd usi; 00210 for (int i = 0; i < us.size(); ++i) { 00211 //Find the region in which ts[i] lies: 00212 if(tks_index < (tks.size()-1)) 00213 while((ts[i] - tks[tks_index+1])>1e-17) 00214 tks_index +=1; 00215 //tks_index = (ts[i] - tks[tks_index+1])<-1e-17?tks_index:(tks_index+1); 00216 //Find the Basis for the normalized coordinate: 00217 double u = (ts[i] - tks[tks_index])/(tks[tks_index+1] - tks[tks_index]); 00218 FindBasis(basis, u); 00219 us[i].setZero(); 00220 //Weight the spline using the basis: 00221 for(int degree_count = 0; degree_count <= degree; degree_count++) 00222 { 00223 //us[i] += basis[degree_count]*s.segment((tks_index+degree_count)*(this->sys.U.n), this->sys.U.n); 00224 us[i] += basis[degree_count]*s.segment((tks_index+degree_count)*nu,nu); 00225 } 00226 00227 // cout<<"ts["<<i<<"]: "<<ts[i]<<endl; 00228 //cout<<"us["<<i<<"]: "<<us[i].transpose()<<"ts: "<<ts[i]<<endl; 00229 this->sys.Step(xs[i+1], us[i], ts[i+1] - ts[i], p); 00230 //cout<<"Xs["<<(i+1)<<"]"<<xs[i+1].transpose()<<endl;//#DEBUG 00231 //cout<<"us["<<i<<"]"<<us[i].transpose()<<endl; 00232 } 00233 return true; 00234 //getchar(); 00235 } 00236 } 00237 00238 #endif
1.7.6.1