Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044 #include "velocityprofile_traphalf.hpp"
00045 #include <algorithm>
00046
00047 namespace KDL {
00048
00049
00050 VelocityProfile_TrapHalf::VelocityProfile_TrapHalf(double _maxvel,double _maxacc,bool _starting):
00051 maxvel(_maxvel),maxacc(_maxacc),starting(_starting) {}
00052
00053 void VelocityProfile_TrapHalf::SetMax(double _maxvel,double _maxacc, bool _starting)
00054 {
00055 maxvel = _maxvel; maxacc = _maxacc; starting = _starting;
00056 }
00057
00058 void VelocityProfile_TrapHalf::PlanProfile1(double v,double a) {
00059 a3 = 0;
00060 a2 = 0;
00061 a1 = startpos;
00062 b3 = a/2.0;
00063 b2 = -a*t1;
00064 b1 = startpos + a*t1*t1/2.0;
00065 c3 = 0;
00066 c2 = v;
00067 c1 = endpos - v*duration;
00068 }
00069
00070 void VelocityProfile_TrapHalf::PlanProfile2(double v,double a) {
00071 a3 = 0;
00072 a2 = v;
00073 a1 = startpos;
00074 b3 = -a/2.0;
00075 b2 = a*t2;
00076 b1 = endpos - a*t2*t2/2.0;
00077 c3 = 0;
00078 c2 = 0;
00079 c1 = endpos;
00080 }
00081
00082 void VelocityProfile_TrapHalf::SetProfile(double pos1,double pos2) {
00083 startpos = pos1;
00084 endpos = pos2;
00085 double s = sign(endpos-startpos);
00086
00087 double vel = std::min(maxvel, sqrt(2.0*s*(endpos-startpos)*maxacc));
00088 duration = s*(endpos-startpos)/vel+vel/maxacc/2.0;
00089 if (starting) {
00090 t1 = 0;
00091 t2 = vel/maxacc;
00092 PlanProfile1(vel*s,maxacc*s);
00093 } else {
00094 t1 = duration-vel/maxacc;
00095 t2 = duration;
00096 PlanProfile2(s*vel,s*maxacc);
00097 }
00098 }
00099
00100 void VelocityProfile_TrapHalf::SetProfileDuration(
00101 double pos1,double pos2,double newduration)
00102 {
00103 SetProfile(pos1,pos2);
00104 double factor = duration/newduration;
00105
00106 if ( factor > 1 )
00107 return;
00108
00109 double s = sign(endpos-startpos);
00110 double tmp = 2.0*s*(endpos-startpos)/maxvel;
00111 double v = s*maxvel;
00112 duration = newduration;
00113 if (starting) {
00114 if (tmp > duration) {
00115 t1 = 0;
00116 double a = v*v/2.0/(v*duration-(endpos-startpos));
00117 t2 = v/a;
00118 PlanProfile1(v,a);
00119 } else {
00120 t2 = duration;
00121 double a = v*v/2.0/(endpos-startpos);
00122 t1 = t2-v/a;
00123 PlanProfile1(v,a);
00124 }
00125 } else {
00126 if (tmp > duration) {
00127 t2 = duration;
00128 double a = v*v/2.0/(v*duration-(endpos-startpos));
00129 t1 = t2-v/a;
00130 PlanProfile2(v,a);
00131 } else {
00132 double a = v*v/2.0/(endpos-startpos);
00133 t1 = 0;
00134 t2 = v/a;
00135 PlanProfile2(v,a);
00136 }
00137 }
00138 }
00139
00140 double VelocityProfile_TrapHalf::Duration() const {
00141 return duration;
00142 }
00143
00144 double VelocityProfile_TrapHalf::Pos(double time) const {
00145 if (time < 0) {
00146 return startpos;
00147 } else if (time<t1) {
00148 return a1+time*(a2+a3*time);
00149 } else if (time<t2) {
00150 return b1+time*(b2+b3*time);
00151 } else if (time<=duration) {
00152 return c1+time*(c2+c3*time);
00153 } else {
00154 return endpos;
00155 }
00156 }
00157 double VelocityProfile_TrapHalf::Vel(double time) const {
00158 if (time < 0) {
00159 return 0;
00160 } else if (time<t1) {
00161 return a2+2*a3*time;
00162 } else if (time<t2) {
00163 return b2+2*b3*time;
00164 } else if (time<=duration) {
00165 return c2+2*c3*time;
00166 } else {
00167 return 0;
00168 }
00169 }
00170
00171 double VelocityProfile_TrapHalf::Acc(double time) const {
00172 if (time < 0) {
00173 return 0;
00174 } else if (time<t1) {
00175 return 2*a3;
00176 } else if (time<t2) {
00177 return 2*b3;
00178 } else if (time<=duration) {
00179 return 2*c3;
00180 } else {
00181 return 0;
00182 }
00183 }
00184
00185 VelocityProfile* VelocityProfile_TrapHalf::Clone() const {
00186 VelocityProfile_TrapHalf* res = new VelocityProfile_TrapHalf(maxvel,maxacc, starting);
00187 res->SetProfileDuration( this->startpos, this->endpos, this->duration );
00188 return res;
00189 }
00190
00191 VelocityProfile_TrapHalf::~VelocityProfile_TrapHalf() {}
00192
00193
00194 void VelocityProfile_TrapHalf::Write(std::ostream& os) const {
00195 os << "TRAPEZOIDALHALF[" << maxvel << "," << maxacc << "," << starting << "]";
00196 }
00197
00198
00199
00200
00201
00202 }
00203