From 0241b30e58f2dc7a893047f5be8acf521ac34e1e Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 25 Oct 2022 16:56:51 +0200
Subject: [PATCH] Updated the trapezoid velocity profile generation.
---
src/vel_trapezoid.cpp | 48 +++++++++++++++++++++++++------------------
1 file changed, 28 insertions(+), 20 deletions(-)
diff --git a/src/vel_trapezoid.cpp b/src/vel_trapezoid.cpp
index 03bed13..ebb3d76 100644
--- a/src/vel_trapezoid.cpp
+++ b/src/vel_trapezoid.cpp
@@ -20,54 +20,62 @@ void CVelTrapezoid::generate(double start_vel,double end_vel,double max_vel, dou
this->set_end_vel(end_vel);
this->set_max_vel(max_vel);
this->set_max_acc(max_acc);
- this->acc_time=(max_vel-start_vel)/max_acc;
- this->dec_time=-(end_vel-max_vel)/max_acc;
- this->acc_length=start_vel*this->acc_time+(max_vel-start_vel)*this->acc_time/2.0;
- this->dec_length=end_vel*this->dec_time-(end_vel-max_vel)*this->dec_time/2.0;
+ if(start_vel==max_vel)
+ this->acc_time=0.0;
+ else
+ this->acc_time=fabs(max_vel-start_vel)/max_acc;
+ if(end_vel==max_vel)
+ this->dec_time=0.0;
+ else
+ this->dec_time=fabs(end_vel-max_vel)/max_acc;
+ this->acc_length=std::min(start_vel,max_vel)*this->acc_time+fabs(max_vel-start_vel)*this->acc_time/2.0;
+ this->dec_length=std::min(end_vel,max_vel)*this->dec_time+fabs(end_vel-max_vel)*this->dec_time/2.0;
this->length=length;
this->generated=true;
}
bool CVelTrapezoid::generate(double start_vel,double end_vel,double max_vel, double max_acc,double length,double &new_start_vel,double &new_max_vel)
{
+ /*
if(start_vel>max_vel)
throw CException(_HERE_,"Start velocity must be smaller than the maximum velocity");
if(end_vel>max_vel)
throw CException(_HERE_,"End velocity must be smaller than the maximum velocity");
if(max_acc<=0.0)
throw CException(_HERE_,"Acceleration must be positive");
+ */
+ if(start_vel>max_vel)
+ start_vel=max_vel;
+ if(end_vel>max_vel)
+ end_vel=max_vel;
+ if(max_acc<=0.0)
+ max_acc=0.0;
this->generate(start_vel,end_vel,max_vel,max_acc,length);
- if(length-this->acc_length-this->dec_length>0.0)
+ if((length-this->acc_length-this->dec_length)>0.0)
{
this->time=(length-this->acc_length-this->dec_length)/this->max_vel+this->acc_time+this->dec_time;
new_start_vel=this->start_vel;
new_max_vel=this->max_vel;
return true;
}
- else
+ else
{
- double value=-pow(this->start_vel-this->end_vel,2.0)*pow(this->max_acc-1.0,2.0)-2.0*this->length*pow(this->max_acc,2.0)*(this->max_acc-1.0);
- if(value>=0.0)
+ new_max_vel=sqrt(length*this->max_acc+(pow(start_vel,2.0)+pow(end_vel,2.0))/2.0);
+ if(new_max_vel>=start_vel)
{
- new_max_vel=((this->max_acc-1.0)*(this->start_vel+this->end_vel)-sqrt(value))/(2.0*(this->max_acc-1.0));
this->generate(this->start_vel,this->end_vel,new_max_vel,this->max_acc,this->length);
this->time=(length-this->acc_length-this->dec_length)/new_max_vel+this->acc_time+this->dec_time;
new_start_vel=this->start_vel;
+ return true;
}
else
{
- value=-this->length*(this->max_acc-1.0);
- if(value>0.0)
- {
- new_max_vel=((this->max_acc-1.0)*this->end_vel-this->max_acc*sqrt(value))/(this->max_acc-1.0);
- new_start_vel=new_max_vel;
- this->generate(new_start_vel,this->end_vel,new_max_vel,this->max_acc,length);
- this->time=(this->length-this->dec_length)/new_max_vel+this->dec_time;
- }
- else
- throw CException(_HERE_,"Impossible to create trapezoid");
+ new_max_vel=sqrt(pow(end_vel,2.0)+2*length*this->max_acc);
+ this->generate(this->start_vel,this->end_vel,new_max_vel,this->max_acc,this->length);
+ this->time=(length-this->acc_length-this->dec_length)/new_max_vel+this->acc_time+this->dec_time;
+ new_start_vel=this->start_vel;
+ return true;
}
- return false;
}
}
--
GitLab