Skip to content

Commit

Permalink
Case with v_init > v_max working
Browse files Browse the repository at this point in the history
  • Loading branch information
@JimmyDaSilva
JimmyDaSilva committed May 9, 2017
1 parent b3c95c3 commit bc40c5f
Show file tree
Hide file tree
Showing 2 changed files with 100 additions and 39 deletions.
8 changes: 4 additions & 4 deletions include/spline_problems/adaptive_s_curve_profile.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,10 +28,10 @@ class AdaptiveSCurveProfile{
double compute_concave_time(double phase_acc_start, double phase_acc_final);
double compute_convexe_distance(double phase_acc_start, double phase_vel_start, double phase_acc_final);
double compute_convexe_time(double phase_acc_start, double phase_acc_final);
double compute_ramp_rise_distance(double phase_vel_start, double rise_vel_start, double rise_vel_final, double rise_acc_start);
double compute_ramp_rise_time(double rise_vel_start, double rise_acc_start, double rise_vel_final);
double compute_ramp_fall_distance(double phase_vel_start, double rise_vel_start, double rise_vel_final, double rise_acc_final);
double compute_ramp_fall_time(double rise_vel_start, double rise_acc_final, double rise_vel_final);
double compute_ramp_rise_distance(double phase_vel_start, double rise_vel_start, double rise_vel_final, double rise_acc_start, double rise_acc, double rise_acc_final);
double compute_ramp_rise_time(double rise_vel_start, double rise_vel_final, double rise_acc_start, double rise_acc, double rise_acc_final );
double compute_ramp_fall_distance(double phase_vel_start, double fall_vel_start, double fall_vel_final, double fall_acc_start, double fall_acc, double fall_acc_final);
double compute_ramp_fall_time(double fall_vel_start, double fall_vel_final ,double fall_acc_start, double fall_acc, double fall_acc_final );
double compute_cruise_distance(double cruise_vel, double phase_pos_start, double phase_pos_final);
double compute_cruise_time(double cruise_vel, double phase_pos_start, double phase_pos_final);

Expand Down
131 changes: 96 additions & 35 deletions src/adaptive_s_curve_profile.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ AdaptiveSCurveProfile::AdaptiveSCurveProfile ( double s_init, double vi_init, do
v_max_ = v_max;
a_max_ = a_max;
j_max_ = j_max;
period_ = 0.001;
period_ = 0.0001;
}

AdaptiveSCurveProfile::AdaptiveSCurveProfile() {
Expand All @@ -22,9 +22,7 @@ void AdaptiveSCurveProfile::set_period ( double period ) {
}

void AdaptiveSCurveProfile::compute_curves(){
// Computation of times...
// ...

// Reset vector with initial conditions
j_vect_.clear();
j_vect_.push_back(0.0);
a_vect_.clear();
Expand All @@ -36,30 +34,43 @@ void AdaptiveSCurveProfile::compute_curves(){
t_vect_.clear();
t_vect_.push_back(0.0);

// vf cannot be over vmax or below -vmax
if (vf_>0)
vf_ = std::min(vf_, v_max_);
else
vf_ = std::max(vf_, -v_max_);

// af cannot be over amax or below -amax
if (af_>0)
af_ = std::min(af_, a_max_);
else
af_ = std::max(af_, -a_max_);

// If initial velocity is smaller than max vel
if(vi_ <= v_max_){
double t_ramp_rise = compute_ramp_rise_time(vi_, ai_, v_max_);
double t_concave_rise = compute_concave_time(ai_,a_max_);
double t_ramp_rise = compute_ramp_rise_time(vi_,v_max_, ai_, a_max_, 0);
double t_concave_rise = compute_concave_time(ai_,a_max_);
double t_convexe_rise = compute_convexe_time(a_max_, 0);
double t_ramp_fall = compute_ramp_fall_time(v_max_, 0.0, vf_);
double t_ramp_fall = compute_ramp_fall_time(v_max_, vf_, 0, -a_max_, af_);
double t_convexe_fall = compute_convexe_time(0, -a_max_);
double t_concave_fall = compute_concave_time(-a_max_,0);

double t_concave_fall = compute_concave_time(-a_max_,af_);

double d_cruise = sf_-si_;
if(t_ramp_rise > 0){
d_cruise = d_cruise - compute_concave_distance(ai_, vi_, a_max_) - compute_ramp_rise_distance(vi_+ai_*t_concave_rise+j_max_/2*t_concave_rise*t_concave_rise,vi_, v_max_, ai_) - compute_convexe_distance(a_max_, vi_+ai_*t_concave_rise+j_max_/2*t_concave_rise*t_concave_rise+t_ramp_rise*a_max_, 0);
d_cruise = d_cruise - compute_concave_distance(ai_, vi_, a_max_) - compute_ramp_rise_distance(vi_+ai_*t_concave_rise+j_max_/2*t_concave_rise*t_concave_rise,vi_, v_max_, ai_, a_max_, 0) - compute_convexe_distance(a_max_, vi_+ai_*t_concave_rise+j_max_/2*t_concave_rise*t_concave_rise+t_ramp_rise*a_max_, 0);
}
else{
// TODO
std::cerr << "ramp rise time is negative" << std::endl;
d_cruise = d_cruise - compute_concave_distance(0,0,0) - compute_convexe_distance(0,0,0);
}
if(t_ramp_fall > 0){
d_cruise = d_cruise - compute_convexe_distance(0, v_max_, -a_max_) - compute_ramp_fall_distance(v_max_-j_max_/2*t_convexe_fall*t_convexe_fall,v_max_,vf_,0.0) - compute_concave_distance(-a_max_,v_max_-j_max_/2*t_convexe_fall*t_convexe_fall-t_ramp_fall*a_max_, 0) ;
d_cruise = d_cruise - compute_convexe_distance(0, v_max_, -a_max_) - compute_ramp_fall_distance(v_max_-j_max_/2*t_convexe_fall*t_convexe_fall,v_max_,vf_,0,-a_max_,af_) - compute_concave_distance(-a_max_,v_max_-j_max_/2*t_convexe_fall*t_convexe_fall-t_ramp_fall*a_max_, af_) ;
}
else{
// TODO
std::cerr << "ramp fall time is negative" << std::endl;
d_cruise = d_cruise - compute_convexe_distance(0,0,0) - compute_concave_distance(0,0,0);
d_cruise = d_cruise - compute_convexe_distance(0,0,0) - compute_concave_distance(0,0,0);
}
double t_cruise = d_cruise/v_max_;

Expand All @@ -81,29 +92,82 @@ void AdaptiveSCurveProfile::compute_curves(){
compute_next_phase(t_convexe_fall,-j_max_);

// Ramp fall
if (t_ramp_fall >0.0){
if (t_ramp_fall >0.0)
compute_next_phase(t_ramp_fall,0.0);
}

// Concave fall
compute_next_phase(t_concave_fall,j_max_);
}
else{
// TODO
std::cerr << "cruise time is negative" << std::endl;
}
}
else{
// TODO Change ramp computation time in case ramp does not start or end at -+amax
double t_ramp_fall1 = compute_ramp_fall_time(vi_, v_max_ ,ai_, -a_max_, 0);
double t_convexe_fall1 = compute_convexe_time(ai_,-a_max_);
double t_concave_fall1 = compute_concave_time(-a_max_, 0);
double t_ramp_fall2 = compute_ramp_fall_time(v_max_, vf_, 0, -a_max_, af_);
double t_convexe_fall2 = compute_convexe_time(0, -a_max_);
double t_concave_fall2 = compute_concave_time(-a_max_,af_);

double d_cruise = sf_-si_;
if(t_ramp_fall1 > 0){
d_cruise = d_cruise - compute_convexe_distance(ai_, vi_, -a_max_) - compute_ramp_fall_distance(vi_+ai_*t_convexe_fall1-j_max_/2*t_convexe_fall1*t_convexe_fall1,vi_, v_max_, ai_, -a_max_, 0) - compute_concave_distance(-a_max_, vi_+ai_*t_convexe_fall1-j_max_/2*t_convexe_fall1*t_convexe_fall1-t_ramp_fall1*a_max_, 0);
}
else{
// TODO
std::cerr << "ramp fall 1 time is negative" << std::endl;
d_cruise = d_cruise - compute_concave_distance(0,0,0) - compute_convexe_distance(0,0,0);
}
if(t_ramp_fall2 > 0){
d_cruise = d_cruise - compute_convexe_distance(0, v_max_, -a_max_) - compute_ramp_fall_distance(v_max_-j_max_/2*t_convexe_fall2*t_convexe_fall2,v_max_,vf_,0, -a_max_, af_) - compute_concave_distance(-a_max_,v_max_-j_max_/2*t_convexe_fall2*t_convexe_fall2-t_ramp_fall2*a_max_, af_) ;
}
else{
// TODO
std::cerr << "ramp fall 2 time is negative" << std::endl;
d_cruise = d_cruise - compute_convexe_distance(0,0,0) - compute_concave_distance(0,0,0);
}
double t_cruise = d_cruise/v_max_;

if(t_cruise>0){
// Convexe fall
compute_next_phase(t_convexe_fall1, -j_max_);

// Ramp fall
if (t_ramp_fall1 >0.0)
compute_next_phase(t_ramp_fall1, 0.0);

// Concave fall
compute_next_phase(t_concave_fall1, j_max_);

// Cruise
compute_next_phase(t_cruise, 0.0);

// Convexe fall
compute_next_phase(t_convexe_fall2,-j_max_);

// Ramp fall
if (t_ramp_fall2 >0.0)
compute_next_phase(t_ramp_fall2,0.0);

// Concave fall
compute_next_phase(t_concave_fall2,j_max_);
}
else{
// TODO
std::cerr << "cruise time is negative" << std::endl;
}


std::cout<<"Concave rise time : " << t_concave_rise<<std::endl;
std::cout<<"Ramp rise time : " << t_ramp_rise<<std::endl;
std::cout<<"Convexe rise time : " << t_convexe_rise<<std::endl;
std::cout<<"Convexe fall 1 time : " << t_convexe_fall1<<std::endl;
std::cout<<"Ramp fall 1 time : " << t_ramp_fall1<<std::endl;
std::cout<<"Concave fall 1 time : " << t_concave_fall1<<std::endl;
std::cout<<"Cruise time : " << t_cruise<<std::endl;
std::cout<<"Convexe fall time : " << t_convexe_fall<<std::endl;
std::cout<<"Ramp fall time : " << t_ramp_fall<<std::endl;
std::cout<<"Concave fall time : " << t_concave_fall<<std::endl;
}
else{
std::cout<<"Convexe fall 2 time : " << t_convexe_fall2<<std::endl;
std::cout<<"Ramp fall 2 time : " << t_ramp_fall2<<std::endl;
std::cout<<"Concave fall 2 time : " << t_concave_fall2<<std::endl;

return;
}
}
Expand Down Expand Up @@ -144,20 +208,20 @@ double AdaptiveSCurveProfile::compute_convexe_time(double phase_acc_start, doubl
return (phase_acc_start-phase_acc_final)/j_max_;
}

double AdaptiveSCurveProfile::compute_ramp_rise_distance(double phase_vel_start, double rise_vel_start, double rise_vel_final, double rise_acc_start){
return compute_phase_distance(compute_ramp_rise_time(rise_vel_start, rise_acc_start, rise_vel_final), 0 , a_max_, phase_vel_start);
double AdaptiveSCurveProfile::compute_ramp_rise_distance(double phase_vel_start, double rise_vel_start, double rise_vel_final, double rise_acc_start, double rise_acc, double rise_acc_final){
return compute_phase_distance(compute_ramp_rise_time(rise_vel_start, rise_vel_final, rise_acc_start, rise_acc, rise_acc_final), 0 , rise_acc, phase_vel_start);
}

double AdaptiveSCurveProfile::compute_ramp_rise_time(double rise_vel_start, double rise_acc_start, double rise_vel_final){
return ((rise_vel_final-rise_vel_start)-(a_max_*a_max_-rise_acc_start*rise_acc_start/2)/j_max_)/a_max_;
double AdaptiveSCurveProfile::compute_ramp_rise_time(double rise_vel_start, double rise_vel_final, double rise_acc_start, double rise_acc, double rise_acc_final ){
return ((rise_vel_final-rise_vel_start)-(rise_acc*rise_acc-rise_acc_start*rise_acc_start/2-rise_acc_final*rise_acc_final/2.0)/j_max_)/rise_acc;
}

double AdaptiveSCurveProfile::compute_ramp_fall_distance(double phase_vel_start, double rise_vel_start, double rise_vel_final, double rise_acc_final){
return compute_phase_distance(compute_ramp_fall_time(rise_vel_start, rise_acc_final, rise_vel_final), 0 , -a_max_, phase_vel_start);
double AdaptiveSCurveProfile::compute_ramp_fall_distance(double phase_vel_start, double fall_vel_start, double fall_vel_final, double fall_acc_start, double fall_acc, double fall_acc_final){
return compute_phase_distance(compute_ramp_fall_time(fall_vel_start, fall_vel_final, fall_acc_start, fall_acc, fall_acc_final), 0 , fall_acc, phase_vel_start);
}

double AdaptiveSCurveProfile::compute_ramp_fall_time(double rise_vel_start, double rise_acc_final, double rise_vel_final){
return ((rise_vel_start-rise_vel_final)-(a_max_*a_max_-rise_acc_final*rise_acc_final/2)/j_max_)/a_max_;
double AdaptiveSCurveProfile::compute_ramp_fall_time(double fall_vel_start, double fall_vel_final ,double fall_acc_start, double fall_acc, double fall_acc_final ){
return ((fall_vel_start-fall_vel_final)-(fall_acc*fall_acc-fall_acc_start*fall_acc_start/2-fall_acc_final*fall_acc_final/2)/j_max_)/(-fall_acc);
}

double AdaptiveSCurveProfile::compute_cruise_distance(double cruise_vel, double phase_pos_start, double phase_pos_final){
Expand All @@ -168,9 +232,6 @@ double AdaptiveSCurveProfile::compute_cruise_time(double cruise_vel, double phas
return (phase_pos_final-phase_pos_start)/cruise_vel;
}




void AdaptiveSCurveProfile::plot_curves() {

// Plot position
Expand Down Expand Up @@ -200,7 +261,7 @@ int main(int argc, char** argv){
ros::init(argc, argv, "adaptive_s_curve_profile");
ros::NodeHandle nh;

AdaptiveSCurveProfile s_curve(0, -0.5, -2, 1, 0, 0, 1, 3, 50);
AdaptiveSCurveProfile s_curve(0, 1.2, -1, 2, 0.5, 2, 1, 2, 30);
s_curve.set_period(0.00001);
s_curve.compute_curves();
s_curve.plot_curves();
Expand Down

0 comments on commit bc40c5f

Please sign in to comment.