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