void PosterReader::softmotionPlotTraj()
{
if( _softmotionPoster == NULL )
{
cout << "softmotion : NULL Poster" << endl;
return;
}
//_softmotionPoster->update();
SM_TRAJ smTraj;
if(_softmotionPoster->getPosterStuct((char *)(&_softmotionPosterStruct)) == false) {
cout << " PosterReader::softmotionPlotTraj() ERROR " << endl;
}
smTraj.importFromSM_TRAJ_STR(&_softmotionPosterStruct);
if( smTraj.getDuration() <= 0) {
// QMessageBox toto(QMessageBox::Information, ,QMessageBox::Close, NULL,Qt::Dialog);
QMessageBox::about (NULL, QString("SoftMotion info"), QString("Trajectory is empty"));
return;
}
cout << "duration " << smTraj.getDuration() << endl;
smTraj.plot();
return;
}
void loadNewTraj(const pr2_soft_controller::SM_TRAJ_STR_ROS& msg)
{
SM_TRAJ_STR tmpTraj;
//copy of the msg in a softmotion structure
tmpTraj.trajId= msg.trajId;
tmpTraj.nbAxis= msg.nbAxis;
//tmpTraj.timePreserved= msg.timePreserved;
for(size_t i=0; i<(unsigned int)msg.nbAxis; ++i){
tmpTraj.qStart[i] = msg.qStart[i];
tmpTraj.qGoal[i] = msg.qGoal[i];
tmpTraj.jmax[i] = msg.jmax[i];
tmpTraj.amax[i] = msg.amax[i];
tmpTraj.vmax[i] = msg.vmax[i];
tmpTraj.traj[i].nbSeg= msg.traj[i].nbSeg;
tmpTraj.traj[i].unsused= msg.traj[i].unsused;
for(size_t j=0; j<(unsigned int)msg.traj[i].nbSeg; ++j){
tmpTraj.traj[i].seg[j].lpId= msg.traj[i].seg[j].lpId;
tmpTraj.traj[i].seg[j].unused= msg.traj[i].seg[j].unused;
tmpTraj.traj[i].seg[j].timeOnTraj= msg.traj[i].seg[j].timeOnTraj;
tmpTraj.traj[i].seg[j].time= msg.traj[i].seg[j].time;
tmpTraj.traj[i].seg[j].ic_a= msg.traj[i].seg[j].ic_a;
tmpTraj.traj[i].seg[j].ic_v= msg.traj[i].seg[j].ic_v;
tmpTraj.traj[i].seg[j].ic_x= msg.traj[i].seg[j].ic_x;
tmpTraj.traj[i].seg[j].jerk= msg.traj[i].seg[j].jerk;
}
}
_newTraj.importFromSM_TRAJ_STR( &tmpTraj );
}
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 main (int argc, char **argv)
{
ros::init(argc, argv, "test_fibonacci");
// create the action client
// true causes the client to spin its own thread
actionlib::SimpleActionClient<soft_move_base::SoftMoveBaseAction> ac("soft_move_base", true);
ROS_INFO("Waiting for action server to start.");
// wait for the action server to start
ac.waitForServer(); //will wait for infinite time
ROS_INFO("Action server started, sending goal.");
// send a goal to the action
soft_move_base::SoftMoveBaseGoal goal;
SM_TRAJ traj;
SM_TRAJ_STR smTraj;
//load test traj
traj.load("/u/magharbi/softMotion.traj", NULL);
//traj.plot();
//convert it
traj.convertToSM_TRAJ_STR(&smTraj);
pr2_soft_controller::SM_TRAJ_STR_ROS smTrajROS_;
smTrajROS_.trajId= 0;
//we use only a subset of the total joints set
smTrajROS_.nbAxis= 6;
smTrajROS_.qStart.resize(smTrajROS_.nbAxis);
smTrajROS_.qGoal.resize(smTrajROS_.nbAxis);
smTrajROS_.traj.resize(smTrajROS_.nbAxis);
// we need the joints trajectory between debut and fin
for(int i=0, n=0; i<smTrajROS_.nbAxis; ++i, ++n){
smTrajROS_.qStart[n] = smTraj.qStart[i];
smTrajROS_.qGoal[n] = smTraj.qGoal[i];
smTrajROS_.traj[n].nbSeg= smTraj.traj[i].nbSeg;
smTrajROS_.traj[n].unsused= smTraj.traj[i].unsused;
smTrajROS_.traj[n].seg.resize(smTrajROS_.traj[n].nbSeg);
for(int j=0; j<smTraj.traj[i].nbSeg; ++j){
smTrajROS_.traj[n].seg[j].lpId= smTraj.traj[i].seg[j].lpId;
smTrajROS_.traj[n].seg[j].unused= 0;
smTrajROS_.traj[n].seg[j].timeOnTraj= smTraj.traj[i].seg[j].timeOnTraj;
smTrajROS_.traj[n].seg[j].time= smTraj.traj[i].seg[j].time;
smTrajROS_.traj[n].seg[j].ic_a= smTraj.traj[i].seg[j].ic_a;
smTrajROS_.traj[n].seg[j].ic_v= smTraj.traj[i].seg[j].ic_v;
smTrajROS_.traj[n].seg[j].ic_x= smTraj.traj[i].seg[j].ic_x;
smTrajROS_.traj[n].seg[j].jerk= smTraj.traj[i].seg[j].jerk;
}
}
goal.traj = smTrajROS_;
ac.sendGoal(goal);
//wait for the action to return
bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
if (finished_before_timeout)
{
actionlib::SimpleClientGoalState state = ac.getState();
ROS_INFO("Action finished: %s",state.toString().c_str());
}
else
ROS_INFO("Action did not finish before the time out.");
//exit
return 0;
}
void getNextRobotCond(double time)
{
_newTraj.getMotionCond(time, _nextRobotCond_w);
}
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;
}
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;
}
