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