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;
}
