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::SetMax(double _maxvel,double _maxacc)
00112 {
00113 maxvel = _maxvel; maxacc = _maxacc;
00114 }
00115
00116 double VelocityProfile_Trap::Duration() const {
00117 return duration;
00118 }
00119
00120 double VelocityProfile_Trap::Pos(double time) const {
00121 if (time < 0) {
00122 return startpos;
00123 } else if (time<t1) {
00124 return a1+time*(a2+a3*time);
00125 } else if (time<t2) {
00126 return b1+time*(b2+b3*time);
00127 } else if (time<=duration) {
00128 return c1+time*(c2+c3*time);
00129 } else {
00130 return endpos;
00131 }
00132 }
00133 double VelocityProfile_Trap::Vel(double time) const {
00134 if (time < 0) {
00135 return 0;
00136 } else if (time<t1) {
00137 return a2+2*a3*time;
00138 } else if (time<t2) {
00139 return b2+2*b3*time;
00140 } else if (time<=duration) {
00141 return c2+2*c3*time;
00142 } else {
00143 return 0;
00144 }
00145 }
00146
00147 double VelocityProfile_Trap::Acc(double time) const {
00148 if (time < 0) {
00149 return 0;
00150 } else if (time<t1) {
00151 return 2*a3;
00152 } else if (time<t2) {
00153 return 2*b3;
00154 } else if (time<=duration) {
00155 return 2*c3;
00156 } else {
00157 return 0;
00158 }
00159 }
00160
00161 VelocityProfile* VelocityProfile_Trap::Clone() const {
00162 VelocityProfile_Trap* res = new VelocityProfile_Trap(maxvel,maxacc);
00163 res->SetProfileDuration( this->startpos, this->endpos, this->duration );
00164 return res;
00165 }
00166
00167 VelocityProfile_Trap::~VelocityProfile_Trap() {}
00168
00169
00170 void VelocityProfile_Trap::Write(std::ostream& os) const {
00171 os << "TRAPEZOIDAL[" << maxvel << "," << maxacc <<"]";
00172 }
00173
00174
00175
00176
00177
00178 }
00179