robodyn_controllers: TrapezoidalVelocityTrajectory.cpp Source File

Go to the documentation of this file.
00001 #include <robodyn_controllers/TrapezoidalVelocityTrajectory.h>
00002 
00003 #include "nasa_common_logging/Logger.h"
00004 #include <tf/tf.h>
00005 #include <tf_conversions/tf_kdl.h>
00006 #include <limits>
00007 
00008 
00009 static const double eps = std::numeric_limits<float>::epsilon();
00010 
00011 TrapezoidalVelocityUtility::TrapezoidalVelocityUtility()
00012     : posInit(0.)
00013     , posFinal(0.)
00014     , velInit(0.)
00015     , velFinal(0.)
00016     , accelInit(0.)
00017     , accelFinal(0.)
00018     , t1(0.)
00019     , t2(0.)
00020     , t3(0.)
00021     , v1(0.)
00022     , a1(0.)
00023     , a2(0.)
00024     , velMax(0.)
00025     , accelMax(0.)
00026     , timeMin(0.)
00027 {
00028 }
00029 
00030 TrapezoidalVelocityUtility::~TrapezoidalVelocityUtility()
00031 {
00032 
00033 }
00034 
00035 void TrapezoidalVelocityUtility::setConstraints(double posInitIn, double posFinalIn, double velInitIn, double velFinalIn, double accelInitIn, double accelFinalIn)
00036 {
00037     posInit    = posInitIn;
00038     posFinal   = posFinalIn;
00039     velInit    = velInitIn;
00040     velFinal   = velFinalIn;
00041     accelInit  = accelInitIn;
00042     accelFinal = accelFinalIn;
00043 }
00044 
00045 double TrapezoidalVelocityUtility::duration(double vmaxIn, double amaxIn, bool spline)
00046 {
00047     velMax = vmaxIn;
00048     accelMax = amaxIn;
00049 
00050     if (fabs(posFinal - posInit) <= eps && fabs(velInit - velFinal) < eps)
00051     {
00053         v1 = velFinal;
00054         a1 = a2 = amaxIn;
00055         t1 = t2 = t3 = 0.;
00056 
00057         std::stringstream str;
00058         str << "duration t1 t2 t3 v1 a1 a2: " << t1 << " " << t2 << " " << t3 << " " << v1 << " " << a1 << " " << a2;
00059         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::DEBUG, str.str());
00060         return 0.;
00061     }
00062     else if (vmaxIn <= eps)
00063     {
00065         std::stringstream err;
00066         err << "duration() - move requested but velocity limited to zero";
00067         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::ERROR, err.str());
00068         throw std::runtime_error(err.str());
00069         return 0.;
00070     }
00071     else if (amaxIn <= eps)
00072     {
00074         std::stringstream err;
00075         err << "duration() - max acceleration limited to zero";
00076         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::ERROR, err.str());
00077         throw std::runtime_error(err.str());
00078         return 0.;
00079     }
00080     else if (fabs(velFinal) > vmaxIn + eps)
00081     {
00083         std::stringstream err;
00084         err << "duration() - final velocity larger than max";
00085         err << "\nvelFinal: " << velFinal;
00086         err << "\nvmaxIn: " << vmaxIn;
00087         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::ERROR, err.str());
00088         throw std::runtime_error(err.str());
00089         return 0.;
00090     }
00091     else
00092     {
00093         if (fabs(velInit) > vmaxIn)
00094         {
00096             vmaxIn = fabs(velInit);
00097         }
00098 
00099         if ((posFinal - posInit) >= 0)
00100         {
00102             v1 = vmaxIn;
00103             a1 = amaxIn;
00104             a2 = -amaxIn;
00105         }
00106         else
00107         {
00109             v1 = -vmaxIn;
00110             a1 = -amaxIn;
00111             a2 = amaxIn;
00112         }
00113 
00114         calculateTimeHelper();
00115 
00116         if (timesValid())
00117         {
00118             timeMin = t3;
00119             std::stringstream str;
00120             str << "duration t1 t2 t3 v1 a1 a2: " << t1 << " " << t2 << " " << t3 << " " << v1 << " " << a1 << " " << a2;
00121             NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::DEBUG, str.str());
00122             return t3;
00123         }
00124 
00125         if (calculateMaxVel() && fabs(v1) <= fabs(vmaxIn))
00126         {
00127             calculateTimeHelper();
00128 
00129             if (timesValid())
00130             {
00131                 timeMin = t3;
00132                 std::stringstream str;
00133                 str << "duration t1 t2 t3 v1 a1 a2: " << t1 << " " << t2 << " " << t3 << " " << v1 << " " << a1 << " " << a2;
00134                 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::DEBUG, str.str());
00135                 return t3;
00136             }
00137         }
00138 
00139         if (spline)
00140         {
00141             if ((posFinal - posInit) >= 0)
00142             {
00143                 v1 = -vmaxIn;
00144                 a1 = -amaxIn;
00145                 a2 = amaxIn;
00146             }
00147             else
00148             {
00149                 v1 = vmaxIn;
00150                 a1 = amaxIn;
00151                 a2 = -amaxIn;
00152             }
00153 
00154             calculateTimeHelper();
00155 
00156             if (timesValid())
00157             {
00158                 timeMin = t3;
00159                 std::stringstream str;
00160                 str << "duration t1 t2 t3 v1 a1 a2: " << t1 << " " << t2 << " " << t3 << " " << v1 << " " << a1 << " " << a2;
00161                 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::DEBUG, str.str());
00162                 return t3;
00163             }
00164 
00165             if (calculateMaxVel() && fabs(v1) <= fabs(vmaxIn))
00166             {
00167                 calculateTimeHelper();
00168 
00169                 if (timesValid())
00170                 {
00171                     timeMin = t3;
00172                     std::stringstream str;
00173                     str << "duration t1 t2 t3 v1 a1 a2: " << t1 << " " << t2 << " " << t3 << " " << v1 << " " << a1 << " " << a2;
00174                     NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::DEBUG, str.str());
00175                     return t3;
00176                 }
00177             }
00178         }
00179 
00180         std::stringstream err;
00181         err << std::setprecision (15);
00182         err << "Can not achieve (position, velocity): ";
00183         err << "initial (" << posInit << ", " << velInit << ") final (" << posFinal << ", " << velFinal << ") ";
00184         err << "with current velocity and acceleration limits: (" << fabs(vmaxIn) << ", " << fabs(a1) << ").  ";
00185         err << "Need at least " << (velFinal * velFinal - velInit * velInit) / (2 * (posFinal - posInit)) << " in acceleration.\n";
00186         err << "\n\ttimes: " << t1 << ", " << t2 << ", " << t3;
00187         err << "\n\tinputs: " << posInit << ", " << posFinal << ", " << velInit << ", " << velFinal;
00188         err << "\n\tconstraints: " << v1 << ", " << a1 << ", " << a2;
00189         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::ERROR, err.str());
00190         throw std::runtime_error(err.str());
00191 
00192     }
00193 
00194     return -1;
00195 
00196 }
00197 
00198 bool TrapezoidalVelocityUtility::timesValid()
00199 {
00200     if (t1 >= -eps && t2 >= -eps && t3 >= -eps && t2 >= t1 - eps && t3 >= t2 - eps)
00201     {
00202         return true;
00203     }
00204 
00205     return false;
00206 }
00207 void TrapezoidalVelocityUtility::setDuration(double durationIn)
00208 {
00209     NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::DEBUG, "in setDuration()");
00210 
00211     if ( fabs(posFinal - posInit) < eps && fabs(velFinal) < eps && fabs(velInit) < eps)
00212     {
00214         t1 = 0;
00215         t2 = durationIn;
00216         t3 = durationIn;
00217         return;
00218     }
00219 
00220     setDurationHelper(durationIn);
00221 
00222     if (t1 < -eps)
00223     {
00231         std::stringstream err;
00232         err << std::setprecision (15);
00233         err << "setDuration() - t1 < 0";
00234         err << "\n\t duration: " << durationIn;
00235         err << "\n\t times: " << t1 << ", " << t2 << ", " << t3;
00236         err << "\n\t inputs: " << posInit << ", " << posFinal << ", " << velInit << ", " << velFinal;
00237         err << "\n\t constraints: " << v1 << ", " << a1 << ", " << a2;
00238         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::DEBUG, err.str());
00239         a1 = -a1;
00240         setDurationHelper(durationIn);
00241     }
00242 
00243     if (t3 < t2)
00244     {
00251         std::stringstream err;
00252         err << std::setprecision (15);
00253         err << "setDuration() - t3 < t2";
00254         err << "\n\t duration: " << durationIn;
00255         err << "\n\t times: " << t1 << ", " << t2 << ", " << t3;
00256         err << "\n\t inputs: " << posInit << ", " << posFinal << ", " << velInit << ", " << velFinal;
00257         err << "\n\t constraints: " << v1 << ", " << a1 << ", " << a2;
00258         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::DEBUG, err.str());
00259         a2 = -a2;
00260         setDurationHelper(durationIn);
00261     }
00262 
00263     if (!timesValid())
00264     {
00266         std::stringstream err;
00267         err << std::setprecision (15);
00268         err << "setDuration() - Cannot achieve duration: " << durationIn;
00269         err << "\n\t times: " << t1 << ", " << t2 << ", " << t3;
00270         err << "\n\t inputs: " << posInit << ", " << posFinal << ", " << velInit << ", " << velFinal;
00271         err << "\n\t constraints: " << v1 << ", " << a1 << ", " << a2;
00272         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::ERROR, err.str());
00273         throw std::runtime_error(err.str());
00274         return;
00275     }
00276     else if (fabs(p(durationIn) - posFinal) > eps)
00277     {
00280         std::stringstream err;
00281         err << std::setprecision (15);
00282         err << "setDuration() - Duration not achievable for (position, velocity): ";
00283         err << "initial (" << posInit << ", " << velInit << ") final (" << posFinal << ", " << velFinal << ") ";
00284         err << "Minimum required duration: " << timeMin << ".\n";
00285         err << "\n\t duration: " << durationIn;
00286         err << "\n\t times: " << t1 << ", " << t2 << ", " << t3;
00287         err << "\n\t inputs: " << posInit << ", " << posFinal << ", " << velInit << ", " << velFinal;
00288         err << "\n\t constraints: " << v1 << ", " << a1 << ", " << a2;
00289         err << "\n\t cacluated final position: " << p(durationIn);
00290         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::ERROR, err.str());
00291         throw std::runtime_error(err.str());
00292         return;
00293     }
00294     else
00295     {
00296         std::stringstream str;
00297         str << "setDuration t1 t2 t3 v1 a1 a2: " << t1 << " " << t2 << " " << t3 << " " << v1 << " " << a1 << " " << a2;
00298         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::DEBUG, str.str());
00299     }
00300 }
00301 
00302 void TrapezoidalVelocityUtility::setDurationHelper(double durationIn)
00303 {
00304 
00305     if (fabs(a1) < eps || fabs(a2) < eps)
00306     {
00308         std::stringstream err;
00309         err << "setDurationHelper() a1 or a2 == 0";
00310         err << "\n\tt1 t2 t3 v1 a1 a2: " << t1 << " " << t2 << " " << t3 << " " << v1 << " " << a1 << " " << a2;
00311         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::ERROR, err.str());
00312         throw std::runtime_error(err.str());
00313         return;
00314     }
00315 
00316     double a = 0.5 * (1 / a2 - 1 / a1);
00317     double b = velInit / a1 - velFinal / a2 + durationIn;
00318     double c = 0.5 * (velFinal * velFinal / a2 - velInit * velInit / a1) + posInit - posFinal;
00319     double d = b * b - 4 * a * c;
00320 
00321     if (d < eps)
00322     {
00323         d = 0;
00324     }
00325 
00326     if (fabs(b) <= eps)
00327     {
00328         t1 = t2 = 0.;
00329     }
00330     else if (fabs(a) <= eps)
00331     {
00332         double vel = -c / b;
00333         t1         = (vel - velInit) / a1;
00334         t2         = durationIn - (velFinal - vel) / a2;
00335     }
00336     else
00337     {
00338         double vel1 = (-b - sqrt(d)) / (2 * a);
00339         double vel2 = (-b + sqrt(d)) / (2 * a);
00340         t1          = (vel2 - velInit) / a1;
00341         t2          = (vel1 - velInit) / a1;
00342     }
00343 
00344     t3 = durationIn;
00345 
00346     if (t1 == 0)
00347     {
00348         v1 = velFinal - a2 * (t3 - t2);
00349     }
00350     else
00351     {
00352         v1 = velInit + a1 * t1;
00353     }
00354 }
00355 
00356 void TrapezoidalVelocityUtility::getTimes(double& t1Out, double& t2Out, double& t3Out) const
00357 {
00358     t1Out = t1;
00359     t2Out = t2;
00360     t3Out = t3;
00361     std::stringstream str;
00362     str << "getTimes t1 t2 t3 v1 a1 a2: " << t1 << " " << t2 << " " << t3 << " " << v1 << " " << a1 << " " << a2;
00363     NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::DEBUG, str.str());
00364 }
00365 
00366 void TrapezoidalVelocityUtility::calculateTimeHelper()
00367 {
00368     if (fabs(a1) < eps || fabs(a2) < eps)
00369     {
00371         std::stringstream err;
00372         err << "calculateTimeHelper() a1 or a2 == 0";
00373         err << "\n\tt1 t2 t3 v1 a1 a2: " << t1 << " " << t2 << " " << t3 << " " << v1 << " " << a1 << " " << a2;
00374         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::ERROR, err.str());
00375         throw std::runtime_error(err.str());
00376         return;
00377     }
00378 
00379     t1 = (v1 - velInit) / a1; //time to reach target velocity using max acceleration
00380     t3 = (velFinal - v1) / a2; //time to reach final velocity using max acceleration
00381 
00382     double temp = posFinal - (posInit + velInit * t1 + (a1 / 2) * t1 * t1 + v1 * t3 + (a2 / 2) * t3 * t3); //distance unaccounted for
00383 
00384     if (fabs(temp) < eps )
00385     {
00386         // don't need a constant velocity
00387         t2 = t1;
00388     }
00389     else if (fabs(v1) < eps)
00390     {
00391         // invalid solution
00392         t2 = -1.;
00393     }
00394     else
00395     {
00396         t2 = temp / v1 + t1;
00397     }
00398 
00399     t3 += t2;
00400 }
00401 
00402 bool TrapezoidalVelocityUtility::calculateMaxVel()
00403 {
00404     if (fabs(a1) < eps || fabs(a2) < eps)
00405     {
00407         std::stringstream err;
00408         err << "calculateMaxVel() a1 or a2 == 0";
00409         err << "\n\tt1 t2 t3 v1 a1 a2: " << t1 << " " << t2 << " " << t3 << " " << v1 << " " << a1 << " " << a2;
00410         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::ERROR, err.str());
00411         throw std::runtime_error(err.str());
00412         return false;
00413     }
00414 
00415     double temp = (-posFinal + posInit + velFinal * velFinal / (2 * a2) - velInit * velInit / (2 * a1)) * a2;
00416 
00417     if (temp < 0.)
00418     {
00419         return false;
00420     }
00421     else
00422     {
00423         if (std::signbit(v1))
00424         {
00425             v1 = -sqrt(temp);
00426         }
00427         else
00428         {
00429             v1 = sqrt(temp);
00430         }
00431 
00432         return true;
00433     }
00434 }
00435 
00436 double TrapezoidalVelocityUtility::p(double t) const
00437 {
00438     if (t <= t1)
00439     {
00440         return (a1 / 2) * t * t + velInit * t + posInit;
00441     }
00442     else if (t <= t2)
00443     {
00444         return posInit + (a1 / 2) * t1 * t1 + velInit * t1 + v1 * (t - t1);
00445     }
00446     else
00447     {
00448         return posInit + (a1 / 2) * t1 * t1 + velInit * t1 + v1 * (t - t1) + (a2 / 2) * (t - t2) * (t - t2);
00449     }
00450 }
00451 
00452 double TrapezoidalVelocityUtility::v(double t) const
00453 {
00454     if (t <= t1)
00455     {
00456         return a1 * t + velInit;
00457     }
00458     else if (t <= t2)
00459     {
00460         return v1;
00461     }
00462     else
00463     {
00464         return v1 + a2 * (t - t2);
00465     }
00466 }
00467 
00468 double TrapezoidalVelocityUtility::a(double t) const
00469 {
00470     if (t <= t1 && t1 > 0)
00471     {
00472         return a1;
00473     }
00474     else if (t <= t2)
00475     {
00476         return 0.;
00477     }
00478     else
00479     {
00480         return a2;
00481     }
00482 }
00483 
00484 void TrapezoidalVelocityUtility::setTimes(double t1In, double t2In, double t3In)
00485 {
00486     t1       = t1In;
00487     t2       = t2In - t1In;
00488     t3       = t3In - t2In;
00489     double A = posFinal - posInit - (velInit * t1 + velFinal * t3) / 2.;
00490     double B = t3 / 2. + t2 + t1 / 2.;
00491 
00492     if (B > eps)
00493     {
00494         v1 = A / B;
00495     }
00496 
00497     if (t1 > eps)
00498     {
00499         a1 = (v1 - velInit) / t1;
00500     }
00501 
00502     if (t3 > eps)
00503     {
00504         a2 = (velFinal - v1) / t3;
00505     }
00506 
00507     t2 += t1;
00508     t3 += t2;
00509     std::stringstream str;
00510     str << "setTimes t1 t2 t3 v1 a1 a2: " << t1 << " " << t2 << " " << t3 << " " << v1 << " " << a1 << " " << a2;
00511     NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityUtility", log4cpp::Priority::DEBUG, str.str());
00512 }
00513 
00514 TrapezoidalVelocityJointTrajectory::TrapezoidalVelocityJointTrajectory(const JointMotionLimiter& jml, const KDL::JntArrayAcc& startPose,
00515         const KDL::JntArrayAcc& goalPose, double durationIn)
00516     : JointTrajectory(), JointMotionLimiter(jml)
00517 {
00518     if (startPose.q.rows() != goalPose.q.rows())
00519     {
00520         std::stringstream err;
00521         err << "TrapezoidalVelocityJointTrajectory::TrapezoidalVelocityJointTrajectory() - startPose and goalPose lengths don't match";
00522         NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityJointTrajectory", log4cpp::Priority::ERROR, err.str());
00523         throw std::runtime_error(err.str());
00524         return;
00525     }
00526 
00527     utils.resize(startPose.q.rows());
00528 
00529     for (unsigned int i = 0; i < utils.size(); ++i)
00530     {
00531         utils[i].setConstraints(startPose.q(i), goalPose.q(i), startPose.qdot(i), goalPose.qdot(i), startPose.qdotdot(i), goalPose.qdotdot(i));
00532 
00533         double jointDur = utils[i].duration(getVelocityLimit(i), getAccelerationLimit(i));
00534 
00535         if (jointDur > durationIn)
00536         {
00537             durationIn = jointDur;
00538         }
00539     }
00540 
00541     setDuration(durationIn);
00542 }
00543 
00544 TrapezoidalVelocityJointTrajectory::~TrapezoidalVelocityJointTrajectory()
00545 {
00546 
00547 }
00548 
00549 void TrapezoidalVelocityJointTrajectory::setDuration(double durationIn)
00550 {
00551     JointTrajectory::setDuration(durationIn);
00552 
00553     double maxT1 = 0.;
00554     double maxT2 = 0.;
00555     double maxT3 = 0.;
00556     double t1, t2, t3;
00557     unsigned int constraintAxis = 0;
00558 
00559     if (utils.empty())
00560     {
00561         throw std::runtime_error("TrapezoidalVelocityJointTrajectory::setDuration cannot set duration for empty list");
00562     }
00563 
00564     for (unsigned int i = 0; i < utils.size(); ++i)
00565     {
00566         utils[i].getTimes(t1, t2, t3);
00567 
00568         if (t3 > maxT3)
00569         {
00570             constraintAxis = i;
00571             maxT1          = t1;
00572             maxT2          = t2;
00573             maxT3          = t3;
00574         }
00575     }
00576 
00577     std::stringstream msg;
00578     msg << "setDuration() constraint axis: " << constraintAxis;
00579     NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityJointTrajectory", log4cpp::Priority::DEBUG, msg.str());
00580 
00581     if (durationIn > maxT3)
00582     {
00583         utils[constraintAxis].setDuration(durationIn);
00584         utils[constraintAxis].getTimes(maxT1, maxT2, maxT3);
00585     }
00586 
00587     for (unsigned int i = 0; i < utils.size(); ++i)
00588     {
00589         utils[i].setTimes(maxT1, maxT2, maxT3);
00590     }
00591 }
00592 void TrapezoidalVelocityJointTrajectory::getPose(double time, KDL::JntArrayAcc& pose)
00593 {
00594     if (getDuration() == 0 && time > 0)
00595     {
00596         throw std::runtime_error("TrapezoidalVelocityJointTrajectory::getPose cannot get a pose for a zero duration trajectory with nonzero time");
00597         return;
00598     }
00599 
00600     pose.resize(utils.size());
00601 
00602     for (unsigned int i = 0; i < utils.size(); ++i)
00603     {
00604         pose.q(i)       = utils[i].p(time);
00605         pose.qdot(i)    = utils[i].v(time);
00606         pose.qdotdot(i) = utils[i].a(time);
00607     }
00608 }
00609 
00610 TrapezoidalVelocityCartesianTrajectory::TrapezoidalVelocityCartesianTrajectory(const CartesianMotionLimiter& cml,
00611         const KDL::FrameAcc& startPose, const KDL::FrameAcc& goalPose, double durationIn)
00612     : CartesianTrajectory(), CartesianMotionLimiter(cml)
00613 {
00614     utils.resize(7);
00615 
00616     // linear
00617     double dur;
00618 
00619     for (unsigned int i = 0; i < 3; ++i)
00620     {
00621         utils[i].setConstraints(startPose.p.p(i), goalPose.p.p(i), startPose.p.v(i), goalPose.p.v(i), startPose.p.dv(i), startPose.p.dv(i));
00622         dur = utils[i].duration(getLinearVelocityLimit(), getLinearAccelerationLimit());
00623 
00624         if (dur > durationIn) { durationIn = dur; }
00625     }
00626 
00627     // angular
00628     double x, y, z, w;
00629 
00630     startPose.M.R.GetQuaternion(x, y, z, w);
00631     startQ.setValue(x, y, z, w);
00632     goalPose.M.R.GetQuaternion(x, y, z, w);
00633     goalQ.setValue(x, y, z, w);
00634 
00635     if (startQ.dot(goalQ) < 0.)
00636     {
00637         // reduce to the short angle case
00638         goalQ = tf::Quaternion(-goalQ.x(), -goalQ.y(), -goalQ.z(), -goalQ.w());
00639     }
00640 
00641     // get init and final rot vels
00642     tf::Quaternion startVel(startPose.M.w.x(), startPose.M.w.y(), startPose.M.w.z(), 0.);
00643     startVel = startVel * startQ * 0.5;
00644     tf::Quaternion goalVel(goalPose.M.w.x(), goalPose.M.w.y(), goalPose.M.w.z(), 0.);
00645     goalVel = goalVel * goalQ * 0.5;
00646 
00647     // quaternion x
00648     utils[3].setConstraints(startQ.x(), goalQ.x(), startVel.x(), goalVel.x(), 0., 0.);
00649     utils[4].setConstraints(startQ.y(), goalQ.y(), startVel.y(), goalVel.y(), 0., 0.);
00650     utils[5].setConstraints(startQ.z(), goalQ.z(), startVel.z(), goalVel.z(), 0., 0.);
00651     utils[6].setConstraints(startQ.w(), goalQ.w(), startVel.w(), goalVel.w(), 0., 0.);
00652 
00653     for (unsigned int i = 3; i < 7; ++i)
00654     {
00655         dur = utils[i].duration(getRotationalVelocityLimit(), getRotationalAccelerationLimit());
00656 
00657         if (dur > durationIn) { durationIn = dur; }
00658     }
00659 
00660     setDuration(durationIn);
00661 }
00662 
00663 TrapezoidalVelocityCartesianTrajectory::~TrapezoidalVelocityCartesianTrajectory()
00664 {
00665 
00666 }
00667 
00668 void TrapezoidalVelocityCartesianTrajectory::setDuration(double durationIn)
00669 {
00670     CartesianTrajectory::setDuration(durationIn);
00671 
00672     double maxT1 = 0.;
00673     double maxT2 = 0.;
00674     double maxT3 = 0.;
00675     double t1, t2, t3;
00676     unsigned int constraintAxis = 0;
00677 
00678     if (utils.empty())
00679     {
00680         throw std::runtime_error("TrapezoidalVelocityCartesianTrajectory::setDuration cannot set duration for empty list");
00681     }
00682 
00683     for (unsigned int i = 0; i < utils.size(); ++i)
00684     {
00685         utils[i].getTimes(t1, t2, t3);
00686 
00687         if (t3 > maxT3)
00688         {
00689             constraintAxis = i;
00690             maxT1          = t1;
00691             maxT2          = t2;
00692             maxT3          = t3;
00693         }
00694     }
00695 
00696     if (durationIn > maxT3)
00697     {
00698         utils[constraintAxis].setDuration(durationIn);
00699         utils[constraintAxis].getTimes(maxT1, maxT2, maxT3);
00700     }
00701 
00702     for (unsigned int i = 0; i < utils.size(); ++i)
00703     {
00704         utils[i].setTimes(maxT1, maxT2, maxT3);
00705     }
00706 }
00707 void TrapezoidalVelocityCartesianTrajectory::getPose(double time, KDL::FrameAcc& pose)
00708 {
00709     pose.p.p.x(utils[0].p(time));
00710     pose.p.p.y(utils[1].p(time));
00711     pose.p.p.z(utils[2].p(time));
00712     tf::Quaternion stepQ(utils[3].p(time), utils[4].p(time), utils[5].p(time), utils[6].p(time));
00713     stepQ.normalize();
00714     pose.M = KDL::Rotation::Quaternion(stepQ.x(), stepQ.y(), stepQ.z(), stepQ.w());
00715 }