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