moveCommand.cpp
Go to the documentation of this file.
00001 
00060 // standard includes
00061 #include <math.h>
00062 #include <stddef.h>
00063 
00064 // own includes
00065 #include <schunk_powercube_chain/moveCommand.h>
00066 
00067 //ToDo : ? Funktion
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                 // RampCommand::RampCommand(double x0, double v0, double xtarget, double amax, double vmax)
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; // x = x0 + v0t + a/2*t^2
00258                 } else {
00259                         //std::cout << "in getPos(double)\n";
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; // x = x0 + v0t + a/2*t^2
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); // x = x1 + v2*t
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                         // x = x2 + v2t + a/2*t^2
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         //std::cout << "getTotalTime()\n";
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         //std::ostream& out(debug);
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                 //double d2 = 0.5 * TG * v0;
00356                 double d3 = 0.5 * v0 * v0 / amax;
00357                 //out << "d1:\t" << d1 << "\n";
00358                 //out << "d2h:\t" << d2h << "\n";
00359                 //out << "d2l:\t" << d2l << "\n";
00360                 //out << "d3:\t" << d3 << "\n";
00361                 //out << "delta:\t" << delta << "\n";
00362                 
00363                 if (T3 > 0)
00364                 {
00366                         if (fabs(delta) >= d1)
00367                         {
00368                                 /* out << "  Case 1\n"; */
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                                 //out << "  a=" << a << " | b=" << b << " | c=" << c << endl;
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                                 /* out << "  Case 2\n"; */
00391                                 /*          \                           */
00392                                 /*       ----------             */
00393                                 /*                 \    */
00395                                 double a = TG;
00396                                 double b = - delta - T3*v0;
00397                                 double c = 0.5 * v0 * v0 * T3;
00398                                 //out << "  a=" << a << " | b=" << b << " | c=" << c << endl;
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                                                         // delta>0 (-> a>0), v0 < 0:
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                                                         // delta<0 (-> a<0), v0 > 0:
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 }


schunk_powercube_chain
Author(s): Florian Weisshardt
autogenerated on Thu Aug 27 2015 15:06:58