Go to the documentation of this file.00001
00060
00061 #include <math.h>
00062 #include <stddef.h>
00063
00064
00065 #include <schunk_powercube_chain/moveCommand.h>
00066
00067
00068 RampCommand::RampCommand(double x0, double v0, double xtarget, double amax, double vmax)
00069 : moveCommand(),
00070 m_x0(x0),
00071 m_v0(v0),
00072 m_xtarget(xtarget),
00073 m_amax(amax),
00074 m_vmax(vmax)
00075 {
00076 m_umkehr = false;
00077 m_nachumkehr = NULL;
00079 m_vmax = fabs(m_vmax);
00080 m_amax = fabs(m_amax);
00082 double delta = m_xtarget - m_x0;
00083 if (delta < 0)
00084 {
00085 m_vmax = -m_vmax;
00086 m_amax = -m_amax;
00087 }
00088
00090 if (m_v0 * m_vmax >= 0)
00091 {
00093 if (fabs(m_vmax) >= fabs(m_v0))
00094 {
00096 double delta1 = (m_vmax*m_vmax - 0.5 * m_v0*m_v0) / m_amax;
00097 double delta2 = 0.5 * m_v0*m_v0 / m_amax;
00098
00099 if (fabs(delta) >= fabs(delta1)) {
00100 m_T1 = (m_vmax-m_v0) / m_amax;
00101 m_a1 = m_amax;
00102 m_T3 = m_vmax / m_amax;
00103 m_a3 = -m_amax;
00104 m_T2 = (delta - delta1) / m_vmax;
00105 m_v2 = m_vmax;
00106 }
00107 else if (fabs(delta) >= fabs(delta2)) {
00108 m_v2 = sqrt( delta*m_amax + 0.5*m_v0*m_v0 );
00109 m_v2 = (delta < 0)?-m_v2:m_v2;
00110 m_T1 = (m_v2-m_v0) / m_amax;
00111 m_a1 = m_amax;
00112 m_T2 = 0.0;
00113 m_T3 = m_v2 / m_amax;
00114 m_a3 = -m_amax;
00115 }
00116 else if (fabs(delta) > 0)
00117 {
00118 m_v2 = -sqrt( 0.5*m_v0*m_v0 - m_amax*delta );
00119 m_v2 = (delta < 0)?-m_v2:m_v2;
00120 m_T2 = 0.0;
00121 m_T1 = (m_v0 - m_v2) / m_amax;
00122 m_a1 = -m_amax;
00123 m_T3 = - m_v2 / m_amax;
00124 m_a3 = m_amax;
00125 }
00127 else
00128 {
00129 m_T1 = 0;
00130 m_T2 = 0;
00131 m_T3 = 0;
00132 m_a1 = 0;
00133 m_v2 = 0;
00134 m_a3 = 0;
00135 }
00137 } else
00138 {
00140 double delta1 = 0.5 * m_v0*m_v0 / m_amax;
00142 double vstern = m_v0 * 0.707106781;
00143
00144 if (fabs(delta) >= fabs(delta1))
00145 {
00146 m_T1 = (m_v0-m_vmax) / m_amax;
00147 m_a1 = -m_amax;
00148 m_T3 = m_vmax / m_amax;
00149 m_a3 = -m_amax;
00150 m_T2 = (delta - delta1) / m_vmax;
00151 m_v2 = m_vmax;
00152 }
00153 else if (fabs(m_vmax) >= fabs(vstern))
00154 {
00155 m_v2 = -sqrt( 0.5*m_v0*m_v0 - m_amax*delta );
00156 m_v2 = (delta < 0)?-m_v2:m_v2;
00157 m_T2 = 0.0;
00158 m_T1 = (m_v0 - m_v2) / m_amax;
00159 m_a1 = -m_amax;
00160 m_T3 = - m_v2 / m_amax;
00161 m_a3 = m_amax;
00162 }
00164 else
00165 {
00166 m_T1 = (m_v0 + m_vmax) / m_amax;
00167 m_a1 = -m_amax;
00168 m_T2 = ( delta - (0.5*m_v0*m_v0/m_amax - m_vmax*m_vmax/m_amax) ) / m_vmax;
00169 m_v2 = -m_vmax;
00170 m_T3 = m_vmax / m_amax;
00171 m_a3 = m_amax;
00172 }
00173 }
00174 }
00177 else
00178 {
00180 m_a1 = m_amax;
00181 m_T1 = -m_v0 / m_amax;
00182 m_umkehr = true;
00183
00184 m_nachumkehr = new RampCommand( m_x0 + 0.5*m_v0*m_T1, 0.0, m_xtarget, m_amax, m_vmax);
00185 m_T2 = 0;
00186 m_T3 = 0;
00187 m_v2 = 0;
00188 m_a3 = 0;
00189 }
00191 }
00192
00193 RampCommand::RampCommand(const RampCommand& rc)
00194 {
00195 m_x0 = rc.m_x0;
00196 m_v0 = rc.m_v0;
00197 m_xtarget = rc.m_xtarget;
00198 m_amax = rc.m_amax;
00199 m_vmax = rc.m_vmax;
00200
00201 m_T1 = rc.m_T1;
00202 m_T2 = rc.m_T2;
00203 m_T3 = rc.m_T3;
00204 m_a1 = rc.m_a1;
00205 m_v2 = rc.m_v2;
00206 m_a3 = rc.m_a3;
00207
00208 m_umkehr = rc.m_umkehr;
00212 if (rc.m_umkehr)
00213 m_nachumkehr = new RampCommand(*rc.m_nachumkehr);
00214 else
00215 m_nachumkehr = NULL;
00216 }
00217
00218 RampCommand& RampCommand::operator=(const RampCommand& rc)
00219 {
00220 if (m_nachumkehr)
00221 delete m_nachumkehr;
00222
00223 m_x0 = rc.m_x0;
00224 m_v0 = rc.m_v0;
00225 m_xtarget = rc.m_xtarget;
00226 m_amax = rc.m_amax;
00227 m_vmax = rc.m_vmax;
00228
00229 m_T1 = rc.m_T1;
00230 m_T2 = rc.m_T2;
00231 m_T3 = rc.m_T3;
00232 m_a1 = rc.m_a1;
00233 m_v2 = rc.m_v2;
00234 m_a3 = rc.m_a3;
00235
00236 m_umkehr = rc.m_umkehr;
00240 if (rc.m_umkehr)
00241 m_nachumkehr = new RampCommand(*rc.m_nachumkehr);
00242 else
00243 m_nachumkehr = NULL;
00244
00245 return *this;
00246 }
00247
00251 double RampCommand::getPos(double TimeElapsed)
00252 {
00253 if (m_umkehr)
00254 {
00255 if (TimeElapsed <= m_T1)
00256 {
00257 return m_x0 + m_v0 * TimeElapsed + 0.5 * m_a1 * TimeElapsed * TimeElapsed;
00258 } else {
00259
00260 return m_nachumkehr->getPos(TimeElapsed-m_T1);
00261 }
00262 } else
00263 {
00264 if (TimeElapsed <= m_T1)
00265 {
00266 return m_x0 + m_v0 * TimeElapsed + 0.5 * m_a1 * TimeElapsed * TimeElapsed;
00267 }
00268 else if (TimeElapsed <= m_T1 + m_T2)
00269 {
00270 return m_x0 + m_v0*m_T1 + 0.5*m_a1*m_T1*m_T1 + m_v2 * (TimeElapsed-m_T1);
00271 }
00272 else if (TimeElapsed <= m_T1 + m_T2 + m_T3)
00273 {
00274 return m_x0 + m_v0*m_T1 + 0.5*m_a1*m_T1*m_T1 + m_v2*m_T2 +
00275 m_v2*(TimeElapsed-m_T1-m_T2) + 0.5*m_a3*(TimeElapsed-m_T1-m_T2)*(TimeElapsed-m_T1-m_T2);
00276
00277 }
00279 else
00280 return m_xtarget;
00281 }
00282 }
00283
00287 double RampCommand::getVel(double TimeElapsed)
00288 {
00289 if (m_umkehr)
00290 {
00291 if (TimeElapsed <= m_T1)
00292 {
00293 return m_v0 + m_a1 * TimeElapsed;
00294 } else {
00295 return m_nachumkehr->getVel(TimeElapsed-m_T1);
00296 }
00297 } else
00298 {
00299 if (TimeElapsed <= m_T1)
00300 {
00301 return m_v0 + m_a1 * TimeElapsed;
00302 }
00303 else if (TimeElapsed <= m_T1 + m_T2)
00304 {
00305 return m_v2;
00306 }
00307 else if (TimeElapsed <= m_T1 + m_T2 + m_T3)
00308 {
00309 return m_v2 + m_a3 * (TimeElapsed - m_T1 - m_T2);
00310 }
00312 else
00313 return 0.0;
00314 }
00315 }
00316
00320 double RampCommand::getTotalTime()
00321 {
00322
00323 if (m_umkehr)
00324 return m_T1 + m_nachumkehr->getTotalTime();
00325 else
00326 return m_T1 + m_T2 + m_T3;
00327 }
00328
00334 void RampCommand::calculateAV(double x0, double v0, double xtarget, double time, double T3, double amax,
00335 double vmax, double& acc, double& vel)
00336 {
00337
00338
00339 double TG = time;
00340
00341 if (TG <= 0)
00342 {
00343 acc = 0;
00344 vel = 0;
00345 } else
00346 {
00347 double delta = xtarget - x0;
00349 amax = fabs(amax);
00350 amax = (delta>=0)?amax:-amax;
00351
00352 double d1 = fabs(v0)*TG - 0.5 * T3 * fabs(v0);
00353 double d2h = (v0>0)?-T3*v0 + v0*sqrt(2*T3*TG):-T3*v0 - v0*sqrt(2*T3*TG);
00354 double d2l = (v0>0)?-T3*v0 - v0*sqrt(2*T3*TG):-T3*v0 + v0*sqrt(2*T3*TG);
00355
00356 double d3 = 0.5 * v0 * v0 / amax;
00357
00358
00359
00360
00361
00362
00363 if (T3 > 0)
00364 {
00366 if (fabs(delta) >= d1)
00367 {
00368
00369
00370
00371
00372
00374 double a = (TG / T3 - 1.0);
00375 double b = v0 - delta/T3;
00376 double c = - 0.5 * v0 * v0;
00377
00379 if (delta >= 0)
00380 vel = (-b + sqrt(b*b - 4.0*a*c)) / (2.0 * a);
00381 else
00382 vel = (-b - sqrt(b*b - 4.0*a*c)) / (2.0 * a);
00383
00385 acc = vel / T3;
00386 }
00387 else if (delta >= d2h || delta <= d2l)
00389 {
00390
00391
00392
00393
00395 double a = TG;
00396 double b = - delta - T3*v0;
00397 double c = 0.5 * v0 * v0 * T3;
00398
00400 if (delta >= 0)
00401 vel = (-b + sqrt(b*b - 4.0*a*c)) / (2.0 * a);
00402 else
00403 vel = (-b - sqrt(b*b - 4.0*a*c)) / (2.0 * a);
00404
00406 acc = vel / T3;
00407 }
00408 else if (fabs(delta) >= d3)
00409 {
00411
00419
00420 acc = amax;
00421
00422 if (delta*v0 > 0)
00423 vel = (2*acc*delta - v0*v0) / (2*acc*TG - 2*v0);
00424 else
00425 if (-4*acc*delta + acc*acc*TG*TG + 2*acc*TG*v0 - v0*v0 >= 0)
00426 if (delta>0)
00427 {
00428
00429 vel = 0.5 * (acc*TG+v0 - sqrt(-4*acc*delta + acc*acc*TG*TG + 2*acc*TG*v0 - v0*v0));
00430 } else
00431 {
00432
00433 vel = 0.5 * (acc*TG+v0 + sqrt(-4*acc*delta + acc*acc*TG*TG + 2*acc*TG*v0 - v0*v0));
00434 }
00435 else
00436 {
00440
00443 vel = vmax;
00444 }
00445
00446 }
00447 else
00448 {
00452
00456 if (4*delta*delta - 4*delta*TG*v0 + 2*TG*TG*v0*v0 >=0)
00458 if (delta*v0 > 0)
00459 acc = ( -2.0*delta + TG*v0 + sqrt(4*delta*delta - 4*delta*TG*v0 + 2*TG*TG*v0*v0) ) / (TG*TG);
00460 else
00461 acc = ( -2.0*delta + TG*v0 - sqrt(4*delta*delta - 4*delta*TG*v0 + 2*TG*TG*v0*v0) ) / (TG*TG);
00462 else
00464 acc = amax;
00465
00466 vel = vmax;
00467 }
00468 }
00469 else if (T3 == 0)
00470 {
00474 acc = amax;
00475 vel = vmax;
00476 } else
00477 {
00479 acc = 0;
00480 vel = 0;
00481 }
00482 }
00486 }