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;
}
void getNextRobotCond(double time)
{
_newTraj.getMotionCond(time, _nextRobotCond_w);
}
int Sm_Approx::approximate(Sm_Curve &curv, double SampTime, double ErrPosMax, double ErrVelMax, int ExpTime, bool flagExport, std::string fileName, SM_TRAJ &smTraj)
{
std::string str2;
FILE *fp_segMotion = NULL;
_sampling = SampTime;
_errMax = ErrPosMax;
_timeStep = ExpTime;
_flag_haus_actif = 0;
_fileName = fileName;
_nbAxis = curv.traj[0].Pos.size() ;
SM_OUTPUT tempo_motion;
double tu, ts;
/* set limits that are only used to compute the error in velocity on overall the trajectory */
_lim.maxJerk = 6*fabs(ErrVelMax);
_lim.maxAcc = 2*fabs(ErrVelMax);
_lim.maxVel = fabs(ErrVelMax);
initializeApproxVariables();
// dans l'attribut _curve il y aura deux courbes, la premiere _curve[0] est la traj ideale
// la second _curve[1] = (_curve.back()) sera la traj approximee
// les suites de cubiques de la traj approxime sont stockées dans _result
// _result.size() est la nombre de segment de cubique (c'est la meme pour tout les axes)
// SAUVE LA TRAJCTOIRE IDEALE (sous la forme d'un tableau)
if(flagExport==true) {
str2.clear();
str2 += "SmIdealTraj.dat";
saveTraj(str2, curv.traj);
SM_LOG(SM_LOG_INFO, "Sm_Approx::approximate Ideal Trajectory Already Computed and Saved");
}
/* Handle the path */
_curve.push_back(curv);
ChronoOn();
ChronoTimes(&tu, &ts);
/// CALCUL DE L'APPROXIMATION //
computeTraj();
SM_LOG(SM_LOG_INFO, "Sm_Approx::approximate Computation Duration : ");
ChronoPrint("");
std::cout << std::endl;
ChronoTimes(&tu, &ts);
ChronoOff();
if(flagExport==true) {
// SAUVE LA TRAJCTOIRE APPROXIMEE (sous la forme d'un tableau)
saveTraj("SmApproxTraj.dat", (_curve.back()).traj);
SM_LOG(SM_LOG_INFO, "Sm_Approx::approximate Approximated Trajectory Saved");
}
SM_LOG(SM_LOG_INFO, "Sm_Approx::approximate Error Max Pos " << (_curve.back()).errorMaxVal);
/* fill result */
for (unsigned int i = 1; i < _result.size(); i++){
for (unsigned int j = 0; j < _result.size()-i ; j++){
if (_result[j].premier_point > _result[j+1].premier_point){
tempo_motion = _result[j];
_result[j] = _result[j+1];
_result[j+1] = tempo_motion;
}
}
}
_result[0].IC.resize(_result[0].Time.size());
for(unsigned int i=0; i<_result[0].Time.size(); i++) {
_result[0].IC[i].x = curv.traj[0].Pos[i];
}
if(flagExport==true) {
//printf("approximate: There are %f axes and %f segments\n", (double)_result[0].Time.size(), (double)_result.size());
fp_segMotion = fopen("SmSegMotion.dat", "w");
if(fp_segMotion==NULL) {
SM_LOG(SM_LOG_ERROR, " cannot open file to write the trajectory");
return 1;
}
for (unsigned int i = 0; i < _result.size(); i++){
fprintf(fp_segMotion, "%d %lf ",_result[i].premier_point, (_result[i].premier_point + 1) * _sampling);
for(unsigned int k=0; k<_result[i].Time.size(); k++) {
fprintf(fp_segMotion, "%lf %lf ",_result[i].Time[k],_result[i].Jerk[k]);
}
fprintf(fp_segMotion, "\n");
}
fclose(fp_segMotion);
//cout << "motion file exported" << endl;
//printf("approximate: There are %f axes and %f segments\n", (double)_result[0].Time.size(), (double)_result.size());
}
// importe le resultat _result dans une classe de type SM_TRAJ Attention on recalcule tte les conditions initiales
// de chaque segment dans cette fonction ainsi que la duree totale de la trajectoire a partir des couples (Ti, Ji)
smTraj.importFromSM_OUTPUT(36, _sampling, _result);
//cout << " ... Approximated Trajectory Computed --" << endl;
if(flagExport==true) {
smTraj.save((char*)"SmApproxTraj_Seg.traj");
SM_LOG(SM_LOG_INFO, "Sm_Approx::approximate Series of cubics Trajectory Saved");
}
/* compare end point */
/* verification du point final atteint par smTraj par rapport a IdealTraj */
std::vector< SM_COND> cond;
smTraj.getMotionCond(smTraj.getDuration(), cond);
double errMax =0;
for(unsigned int i=0; i<cond.size(); i++) {
double err = fabs(cond[i].x - (_curve.back()).traj[ (_curve.back()).traj.size()-1].Pos[i] );
if( err > 0.001) {
SM_LOG(SM_LOG_WARNING, "Sm_Approx::approximate: final pose on axis " << i << ", err=" << err);
}
if(err > errMax){
errMax = err;
}
}
if(errMax < 0.001) {
SM_LOG(SM_LOG_INFO, "Sm_Approx::approximate runs successfully ... Algo Written by Xavier BROQUERE (Feb 2011)");
}
//printf("Final Conditions of the Ideal trajectory\n");
//for(unsigned int i=0; i<cond.size(); i++) {
// printf("%f ",(_curve.back()).traj[ (_curve.back()).traj.size()-1].Pos[i] );
//}
//printf("\n");
//printf("Final Conditions of the smTraj trajectory\n");
//for(unsigned int i=0; i<cond.size(); i++) {
// printf("%f ",cond[i].x);
//}
//printf("\n");
return 0;
}
