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
00046 namespace KDL {
00047
00048
00049 VelocityProfile_TrapHalf::VelocityProfile_TrapHalf(double _maxvel,double _maxacc,bool _starting):
00050 maxvel(_maxvel),maxacc(_maxacc),starting(_starting) {}
00051
00052 void VelocityProfile_TrapHalf::SetMax(double _maxvel,double _maxacc, bool _starting)
00053 {
00054 maxvel = _maxvel; maxacc = _maxacc; starting = _starting;
00055 }
00056
00057 void VelocityProfile_TrapHalf::PlanProfile1(double v,double a) {
00058 a3 = 0;
00059 a2 = 0;
00060 a1 = startpos;
00061 b3 = a/2.0;
00062 b2 = -a*t1;
00063 b1 = startpos + a*t1*t1/2.0;
00064 c3 = 0;
00065 c2 = v;
00066 c1 = endpos - v*duration;
00067 }
00068
00069 void VelocityProfile_TrapHalf::PlanProfile2(double v,double a) {
00070 a3 = 0;
00071 a2 = v;
00072 a1 = startpos;
00073 b3 = -a/2.0;
00074 b2 = a*t2;
00075 b1 = endpos - a*t2*t2/2.0;
00076 c3 = 0;
00077 c2 = 0;
00078 c1 = endpos;
00079 }
00080
00081 void VelocityProfile_TrapHalf::SetProfile(double pos1,double pos2) {
00082 startpos = pos1;
00083 endpos = pos2;
00084 double s = sign(endpos-startpos);
00085
00086 double vel = std::min(maxvel, sqrt(2.0*s*(endpos-startpos)*maxacc));
00087 duration = s*(endpos-startpos)/vel+vel/maxacc/2.0;
00088 if (starting) {
00089 t1 = 0;
00090 t2 = vel/maxacc;
00091 PlanProfile1(vel*s,maxacc*s);
00092 } else {
00093 t1 = duration-vel/maxacc;
00094 t2 = duration;
00095 PlanProfile2(s*vel,s*maxacc);
00096 }
00097 }
00098
00099 void VelocityProfile_TrapHalf::SetProfileDuration(
00100 double pos1,double pos2,double newduration)
00101 {
00102 SetProfile(pos1,pos2);
00103 double factor = duration/newduration;
00104
00105 if ( factor > 1 )
00106 return;
00107
00108 double s = sign(endpos-startpos);
00109 double tmp = 2.0*s*(endpos-startpos)/maxvel;
00110 double v = s*maxvel;
00111 duration = newduration;
00112 if (starting) {
00113 if (tmp > duration) {
00114 t1 = 0;
00115 double a = v*v/2.0/(v*duration-(endpos-startpos));
00116 t2 = v/a;
00117 PlanProfile1(v,a);
00118 } else {
00119 t2 = duration;
00120 double a = v*v/2.0/(endpos-startpos);
00121 t1 = t2-v/a;
00122 PlanProfile1(v,a);
00123 }
00124 } else {
00125 if (tmp > duration) {
00126 t2 = duration;
00127 double a = v*v/2.0/(v*duration-(endpos-startpos));
00128 t1 = t2-v/a;
00129 PlanProfile2(v,a);
00130 } else {
00131 double a = v*v/2.0/(endpos-startpos);
00132 t1 = 0;
00133 t2 = v/a;
00134 PlanProfile2(v,a);
00135 }
00136 }
00137 }
00138
00139 double VelocityProfile_TrapHalf::Duration() const {
00140 return duration;
00141 }
00142
00143 double VelocityProfile_TrapHalf::Pos(double time) const {
00144 if (time < 0) {
00145 return startpos;
00146 } else if (time<t1) {
00147 return a1+time*(a2+a3*time);
00148 } else if (time<t2) {
00149 return b1+time*(b2+b3*time);
00150 } else if (time<=duration) {
00151 return c1+time*(c2+c3*time);
00152 } else {
00153 return endpos;
00154 }
00155 }
00156 double VelocityProfile_TrapHalf::Vel(double time) const {
00157 if (time < 0) {
00158 return 0;
00159 } else if (time<t1) {
00160 return a2+2*a3*time;
00161 } else if (time<t2) {
00162 return b2+2*b3*time;
00163 } else if (time<=duration) {
00164 return c2+2*c3*time;
00165 } else {
00166 return 0;
00167 }
00168 }
00169
00170 double VelocityProfile_TrapHalf::Acc(double time) const {
00171 if (time < 0) {
00172 return 0;
00173 } else if (time<t1) {
00174 return 2*a3;
00175 } else if (time<t2) {
00176 return 2*b3;
00177 } else if (time<=duration) {
00178 return 2*c3;
00179 } else {
00180 return 0;
00181 }
00182 }
00183
00184 VelocityProfile* VelocityProfile_TrapHalf::Clone() const {
00185 VelocityProfile_TrapHalf* res = new VelocityProfile_TrapHalf(maxvel,maxacc, starting);
00186 res->SetProfileDuration( this->startpos, this->endpos, this->duration );
00187 return res;
00188 }
00189
00190 VelocityProfile_TrapHalf::~VelocityProfile_TrapHalf() {}
00191
00192
00193 void VelocityProfile_TrapHalf::Write(std::ostream& os) const {
00194 os << "TRAPEZOIDALHALF[" << maxvel << "," << maxacc << "," << starting << "]";
00195 }
00196
00197
00198
00199
00200
00201 }
00202