void Sm_Approx::approximate(double jmax,double amax,double vmax,double sampTime, double ErrMax,
int ExpTime, bool flagExport, std::string fileName, SM_TRAJ &traj)
{
this->approximate(jmax, amax, vmax, sampTime, ErrMax, ExpTime, flagExport, fileName);
traj.clear();
return;
}
int executeCB(const soft_move_base::SoftMoveBaseGoalConstPtr &goal)
{
ROS_INFO("Received new trajectory");
SM_TRAJ tmpTraj;
ros::Rate _loop_rate(1/_dt);
_timeFromStart= 0;
_tmpTimeFromStart= 0;
_timer = 0;
loadNewTraj(goal->traj);
double duration = 0.0;
duration = _newTraj.getDuration();
limits.clear();
for(int i=0; i<6; ++i)
{
limit.maxJerk = _newTraj.jmax[i] * 1.1;
limit.maxAcc = _newTraj.amax[i] * 1.1;
limit.maxVel = _newTraj.vmax[i] * 1.1;
limits.push_back(limit);
}
//double t = 0;
//FILE* file;
//file = fopen("baseExperiment.dat", "w");
bool cont = true;
do {
// end if goal canceled
if( as_.isPreemptRequested())
break;
getNextRobotCond(_timeFromStart + _dt);
//printCond(_nextRobotCond_w);
getPoseInRobotFrame(_nextRobotCond_w);
//printCond(_nextRobotCond_r);
_currRobotCond_r[0].x = 0;
_currRobotCond_r[0].v = _lastVel[0];
_currRobotCond_r[0].a = _lastAcc[0];
_currRobotCond_r[1].x = 0;
_currRobotCond_r[1].v = _lastVel[1];
_currRobotCond_r[1].a = _lastAcc[1];
_currRobotCond_r[5].x = 0;
_currRobotCond_r[5].v = _lastVel[5];
_currRobotCond_r[5].a = _lastAcc[5];
//printCond(_currRobotCond_r);
ROS_INFO("TMP TIME:%f", _tmpTimeFromStart);
if( _timeFromStart == 0.0)
{
//correction
tmpTraj.clear();
boundsCond(_currRobotCond_r);
boundsCond(_nextRobotCond_r);
tmpTraj.computeTraj(_currRobotCond_r, _nextRobotCond_r, limits, SM_TRAJ::SM_INDEPENDANT);
_tmpTimeFromStart = _dt;
//printCond( _newRobotCond_r);
}
else if(_timer - 0.5 >= 0.0001
|| _tmpTimeFromStart > tmpTraj.getDuration())
{
_timer = 0;
//correction
tmpTraj.clear();
tmpTraj.computeTraj(_currRobotCond_r, _nextRobotCond_r, limits, SM_TRAJ::SM_INDEPENDANT);
_tmpTimeFromStart = _dt;
}
tmpTraj.getMotionCond(_tmpTimeFromStart,_newRobotCond_r);
_lastVel[0]= _newRobotCond_r[0].v;
_lastVel[1]= _newRobotCond_r[1].v;
_lastVel[5]= _newRobotCond_r[5].v;
_lastAcc[0]= _newRobotCond_r[0].a;
_lastAcc[1]= _newRobotCond_r[1].a;
_lastAcc[5]= _newRobotCond_r[5].a;
//printCond(_newRobotCond_r);
publishNewCond(_newRobotCond_r);
_timeFromStart += _dt;
_tmpTimeFromStart += _dt;
_timer += _dt;
if(_timeFromStart > _newTraj.getDuration())
_timeFromStart = _newTraj.getDuration();
_loop_rate.sleep();
if(_timeFromStart >= _newTraj.getDuration())
{
cont = fabs(_newRobotCond_r[0].x) > _tolerance[0]
|| fabs(_newRobotCond_r[1].x) > _tolerance[1]
|| fabs(_newRobotCond_r[5].x) > _tolerance[2];
}
ros::spinOnce();
} while(cont);
soft_move_base::SoftMoveBaseResult result;
result.result = true;
as_.setSucceeded(result);
return 0;
}
