int Reaching::PerformPerturbedTrajectory(string *infname, string *outfname){ string line; int i,perturbationTime,n=1; cart_vec_t& tar; joint_vec_t& pos,acc; ifstream istr(infname->c_str()); string fname(*outfname); if(istr.fail()){ cout <<"can't open file "<<infname->c_str()<<endl; return 0; } else{ getline(istr,line); if(7 != sscanf(line.c_str(),"%f %f %f : %f %f %f %f ",&(tar[0]),&(tar[1]),&(tar[2]), &(pos[0]),&(pos[1]),&(pos[2]),&(pos[3]))){ #ifndef SILENT cout << "line " << n << " not decripted"<<endl; #endif return -3; } else{ if(SetTarget(tar)){ if(SetActualRobPosition(pos)){ ofstream out(fname.c_str()); while(!istr.eof()){ n++; getline(istr,line); if(5!= sscanf(line.c_str(),"%d : %f %f %f %f",&perturbationTime, &(acc[0]),&(acc[1]), &(acc[2]),&(acc[3]))){ #ifndef SILENT cout << "line " << n << " not decripted"<<endl; #endif } else{ for(i=1;i<perturbationTime;i++){ ReachingStep(); out << *this << endl; } SetRobAcceleration(acc); out << *this << endl; } } for(i=0;i<MAX_IT && !TargetReached();i++){ ReachingStep(); out << *this << endl; } out.close(); return TargetReached(); } else{ return -1; } } else{ return -2; } } } }
int Reaching::Reach(){ for (int i=0;i<MAX_IT && !TargetReached();i++){ ReachingStep(); } if(TargetReached()){ return 0; // everything ok; } else{ return 1; } }
int Reaching::StartRecordTraj(joint_vec_t& initpos,ofstream& out){ if(SetActualRobPosition(initpos)){ out << *this << endl; for (int i=0;i<MAX_IT && !TargetReached();i++){ ReachingStep(); out << *this << endl; } out.close(); if(TargetReached()){ return 0; // everything ok; } else{ return 1; } } return -1; //unvalid init position }
bool LinearSlide :: IsHoming () { switch ( State ) { case kMode_Velocity: case kMode_Position: return false; default: break; } return TargetReached ( 0.0 ); };
int Reaching::PerformTrajectories(string *infname, string *outfname){ ifstream istr(infname->c_str()); if(istr.fail()){ cout <<"can't open file "<<infname->c_str()<<endl; return 0; } else{ int timeOfJump, tar_ok,tar_spec; string fname(*outfname); string line; char nc[80]; joint_vec_t& pos, new_tar; int n=0; sprintf(nc,"%d",n); fname = *outfname + nc +".txt"; // appending the traj number to the filename ofstream out(fname.c_str()); getline(istr,line); if(4 != sscanf(line.c_str(),"%f %f %f %f \n", &(pos[0]),&(pos[1]),&(pos[2]),&(pos[3]))){ #ifndef SILENT cout << "line " << n << " not decripted"<<endl; #endif } else{ SetActualRobPosition(pos); } out << *this << endl; while(!istr.eof()){ getline(istr,line); tar_spec = sscanf(line.c_str(),"%d: %f %f %f %f\n", &timeOfJump, &(new_tar[0]),&(new_tar[1]),&(new_tar[2]), &(new_tar[3])); if ((tar_spec !=5) && (tar_spec != 4)){ #ifndef SILENT //cout << "line " << n << " not decripted"<<endl; #endif } else{ if (tar_spec ==4){ // a 3d cartesian end-effector is specified tar_ok = SetTarget(new_tar); // only the first 3 values } else{ tar_ok = SetArmConfigurationTarget(new_tar); // a 4d arm configuration is specified // coutvec4(new_tar); } if(tar_ok){ for (int i=0;i<timeOfJump && !TargetReached();i++){ ReachingStep(); #ifdef LEARNING LearningLocal(); #endif out << *this << endl; } #ifdef LEARNING LearningGlobal(); #endif } } } out.close(); if(TargetReached()){ return 1; } } return 0; }