void Cubic_spline::findCutoffs() { //Find the cutoff lengths for each function. rcut.Resize(nfunctions); int totfunc=0; for(int i=0; i < nsplines; i++) { int nrep=0; switch(symmetry(i)) { case sym_S: nrep=1; break; case sym_P: case sym_P_siesta: nrep=3; break; case sym_5D: case sym_D_siesta: nrep=5; break; case sym_6D: nrep=6; break; case sym_7F: case sym_7F_crystal://CRYSTAL F orbital case sym_F_siesta: nrep=7; break; case sym_10F: nrep=10; break; case sym_9G: nrep=9; break; case sym_15G: nrep=15; break; default: error("I don't have this symmetry:", symmetry(i)); } doublevar cutoff=splines(i).findCutoff(); for(int j=0; j< nrep; j++) { rcut(totfunc++)=cutoff; } } threshold=0; for(int i=0; i< rcut.GetDim(0); i++) { if(threshold < rcut(i)) threshold=rcut(i); } for(int i=0; i< nsplines; i++) { splines(i).pad(threshold); } }
//------------------------------------------------------------------------- void Cubic_spline::raw_input(ifstream & input) { splines.Resize(nsplines); for(int s=0; s< nsplines; s++) { splines(s).raw_input(input); } }
//------------------------------------------------------------------------- int Cubic_spline::readspline(vector <string> & words) { unsigned int pos=0; vector <vector <string> > splinefits; vector <string> onesection; while(readsection(words, pos, onesection, "SPLINE")) { splinefits.push_back(onesection); } if(splinefits.size()==0) { return 0; } nsplines=splinefits.size(); splines.Resize(nsplines); symmetry.Resize(nsplines); for(int i=0; i< nsplines; i++) { symmetry(i)=symmetry_lookup(splinefits[i][0]); //splinefits[i].erase(splinefits[i].begin()); } assign_indiv_symmetries(); //cout << "created " << nfunc() << " total functions " << endl; double max_support=0.0; for(int i=0; i< nsplines; i++) { int n=splinefits[i].size(); double supp=atof(splinefits[i][n-2].c_str()); if(supp > max_support) max_support=supp; } //cout << "maximum support " << max_support <<endl; for(int s=0; s< nsplines; s++) { splines(s).readspline(splinefits[s], enforce_cusp, cusp/double(symmetry_lvalue(symmetry(s))+1)); if(requested_cutoff > 0) { splines(s).enforceCutoff(requested_cutoff); } } findCutoffs(); return nfunc(); }
int Cubic_spline::writeinput(string & indent, ostream & os) { os << indent << atomname << endl; os << indent << "AOSPLINE \n"; os << indent << "NORMTYPE " << norm_type << endl; if(renormalize==false) { os << indent << "NORENORMALIZE\n"; } if(zero_derivative) { os << indent << "ZERO_DERIVATIVE\n"; } if(customspacing!=0.02) os << indent << "SPACING "<<customspacing<<endl; if(requested_cutoff > 0 ) { os << indent << "CUTOFF " << requested_cutoff << endl; } if(enforce_cusp) { os << indent << "CUSP " << cusp << endl; os << indent << "CUSP_MATCHING " << cusp_matching << endl; } if(exponent.size() > 0) { os << indent << "GAMESS { \n"; for(int funcNum=0; funcNum<nsplines; funcNum++) { os << indent << symmetry_lookup(symmetry(funcNum)) << endl; os << indent << exponent[funcNum].size() << endl; for(unsigned int i=0; i < exponent[funcNum].size(); i++) { os << indent << " " << i << " " << exponent[funcNum][i] << " " << coefficient[funcNum][i] << endl; } } os << indent << " } \n"; } else { for(int funcNum=0; funcNum < nsplines; funcNum++) { os << indent << "SPLINE { \n"; os << indent << symmetry_lookup(symmetry(funcNum)) << endl; splines(funcNum).writeinput(indent, os); os << indent << " }\n"; } } return 1; }
/*! This takes a vector of words presumably gotten from an input file and parses them into a basis function. It takes something very similar to GAMESS input, but it does not allow you to have a normalization constant.(ie, all function headers should be like S 1, not S 1 1.0) It will normalize it anyway. */ int Cubic_spline::readbasis(vector <string> & words,unsigned int & pos, Array1 <double> & parms){ doublevar spacing=parms(0); int n= (int) parms(1); double yp1=parms(2); double ypn=parms(3); int symmtype=0; if(parms.GetDim(0) > 4) symmtype=(int) parms(4); assert(symmtype==0 || symmtype==1); string symm; int ngauss; vector <symmetry_type> symmetry_temp; for(unsigned int p=pos; p<words.size(); p++) { symm=words[p]; ngauss=atoi(words[++p].c_str()); symmetry_temp.push_back(symmetry_lookup(symm)); vector <doublevar> exp(ngauss); vector <doublevar> c(ngauss); if(p+ngauss*3 >= words.size()) { error("Unexpected end of GAMESS section. Count is wrong."); } for(int i=0; i<ngauss; i++) { p++; exp[i]=atof(words[++p].c_str()); c[i]=atof(words[++p].c_str()); } exponent.push_back(exp); coefficient.push_back(c); } nsplines=exponent.size(); Array1 <doublevar> y(n); Array1 <doublevar> x(n); splines.Resize(nsplines); symmetry.Resize(nsplines); for(int i=0; i< nsplines; i++) { symmetry(i)=symmetry_temp[i]; } //cout << "nfunc " << nfunc() << endl; const double normtol=1e-5; //tolerance for the normalization of the basis fn for(int funcNum=0; funcNum<nsplines; funcNum++) { //calculate interpolation points for(int j=0; j<n; j++) { x(j)=j*spacing; } y=0; for(unsigned int i=0; i < exponent[funcNum].size(); i++) { doublevar norm; if(norm_type=="GAMESSNORM") { doublevar fac=sqrt(2.*exponent[funcNum][i]/pi); doublevar feg=4.*exponent[funcNum][i]; doublevar feg2=feg*feg; switch(symmetry(funcNum)) { case sym_S: norm=sqrt(2.*feg*fac); break; case sym_P: norm=sqrt(2.*feg2*fac/3.); break; case sym_5D: case sym_6D: norm=sqrt(2.*feg*feg2*fac/15.); break; case sym_7F: case sym_7F_crystal://CRYSTAL F orbital case sym_10F: norm=sqrt(2.*feg2*feg2*fac/105.); break; case sym_9G: case sym_15G: norm=sqrt(2.*feg2*feg2*feg*fac/945.); //Lubos-done break; /* general formula here with n principal quantum numbers norm=sqrt[(2 ((4exponent)**n) sqrt(2exponent/pi))/(2n-1)!!] */ default: norm=0; error("Unknown symmetry in Cubic_spline::readbasis! Shouldn't be here!"); } } else if(norm_type=="CRYSTAL") { switch(symmetry(funcNum)) { case sym_S: norm=1; break; case sym_P: norm=1/sqrt(3.0); break; case sym_5D: case sym_6D: norm=1/sqrt(5.0); break; case sym_7F: case sym_7F_crystal://CRYSTAL F orbital case sym_10F: norm=1/sqrt(7.0); break; case sym_9G: case sym_15G: norm=1/sqrt(9.0); break; default: norm=0; error("unknown symmetry in cubic_spline: ", symmetry(funcNum)); } } else if(norm_type=="NONE") { norm=1; } else { norm=0; error("Didn't understand NORMTYPE ", norm_type); } for(int j=0; j<n; j++) { y(j)+=norm*coefficient[funcNum][i] *exp(-exponent[funcNum][i]*x(j)*x(j)); } } //Renormalize to one if(renormalize) { //check normalization doublevar sum=0; for(int j=0; j<n; j++) { int p=0; switch(symmetry(funcNum)) { case sym_S: p=1; break; case sym_P: p=2; break; case sym_5D: case sym_6D: p=3; break; case sym_7F: case sym_7F_crystal://CRYSTAL F orbital case sym_10F: p=4; break; case sym_9G: case sym_15G: p=5; break; default: error("unknown symmetry when checking the normalization"); } //doublevar power=pow(x(j), 2*(symmetry(funcNum)+1) ); doublevar power=pow(x(j), 2*p ); sum+=spacing*power*y(j)*y(j); } //cout << "normalization : " << sqrt(sum) << endl; //renormalize if needed if(fabs(sum-1) > normtol){ //debug_write(cout, "Normalizing basis function, sum: ", sum, "\n"); for(int j=0; j<n; j++){ y(j)=y(j)/sqrt(sum); } } } //Force the function to go to zero if the user requested a cutoff if(requested_cutoff > 0) { const double smooth=1.2; const double cutmax=requested_cutoff-1e-6; const double cutmin=cutmax-smooth; for(int j=0; j< n; j++) { if(x(j) > cutmin) { if(x(j) > cutmax) { //if we're beyond the cutoff completely y(j)=0; } else { //if we're in the smooth cutoff region double zz=(x(j)-cutmin)/smooth; y(j) *= (1-zz*zz*zz*(6*zz*zz-15*zz+10)); } } } } //--Estimate the derivatives on the boundaries if ( zero_derivative ) { yp1=0.0; } else { yp1=(y(1)-y(0))/spacing; } ypn=(y(n-1) - y(n-2) ) /spacing; if(enforce_cusp) { double der=cusp/double(symmetry_lvalue(symmetry(funcNum))+1); yp1=der; // KMR: using an STO near the atom should be a separate decision from // passing the first derivative to the spliner if(match_sto) { //inspired from J. Chem. Phys. 130, 114107 (2009) //Here, we match the value, first derivative, and second derivative to the //given smooth function at some correction radius rc. We can then safely //replace the function from [0:rc] with the Slater function. //cout << "enforcing cusp" << endl; double rc=cusp_matching; int closest=rc/spacing; rc=x(closest); //cout << "rc " << rc << endl; double curve=(y(closest+1)+y(closest-1)-2*y(closest))/(spacing*spacing); double deriv=(y(closest+1)-y(closest-1))/(2*spacing); double f=y(closest); double b=(deriv*der*der-curve*der)/(curve*der*rc*rc+curve*rc*2-deriv*der*der*rc*rc-deriv*4*der*rc-2*deriv); double a=curve/(exp(der*rc)*(der*der*(1+b*rc*rc)+4*der*b*rc+2*b)); double c=f-a*exp(der*rc)*(1+b*rc*rc); //cout << "a " << a << " b " << b << " c " << c << endl; for(int j=0; j <= closest; j++) { y(j)=a*exp(der*x(j))*(1+b*x(j)*x(j))+c; } } } /* for(int j=0; j< n; j++) { cout << "jjjjjjjj " << x(j) << " " << y(j) << endl; } cout << "jjjjjjj " << endl; */ splines(funcNum).splinefit(x,y,yp1, ypn); } nfunctions=nfunc(); assign_indiv_symmetries(); findCutoffs(); return nfunctions; }
//------------------------------------------------------------------------- int Cubic_spline::read( vector <string> & words, unsigned int & pos ) { Array1 <double> basisparms(4); atomname=words[0]; basisparms(0)=.02; //spacing basisparms(1)=2500; //number of points basisparms(2)=1e31; basisparms(3)=1e31; if(!readvalue(words, pos=0, norm_type, "NORMTYPE")) { norm_type="GAMESSNORM"; } if(haskeyword(words, pos=0, "NORENORMALIZE")) { renormalize=false; } else { renormalize=true; } doublevar cutmax; if(readvalue(words, pos=0, cutmax, "CUTOFF")) { requested_cutoff=cutmax; } else { requested_cutoff=-1; } enforce_cusp=false; if(readvalue(words, pos=0, cusp, "CUSP")) enforce_cusp=true; match_sto=false; if(readvalue(words, pos=0, cusp_matching, "CUSP_MATCHING")) match_sto=true; zero_derivative=false; if(haskeyword(words, pos=0, "ZERO_DERIVATIVE")) zero_derivative=true; customspacing=basisparms(0); if(readvalue(words, pos=0, customspacing, "SPACING")) { basisparms(0)=customspacing; basisparms(1)=50.0/customspacing; } vector <string> basisspec; string read_file; //read positions from a file if(readsection(words,pos=0, basisspec, "GAMESS")) { unsigned int newpos=0; return readbasis(basisspec, newpos, basisparms); } else { pos=0; if(readspline(words)) { return 1; } else { error("couldn't find proper basis section in AOSPLINE. " "Try GAMESS { .. }."); return 0; } } //We assume that all splines use the same interval, //which saves a few floating divisions, so check to make //sure the assumption is good. for(int i=0; i< splines.GetDim(0); i++) { if(!splines(0).match(splines(i))) error("spline ", i, " doesn't match the first one "); } }
Array<SmoothLinearSpline> Trajectory<JointAngles>::generateLinearSplines() throw (DataException<int> ) { // n = number of segments // n+1 = number of input waypoints // n+3 = number of waypoints (inc. pre-post pseudo) // This is a bit different from the cubic as we don't use this in a combo interpolater. Array<SmoothLinearSpline> splines(dimension()); unsigned int n = waypoints.size() - 1; // n = number of segments Array<double> waypoint_times(n + 3), values(n + 3); // Including pre and post points /****************************************** ** Check rates are set for head/tail *******************************************/ // Note that we may yet change values[1] and values[n+1] when fixing the pseudo points below // Set them all to zero (default option, i.e. starting at rest). if (!waypoints[0].rates_configured()) { waypoints[0].rates() = WayPoint<JointAngles>::JointDataArray::Constant(waypoints[0].dimension(), 0.0); } if (!waypoints[n].rates_configured()) { waypoints[n].rates() = WayPoint<JointAngles>::JointDataArray::Constant(waypoints[n].dimension(), 0.0); } /****************************************** ** Make the pre pseudo point. *******************************************/ /* * Pull back the first waypoint, find intersection of initial tangent line and the nominal rate line. * Why? This lets us ensure the first segment is of the specified velocity, and the second segment will * fall the correct between nominal rates required to get to w_1. * * x_ * ^ \__ * / \ * o ------ o * / * / * x * * - diagonal lines : nominal rate slopes * - x : possible pseudo waypoints (at intersection of initial velocity line and nominal rate line) * * Basic process: * * - pullback a bit * - get intersections as above * - find minimum time intersection time (t_first_duration) * - synchronise a pseudo waypoint at that time on the initial tangents for each joint (only 1 of these will be an actual intersection point) * - check acceleration constraints of quintics for this waypoint at each joint * - if fail, pullback again, if success, done! */ std::vector<LinearFunction> initial_tangents; // these are the lines from potential pseudo point to w_1, our eventual pseudo point must lie on it. std::vector<double> initial_angles, initial_rates; for (unsigned int j = 0; j < dimension(); ++j) { initial_angles.push_back(waypoints[0].angles()[j]); initial_rates.push_back(waypoints[0].rates()[j]); initial_tangents.push_back(LinearFunction::PointSlopeForm(0, initial_angles[j], initial_rates[j])); } std::vector<CartesianPoint2d, Eigen::aligned_allocator<CartesianPoint2d> > pseudo_points(dimension()); std::vector<LinearFunction> nominal_rate_lines(dimension()); WayPoint<JointAngles> pre_pseudo_waypoint(dimension()); double t_pullback = 0.001; // start with 1ms double t_pullback_max_ = waypoints[0].duration(); double t_pre_intersect_duration = 0.0; // duration from the pulled back wp0 to the intersection point bool acceleration_constraint_broken = true; while (acceleration_constraint_broken && (t_pullback <= t_pullback_max_)) { // establish t_pre_intersect_duration as the minimum time to the intersection of initial tangent // and nominal rate line t_pre_intersect_duration = t_pullback_max_ / 2; for (unsigned int j = 0; j < dimension(); ++j) { LinearFunction nominal_rate_line = LinearFunction::PointSlopeForm( t_pullback, initial_angles[j], -1 * ecl::psign(initial_rates[j]) * waypoints[0].nominalRates()[j]); CartesianPoint2d pseudo_point = LinearFunction::Intersection(initial_tangents[j], nominal_rate_line); if (pseudo_point.x() < t_pre_intersect_duration) { t_pre_intersect_duration = pseudo_point.x(); } nominal_rate_lines[j] = nominal_rate_line; } acceleration_constraint_broken = false; for (unsigned int j = 0; j < dimension(); ++j) { CartesianPoint2d pseudo_point(t_pre_intersect_duration, initial_tangents[j](t_pre_intersect_duration)); // from the pseudo way point to the next way point (wp1) double pseudo_point_duration = t_pullback + waypoints[0].duration() - t_pre_intersect_duration; // Will test 5 positions, which cover all of t_pre_intersect_duration // and half of pseudo_point_duration for (unsigned int i = 1; i <= 5; ++i) { double t_l = t_pre_intersect_duration - i * t_pre_intersect_duration / 5.0; double t_r = t_pre_intersect_duration + i * pseudo_point_duration / 10.0; LinearFunction segment = LinearFunction::Interpolation(pseudo_point.x(), pseudo_point.y(), t_pullback + waypoints[0].duration(), waypoints[1].angles()[j]); double y_0 = initial_tangents[j](t_l); double y_0_dot = initial_tangents[j].derivative(t_l); double y = segment(t_r); double y_dot = segment.derivative(t_r); // slope QuinticPolynomial quintic = QuinticPolynomial::Interpolation(t_l, y_0, y_0_dot, 0.0, t_r, y, y_dot, 0.0); if ((fabs(CubicPolynomial::Maximum(t_l, t_r, quintic.derivative().derivative())) < fabs(max_accelerations[j])) && (fabs(CubicPolynomial::Minimum(t_l, t_r, quintic.derivative().derivative())) < fabs(max_accelerations[j]))) { acceleration_constraint_broken = false; break; // we're good, get out and continue through all the joints } if (i == 5) { #ifdef DEBUG_LINEAR_INTERPOLATION std::cout << "Linear Interpolation: pre psuedo point failed with acceleration checks [" << t_pre_intersect_duration << "][" << t_pullback << "]" << std::endl; #endif acceleration_constraint_broken = true; } } } t_pullback = t_pullback * 2; // takes 11 runs to get from 1ms to 1s; adjust, if this is too much } if (acceleration_constraint_broken) { throw DataException<int>(LOC, ConstructorError, "Max acceleration bound broken by pre pseudo point.", 0); } pre_pseudo_waypoint.duration(t_pullback + waypoints[0].duration() - t_pre_intersect_duration); for (unsigned int j = 0; j < dimension(); ++j) { pre_pseudo_waypoint.angles()[j] = initial_tangents[j](t_pre_intersect_duration); } #ifdef DEBUG_LINEAR_INTERPOLATION std::cout << "Linear Interpolation: pre psuedo point done!" << std::endl; std::cout << " : angles " << pre_pseudo_waypoint.angles() << std::endl; std::cout << " : pre pseudo duration " << t_pre_intersect_duration << std::endl; std::cout << " : modified first waypoint duration " << pre_pseudo_waypoint.duration() << std::endl; #endif /****************************************** ** Make the post pseudo point. *******************************************/ std::vector<double> final_angles, final_rates; for (unsigned int j = 0; j < dimension(); ++j) { final_angles.push_back(waypoints[n].angles()[j]); final_rates.push_back(waypoints[n].rates()[j]); nominal_rate_lines[j] = LinearFunction::PointSlopeForm( 0, final_angles[j], -1 * ecl::psign(final_rates[j]) * waypoints[n - 1].nominalRates()[j]); } std::vector<LinearFunction> final_tangents(dimension()); WayPoint<JointAngles> post_pseudo_waypoint(dimension()); double t_pullforward = 0.001; // start with 1ms double t_pullforward_max_ = waypoints[n - 1].duration(); double t_post_intersect_duration = 0.0; // duration from the pulled back wp0 to the intersection point acceleration_constraint_broken = true; while (acceleration_constraint_broken && (t_pullforward <= t_pullforward_max_)) { // establish t_final as the maximum time to the intersection of final tangent and nominal rate line t_post_intersect_duration = 0.0; for (unsigned int j = 0; j < dimension(); ++j) { LinearFunction final_tangent = LinearFunction::PointSlopeForm(t_pullforward, final_angles[j], final_rates[j]); final_tangents[j] = final_tangent; CartesianPoint2d pseudo_point = LinearFunction::Intersection(final_tangents[j], nominal_rate_lines[j]); if (pseudo_point.x() > t_post_intersect_duration) { t_post_intersect_duration = pseudo_point.x(); } } // check acceleration constraints acceleration_constraint_broken = false; for (unsigned int j = 0; j < dimension(); ++j) { CartesianPoint2d pseudo_point(t_post_intersect_duration, final_tangents[j](t_post_intersect_duration)); double pseudo_point_duration = t_pullforward - t_post_intersect_duration; for (unsigned int i = 1; i <= 5; ++i) { double t_l = t_post_intersect_duration - i * (waypoints[n - 1].duration() + t_post_intersect_duration) / 10.0; double t_r = t_post_intersect_duration + i * pseudo_point_duration / 5.0; LinearFunction segment = LinearFunction::Interpolation(-waypoints[n - 1].duration(), waypoints[n - 1].angles()[j], pseudo_point.x(), pseudo_point.y()); double y_0 = segment(t_l); double y_0_dot = segment.derivative(t_l); double y = final_tangents[j](t_r); double y_dot = final_tangents[j].derivative(t_r); QuinticPolynomial quintic = QuinticPolynomial::Interpolation(t_l, y_0, y_0_dot, 0.0, t_r, y, y_dot, 0.0); if ((fabs(CubicPolynomial::Maximum(t_l, t_r, quintic.derivative().derivative())) < fabs(max_accelerations[j])) && (fabs(CubicPolynomial::Minimum(t_l, t_r, quintic.derivative().derivative())) < fabs(max_accelerations[j]))) { acceleration_constraint_broken = false; break; // we're good, get out and continue through all the joints } if (i == 5) { #ifdef DEBUG_LINEAR_INTERPOLATION std::cout << "Linear Interpolation: post psuedo point failed with acceleration checks [" << t_post_intersect_duration << "][" << t_pullforward << "]" << std::endl; #endif acceleration_constraint_broken = true; } } } t_pullforward = t_pullforward * 2; // takes 12 runs to get from 1ms to 1s, adjust, if this is too much } if (acceleration_constraint_broken) { throw DataException<int>(LOC, ConstructorError, "Max acceleration bound broken by pre pseudo point.", 0); } post_pseudo_waypoint.duration(t_pullforward - t_post_intersect_duration); for (unsigned int j = 0; j < dimension(); ++j) { post_pseudo_waypoint.angles()[j] = final_tangents[j](t_post_intersect_duration); } #ifdef DEBUG_LINEAR_INTERPOLATION std::cout << "Linear Interpolation: post psuedo point done!" << std::endl; std::cout << " : angles " << post_pseudo_waypoint.angles() << std::endl; std::cout << " : modified last point duration " << waypoints[n-1].duration() + t_post_intersect_duration << std::endl; std::cout << " : post pseudo duration " << post_pseudo_waypoint.duration() << std::endl; #endif /********************* ** Waypoint Times **********************/ // n+3 points (w_0...w_n + pre and post pseudos) // n+3 waypoint_times waypoint_times[0] = 0.0; waypoint_times[1] = t_pre_intersect_duration; waypoint_times[2] = t_pre_intersect_duration + pre_pseudo_waypoint.duration(); for (unsigned int i = 2; i < n; ++i) { waypoint_times[i + 1] = waypoint_times[i] + waypoints[i - 1].duration(); } waypoint_times[n + 1] = waypoint_times[n] + waypoints[n - 1].duration() + t_post_intersect_duration; waypoint_times[n + 2] = waypoint_times[n + 1] + post_pseudo_waypoint.duration(); #ifdef DEBUG_LINEAR_INTERPOLATION std::cout << "Linear Interpolation: waypoint time estimates before interpolation: " << waypoint_times << std::endl; #endif for (unsigned int j = 0; j < dimension(); ++j) { /****************************************** ** Set Values *******************************************/ values[0] = waypoints[0].angles()[j]; values[1] = pre_pseudo_waypoint.angles()[j]; for (unsigned int i = 2; i <= n; ++i) { values[i] = waypoints[i - 1].angles()[j]; } values[n + 1] = post_pseudo_waypoint.angles()[j]; values[n + 2] = waypoints[n].angles()[j]; #ifdef DEBUG_LINEAR_INTERPOLATION std::cout << "Linear Interpolation: values[" << j << "]: " << values << std::endl; #endif /****************************************** ** Generate Spline *******************************************/ try { splines[j] = SmoothLinearSpline::Interpolation(waypoint_times, values, max_accelerations[j]); } catch (DataException<int> &e) { throw DataException<int>(LOC, e); } } #ifdef DEBUG_LINEAR_INTERPOLATION for ( unsigned int j = 0; j < dimension(); ++j) { std::cout << "Linear Interpolation: discretised domain [" << j << "]: " << splines[j].domain() << std::endl; } #endif return splines; }