trajectory.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00032 #include "pr2_mechanism_controllers/trajectory.h"
00033 #include <angles/angles.h>
00034 #include <cstdlib>
00035 
00036 #define MAX_ALLOWABLE_TIME 1.0e8
00037 #define EPS_TRAJECTORY 1.0e-8
00038 
00039 #define MAX_NUM_POINTS 1000
00040 #define MAX_COEFF_SIZE 5
00041 
00042 using namespace trajectory;
00043 
00044 Trajectory::Trajectory(int dimension): dimension_(dimension)
00045 {
00046   interp_method_ = "linear";
00047   autocalc_timing_ = false;
00048   init(MAX_NUM_POINTS,dimension);
00049 }
00050 
00051 void Trajectory::init(int num_points, int dimension)
00052 {
00053   tp_.resize(num_points,dimension);
00054   tc_.resize(num_points-1);
00055   min_limit_.resize(dimension);
00056   max_limit_.resize(dimension);
00057   max_rate_.resize(dimension);
00058   max_acc_.resize(dimension);
00059   joint_wraps_.resize(dimension);
00060   for(int i=0; i<num_points; i++)
00061   {
00062     tp_[i].setDimension(dimension);
00063   }
00064 
00065   for(int i=0; i<num_points-1; i++)
00066   {
00067     tc_[i].coeff_.resize(dimension);
00068     for(int j=0; j < dimension; j++)
00069       tc_[i].coeff_[j].resize(MAX_COEFF_SIZE);
00070   }
00071   for(int i=0; i < dimension; i++)
00072   {
00073     joint_wraps_[i] = false;
00074   }
00075 }
00076 
00077 void Trajectory::clear()
00078 {
00079   tp_.resize(0);
00080   tc_.resize(0);
00081   min_limit_.resize(0);
00082   max_limit_.resize(0);
00083   max_rate_.resize(0);
00084   max_acc_.resize(0);
00085 }
00086 
00087 int Trajectory::setTrajectory(const std::vector<TPoint>& tp)
00088 {
00089   if(tp.size() < 2)
00090   {
00091     ROS_WARN("Trying to set trajectory with number of points <= 0");
00092     return -1;
00093   }
00094   if(tp.begin()->dimension_ != dimension_)
00095   {
00096     ROS_WARN("Dimension of trajectory point %d does not match dimension of trajectory %d",tp[0].dimension_, dimension_);
00097     return -1;
00098   }
00099 
00100 //  ROS_INFO("Initializing trajectory with %d points",tp.size());
00101 
00102   num_points_ = tp.size();
00103 
00104 //  tp_.resize(num_points_);
00105 
00106   for(int i=0; i<num_points_; i++)
00107   {
00108 //    tp_[i].setDimension(dimension_);
00109     tp_[i] = tp[i];
00110 
00111     for(int j=0; j<dimension_; j++)
00112     {
00113       if(joint_wraps_[j])
00114       {
00115 //        ROS_INFO("Wrapping joint angle for joint: %d",j);
00116         tp_[i].q_[j] = angles::normalize_angle(tp_[i].q_[j]);
00117       }
00118     }
00119 //    ROS_INFO("Input point: %d is ",i);
00120 //    for(int j=0; j < dimension_; j++)
00121 //      ROS_INFO("%f ",tp_[i].q_[j]);
00122 
00123 //    ROS_INFO(" ");
00124   }
00125 
00126   parameterize();
00127   return 1;
00128 }  
00129 
00130 int Trajectory::setTrajectory(const std::vector<double> &p, int numPoints)
00131 {
00132   num_points_ = numPoints;
00133 
00134   if((int) p.size() < num_points_*dimension_)
00135   {
00136     ROS_WARN("Input has only %zd values, expecting %d values for a %d dimensional trajectory with %d number of points",p.size(), num_points_*dimension_, dimension_, num_points_);
00137     return -1;
00138   }   
00139   autocalc_timing_ = true;//Enable autocalc timing by default since no time information given in trajectory
00140 //tp_.resize(num_points_);
00141 
00142   for(int i=0; i<num_points_;i++)
00143   {
00144 //    tp_[i].setDimension(dimension_);
00145     tp_[i].time_ = 0.0;
00146     for(int j=0; j<dimension_; j++)
00147     {
00148       tp_[i].q_[j] = p[i*dimension_+j];
00149       tp_[i].qdot_[j] = 0.0;
00150     }
00151 
00152 //    ROS_INFO("Input point: %d is ",i);
00153 //    for(int j=0; j < dimension_; j++)
00154 //      ROS_INFO("%f ",tp_[i].q_[j]);
00155 
00156 //    ROS_INFO(" ");
00157 
00158   }
00159   parameterize();
00160   return 1;
00161 }  
00162 
00163 void Trajectory::setJointWraps(int index)
00164 {
00165   if(index > dimension_)
00166   {
00167     ROS_ERROR("Index exceeds number of joints");
00168     return;
00169   }
00170   joint_wraps_[index] = true;
00171 }
00172 
00173 double Trajectory::jointDiff(double from, double to, int index)
00174 {
00175   if(joint_wraps_[index])
00176   {
00177 //    ROS_INFO("Returning shortest angular difference from: %f and to: %f",from,to);
00178     return angles::shortest_angular_distance(from,to);
00179   }
00180   else
00181   {
00182 //    ROS_INFO("Returning angular difference from: %f and to: %f",from,to);
00183     return (to-from);
00184   }
00185 }
00186 
00187 int Trajectory::setTrajectory(const std::vector<double> &p, const std::vector<double> &time, int numPoints)
00188 {
00189   num_points_ = numPoints;
00190 
00191 //  tp_.resize(num_points_);
00192 
00193   if((int) time.size() != num_points_)
00194   {
00195     ROS_WARN("Number of points in vector specifying time (%d)  does not match number of points %d",(int) time.size(), num_points_);
00196     return -1;
00197   }
00198   if((int) p.size() < num_points_*dimension_)
00199   {
00200     ROS_WARN("Input has only %zd values, expecting %d values for a %d dimensional trajectory with %d number of points",p.size(), num_points_*dimension_, dimension_, num_points_);
00201     return -1;
00202   }   
00203 
00204   for(int i=0; i<num_points_;i++)
00205   {
00206 //    tp_[i].setDimension(dimension_);
00207     tp_[i].time_ = time[i];
00208     for(int j=0; j<dimension_; j++)
00209     {
00210       tp_[i].q_[j] = p[i*(dimension_)+j];
00211     }
00212   }
00213 
00214   parameterize();
00215 
00216   return 1;
00217 }
00218 
00219 int Trajectory::setTrajectory(const std::vector<double> &p, const std::vector<double> &pdot, const std::vector<double> &time, int numPoints)
00220 {
00221   num_points_ = numPoints;
00222 
00223 //  tp_.resize(num_points_);
00224 
00225   if((int) time.size() != num_points_)
00226   {
00227     ROS_WARN("Number of points in vector specifying time (%d)  does not match number of points %d",(int) time.size(), num_points_);
00228     return -1;
00229   }
00230   if((int) p.size() < num_points_*dimension_)
00231   {
00232     ROS_WARN("Input has only %zd values, expecting %d values for a %d dimensional trajectory with %d number of points",p.size(), num_points_*dimension_, dimension_, num_points_);
00233     return -1;
00234   }   
00235 
00236   for(int i=0; i<num_points_;i++)
00237   {
00238 //    tp_[i].setDimension(dimension_);
00239     tp_[i].time_ = time[i];
00240     for(int j=0; j<dimension_; j++)
00241     {
00242       tp_[i].q_[j] = p[i*dimension_+j];
00243       tp_[i].qdot_[j] = pdot[i*dimension_+j];
00244     }
00245   }
00246   parameterize();
00247   return 1;
00248 }
00249 
00250 void Trajectory::addPoint(const TPoint tp)
00251 {
00252   double time = tp.time_;
00253 
00254   int index = findTrajectorySegment(time);
00255   std::vector<TPoint>::iterator it = tp_.begin() + index;
00256   tp_.insert(it,tp);
00257   num_points_++;
00258   parameterize();
00259 }
00260 
00261 int Trajectory::findTrajectorySegment(double time)
00262 {
00263   int result = 0;
00264 
00265   while(time > tp_[result+1].time_)
00266     result++;
00267 
00268   return result;
00269 }
00270 
00271 //inline double Trajectory::getTotalTime()
00272 double Trajectory::getTotalTime()
00273 {
00274   if(num_points_ == 0)
00275     return 0.0;
00276 
00277 //  return (tp_.back().time_ - tp_.front().time_);
00278   return (lastPoint().time_ - tp_.front().time_);
00279 }
00280 
00281 
00282 const Trajectory::TPoint& Trajectory::lastPoint()
00283 {
00284   return *(tp_.begin()+num_points_-1);
00285 }
00286 
00287 int Trajectory::sample(TPoint &tp, double time)
00288 {
00289 //  ROS_INFO("Trajectory has %d points",num_points_);
00290 //  ROS_INFO("Time: %f, %f, %f",time,tp_.front().time_,lastPoint().time_);
00291   if(time > lastPoint().time_)
00292   {
00293 //    ROS_WARN("Invalid input sample time.");
00294     time = lastPoint().time_;
00295   }
00296   else if( time < tp_.front().time_)
00297   {
00298     time = tp_.front().time_;
00299 //    ROS_WARN("Invalid input sample time.");
00300   }
00301 
00302   if((int) tp.q_.size() != dimension_ || (int) tp.qdot_.size() != dimension_)
00303   {
00304     ROS_WARN("Dimension of sample point passed in = %zd does not match dimension of trajectory = %d",tp.q_.size(),dimension_);
00305     return -1;
00306   } 
00307   int segment_index = findTrajectorySegment(time);
00308 //  ROS_INFO("segment index : %d",segment_index);
00309   if(interp_method_ == std::string("linear"))
00310     sampleLinear(tp,time,tc_[segment_index],tp_[segment_index].time_);
00311   else if(interp_method_ == std::string("cubic"))
00312     sampleCubic(tp,time,tc_[segment_index],tp_[segment_index].time_);
00313   else if(interp_method_ == std::string("blended_linear"))
00314     sampleBlendedLinear(tp,time,tc_[segment_index],tp_[segment_index].time_);
00315   else
00316     ROS_WARN("Unrecognized interp_method type: %s\n",interp_method_.c_str());
00317 
00318   return 1;
00319 }
00320 
00321 /*
00322 int Trajectory::sample(std::vector<TPoint> &tp, double start_time, double end_time)
00323 {
00324   if(time > lastPoint().time_ || time < tp_.front().time_)
00325   {
00326     ROS_WARN("Invalid input sample time.");
00327     return -1;
00328   }
00329   if((int) tp.q_.size() != dimension_ || (int) tp.qdot_.size() != dimension_)
00330   {
00331     ROS_WARN("Dimension of sample point passed in = %d does not match dimension of trajectory = %d",tp.q_.size(),dimension_);
00332     return -1;
00333   } 
00334   int segment_index = findTrajectorySegment(time);
00335 
00336   if(interp_method_ == std::string("linear"))
00337     sampleLinear(tp,time,tc_[segment_index],tp_[segment_index].time_);
00338   else if(interp_method_ == std::string("cubic"))
00339     sampleCubic(tp,time,tc_[segment_index],tp_[segment_index].time_);
00340   else if(interp_method_ == std::string("blended_linear"))
00341     sampleBlendedLinear(tp,time,tc_[segment_index],tp_[segment_index].time_);
00342   else
00343     ROS_WARN("Unrecognized interp_method type: %s\n",interp_method_.c_str());
00344 
00345   return 1;
00346 }
00347 */
00348 
00349 int Trajectory::setMaxRates(std::vector<double> max_rate)
00350 {
00351   if((int) max_rate.size() != dimension_)
00352   {
00353     ROS_WARN("Input size: %zd does not match dimension of trajectory = %d",max_rate.size(),dimension_);
00354     return -1;
00355   }
00356 //  max_rate.resize(dimension_);
00357   for(int i=0; i < dimension_; i++)
00358     max_rate_[i] = max_rate[i]; 
00359 
00360   max_rate_set_ = true;
00361 
00362   return 1;
00363 }
00364 
00365 int Trajectory::setMaxAcc(std::vector<double> max_acc)
00366 {
00367   if((int) max_acc.size() != dimension_)
00368   {
00369     ROS_WARN("Input size: %zd does not match dimension of trajectory = %d",max_acc.size(),dimension_);
00370     return -1;
00371   }
00372 //  max_acc.resize(dimension_);
00373   for(int k=0; k < dimension_; k++)
00374     max_acc_[k] = max_acc[k]; 
00375 
00376   max_acc_set_ = true;
00377 
00378   return 1;
00379 }
00380 
00381 int Trajectory::minimizeSegmentTimes()
00382 {
00383   int error_code = -1;
00384   if(interp_method_ == std::string("linear"))
00385      error_code = minimizeSegmentTimesWithLinearInterpolation();
00386   else if(interp_method_ == std::string("cubic"))
00387      error_code = minimizeSegmentTimesWithCubicInterpolation();
00388   else if(interp_method_ == std::string("blended_linear"))
00389      error_code = minimizeSegmentTimesWithBlendedLinearInterpolation();
00390   else
00391     ROS_WARN("minimizeSegmentTimes:: Unrecognized interp_method type: %s\n",interp_method_.c_str());
00392 
00393   return error_code;
00394 }
00395 
00396 int Trajectory::minimizeSegmentTimesWithLinearInterpolation()
00397 {
00398   double dT(0);
00399 
00400 /*  TCoeff tc;
00401   std::vector<std::vector<double> > coeff;
00402   std::vector<double> temp;
00403   temp.resize(2);
00404   tc.degree_ = 1;
00405   tc.dimension_ = dimension_;
00406   tc_.clear();
00407 */
00408 
00409   double temp[2];
00410 
00411   if(!max_rate_set_ || (int) max_rate_.size() < 0)
00412   {
00413     ROS_WARN("Trying to apply rate limits without setting max rate information. Use setMaxRate first");
00414     return -1;
00415   }
00416 
00417   for(int i=1; i < (int) num_points_ ; i++)
00418   {
00419 //    tc.coeff_.clear();
00420     dT = calculateMinimumTimeLinear(tp_[i-1],tp_[i]);
00421     tp_[i].time_ = tp_[i-1].time_ + dT;
00422 
00423 //    tc.duration_ = dT;
00424     tc_[i-1].duration_ = dT;
00425 
00426     for(int j=0; j < dimension_; j++)
00427     {
00428       temp[0] = tp_[i-1].q_[j];
00429 //      temp[1] = (tp_[i].q_[j] - tp_[i-1].q_[j])/tc_[i-1].duration_;  
00430       temp[1] = jointDiff(tp_[i-1].q_[j],tp_[i].q_[j],j)/tc_[i-1].duration_;  
00431 
00432       tc_[i-1].coeff_[j][0] = temp[0];
00433       tc_[i-1].coeff_[j][1] = temp[1];
00434       tc_[i-1].degree_ = 1;
00435       tc_[i-1].dimension_ = dimension_;
00436 
00437 //      tc.coeff_.push_back(temp);
00438     }
00439 //    tc_.push_back(tc);
00440   }
00441   return 1;
00442 }
00443 
00444 int Trajectory::minimizeSegmentTimesWithCubicInterpolation()
00445 {
00446   double dT(0);
00447 
00448 /*  TCoeff tc;
00449   std::vector<double> temp;
00450   temp.resize(4);
00451   tc.degree_ = 1;
00452   tc.dimension_ = dimension_;
00453   tc_.clear();
00454 */
00455 
00456   double temp[4];
00457 
00458     if(!max_rate_set_ || (int) max_rate_.size() < 1)
00459     {
00460       ROS_WARN("Trying to apply rate limits without setting max rate information. Use setMaxRate first");
00461       return -1;
00462     }
00463 
00464   for(int i=1; i < (int) num_points_ ; i++)
00465   {
00466 //    tc.coeff_.clear();
00467     dT = calculateMinimumTimeCubic(tp_[i-1],tp_[i]);
00468     tp_[i].time_ = tp_[i-1].time_ + dT;
00469 //    tc.duration_ = dT;
00470     tc_[i-1].duration_ = dT;
00471 
00472     for(int j=0; j < dimension_; j++)
00473     {
00474       double diff = jointDiff(tp_[i-1].q_[j],tp_[i].q_[j],j);
00475       temp[0] = tp_[i-1].q_[j];
00476       temp[1] = tp_[i-1].qdot_[j];
00477 //      temp[2] = (3*(tp_[i].q_[j]-tp_[i-1].q_[j])-(2*tp_[i-1].qdot_[j]+tp_[i].qdot_[j])*tc_[i-1].duration_)/(tc_[i-1].duration_*tc_[i-1].duration_);
00478 //      temp[3] = (2*(tp_[i-1].q_[j]-tp_[i].q_[j])+(tp_[i-1].qdot_[j]+tp_[i].qdot_[j])*tc_[i-1].duration_)/(pow(tc_[i-1].duration_,3));
00479       temp[2] = (3*diff-(2*tp_[i-1].qdot_[j]+tp_[i].qdot_[j])*tc_[i-1].duration_)/(tc_[i-1].duration_*tc_[i-1].duration_);
00480       temp[3] = (-2*diff+(tp_[i-1].qdot_[j]+tp_[i].qdot_[j])*tc_[i-1].duration_)/(pow(tc_[i-1].duration_,3));
00481 
00482       tc_[i-1].coeff_[j][0] = temp[0];
00483       tc_[i-1].coeff_[j][1] = temp[1];
00484       tc_[i-1].coeff_[j][2] = temp[2];
00485       tc_[i-1].coeff_[j][3] = temp[3];
00486       tc_[i-1].degree_ = 1;
00487       tc_[i-1].dimension_ = dimension_;
00488 
00489 //      tc.coeff_.push_back(temp);
00490     }
00491 //    tc_.push_back(tc);
00492   }
00493   return 1;
00494 }
00495 
00496 
00497 int Trajectory::minimizeSegmentTimesWithBlendedLinearInterpolation()
00498 {
00499    double dT(0),acc(0.0),tb(0.0);
00500 
00501 /*  TCoeff tc;
00502   std::vector<double> temp;
00503   temp.resize(5);
00504   tc.degree_ = 1;
00505   tc.dimension_ = dimension_;
00506   tc_.clear();
00507 */
00508 
00509   double temp[5];
00510 
00511     if(!max_rate_set_ || (int) max_rate_.size() != dimension_ || !max_acc_set_ || (int) max_acc_.size() != dimension_)
00512     {
00513       ROS_WARN("Trying to apply rate and acc limits without setting them. Use setMaxRate and setMaxAcc first");
00514       return -1;
00515     }
00516 
00517   for(int i=1; i < (int) num_points_ ; i++)
00518   {
00519 //    tc.coeff_.clear();
00520     dT = calculateMinimumTimeLSPB(tp_[i-1],tp_[i]);
00521     tp_[i].time_ = tp_[i-1].time_ + dT;
00522 //    tc.duration_ = dT;
00523     tc_[i-1].duration_ = dT;
00524     for(int j=0; j < dimension_; j++)
00525     {
00526       double diff = jointDiff(tp_[i-1].q_[j],tp_[i].q_[j],j);
00527 //       if(tp_[i].q_[j]-tp_[i-1].q_[j] > 0)
00528       if(diff > 0)
00529           acc = max_acc_[j];
00530        else
00531           acc = -max_acc_[j];
00532 //      tb =  blendTime(acc,-acc*tc_[i-1].duration_,tp_[i].q_[j]-tp_[i-1].q_[j]);
00533       tb =  blendTime(acc,-acc*tc_[i-1].duration_,diff);
00534 
00535       temp[0] = tp_[i-1].q_[j];
00536       temp[1] = 0;
00537       temp[2] = 0.5*acc;
00538       temp[3] = tb;
00539       temp[4] = std::max(tc_[i-1].duration_-2*tb,0.0);
00540 
00541       tc_[i-1].coeff_[j][0] = temp[0];
00542       tc_[i-1].coeff_[j][1] = temp[1];
00543       tc_[i-1].coeff_[j][2] = temp[2];
00544       tc_[i-1].coeff_[j][3] = temp[3];
00545       tc_[i-1].coeff_[j][4] = temp[4];
00546       tc_[i-1].degree_ = 1;
00547       tc_[i-1].dimension_ = dimension_;
00548 //      tc.coeff_.push_back(temp);
00549     }
00550 //    tc_.push_back(tc);
00551   }
00552   return 1;
00553 }
00554 
00555 double Trajectory::blendTime(double aa,double bb,double cc)
00556 {
00557    double disc = (pow(bb,2) - 4*aa*cc);
00558    if(disc < 0)
00559       return 0.0;
00560 
00561    double tb1 = (-bb + sqrt(disc))/(2*aa);
00562    double tb2 = (-bb - sqrt(disc))/(2*aa);
00563    if(std::isnan(tb1))
00564      tb1 = 0.0;
00565    if(std::isnan(tb2))
00566      tb2 = 0.0;
00567    return std::min(tb1,tb2);
00568 }
00569 
00570 
00571 void Trajectory::sampleLinear(TPoint &tp, double time, const TCoeff &tc, double segment_start_time)
00572 {
00573   double segment_time = time - segment_start_time;
00574 //  ROS_INFO("Coeff size: %d",tc.coeff_.size());
00575 //  ROS_INFO("Coeff internal size: %d", tc.coeff_[0].size());
00576   for(int i =0; i < dimension_; i++)
00577   {
00578 //    ROS_INFO("Coeffs: %f %f", tc.coeff_[i][0], tc.coeff_[i][1]);
00579     tp.q_[i]    =  tc.coeff_[i][0] + segment_time * tc.coeff_[i][1];
00580     tp.qdot_[i] =  tc.coeff_[i][1];
00581 
00582     if(joint_wraps_[i])
00583       tp.q_[i] = angles::normalize_angle(tp.q_[i]);
00584 
00585   }
00586   tp.time_ = time;
00587   tp.dimension_ = dimension_;
00588 }
00589 
00590 
00591 void Trajectory::sampleBlendedLinear(TPoint &tp, double time, const TCoeff &tc, double segment_start_time)
00592 {
00593   double segment_time = time - segment_start_time;
00594   for(int i =0; i < dimension_; i++)
00595   {
00596     double taccend = tc.coeff_[i][3];
00597     double tvelend = tc.coeff_[i][3] + tc.coeff_[i][4];
00598     double tvel = tc.coeff_[i][4];
00599     double acc = tc.coeff_[i][2]*2;
00600     double v0 = tc.coeff_[i][1];
00601  
00602     if(segment_time <= taccend)
00603     {
00604       tp.q_[i]    =  tc.coeff_[i][0] + segment_time * v0 + 0.5 * segment_time * segment_time * acc;
00605       tp.qdot_[i] =  tc.coeff_[i][1] + segment_time * acc;
00606     }
00607     else if(segment_time >= tvelend)
00608     {
00609       double dT = segment_time - tvelend;
00610       tp.q_[i] = tc.coeff_[i][0] +  v0 * taccend + 0.5 * acc * taccend * taccend + acc * taccend * tvel + acc * taccend * dT - 0.5 * acc * dT * dT;
00611       tp.qdot_[i] = acc*taccend - acc*dT;
00612     }
00613     else
00614     {
00615       double dT = segment_time - taccend;
00616       tp.q_[i] = tc.coeff_[i][0] +  v0 * taccend + 0.5 * acc * taccend * taccend + acc * taccend * dT;
00617       tp.qdot_[i] = acc * taccend;
00618     }
00619 
00620     if(joint_wraps_[i])
00621       tp.q_[i] = angles::normalize_angle(tp.q_[i]);
00622   }
00623   tp.time_ = time;
00624   tp.dimension_ = dimension_;
00625 }
00626 
00627 
00628 void Trajectory::sampleCubic(TPoint &tp, double time, const TCoeff &tc, double segment_start_time)
00629 {
00630   double segment_time = time - segment_start_time;
00631   for(int i =0; i < dimension_; i++)
00632   {
00633     tp.q_[i]    = tc.coeff_[i][0] + segment_time * tc.coeff_[i][1] + segment_time*segment_time*tc.coeff_[i][2] + segment_time*segment_time*segment_time*tc.coeff_[i][3];
00634     tp.qdot_[i] = tc.coeff_[i][1] + 2*segment_time*tc.coeff_[i][2] + 3*segment_time*segment_time*tc.coeff_[i][3];
00635 
00636     if(joint_wraps_[i])
00637       tp.q_[i] = angles::normalize_angle(tp.q_[i]);
00638 
00639   }
00640   tp.time_ = time;
00641   tp.dimension_ = dimension_;
00642 }
00643 
00644 int Trajectory::getNumberPoints()
00645 {
00646   return num_points_;
00647 }
00648 
00649 int Trajectory::getDuration(std::vector<double> &duration)
00650 {
00651   if((int) duration.size() != num_points_-1)
00652   {
00653     ROS_WARN("Size of duration vector %zd does not match number of segments in trajectory %d", duration.size(), num_points_-1);
00654     return -1;
00655   }
00656   for(int i = 0; i < num_points_-1; i++)
00657     duration[i] = tc_[i].duration_;
00658 
00659   return 1;
00660 }
00661 
00662 int Trajectory::getDuration(int index, double &duration)
00663 {
00664   if(index > num_points_ -1)
00665   {
00666     ROS_WARN("Index %d outside number of segments in the trajectory %d", index, num_points_-1);
00667     return -1;
00668   }
00669 
00670   duration = tc_[index].duration_;
00671 
00672   return 1;
00673 
00674 }
00675 
00676 int Trajectory::getTimeStamps(std::vector<double> &timestamps)
00677 {
00678   if((int) timestamps.size() != num_points_)
00679   {
00680     ROS_WARN("Size of timestamps vector %zd does not match number of points in trajectory %d", timestamps.size(), num_points_);
00681     return -1;
00682   }
00683   for(int i = 0; i < num_points_; i++)
00684     timestamps[i] = tp_[i].time_;
00685 
00686   return 1;
00687 }
00688 
00689 double Trajectory::calculateMinimumTimeLinear(const TPoint &start, const TPoint &end)
00690 {
00691   double minJointTime(MAX_ALLOWABLE_TIME);
00692   double minTime(0);
00693 
00694   for(int i = 0; i < start.dimension_; i++)
00695   {
00696     double diff = jointDiff(start.q_[i],end.q_[i],i);
00697     if(max_rate_[i] > 0)
00698 //      minJointTime = fabs(end.q_[i]-start.q_[i]) / max_rate_[i];
00699         minJointTime = fabs(diff) / max_rate_[i];
00700     else
00701       minJointTime = MAX_ALLOWABLE_TIME;
00702 
00703     if(minTime < minJointTime)
00704       minTime = minJointTime;
00705 
00706   }
00707 
00708   return minTime;
00709 }
00710 
00711 double Trajectory::calculateMinimumTimeCubic(const TPoint &start, const TPoint &end)
00712 {
00713   double minJointTime(MAX_ALLOWABLE_TIME);
00714   double minTime(0);
00715 
00716   for(int i = 0; i < start.dimension_; i++)
00717   {
00718     if(max_rate_[i] > 0)
00719       minJointTime = calculateMinTimeCubic(start.q_[i],end.q_[i],start.qdot_[i],end.qdot_[i],max_rate_[i],i);
00720     else
00721       minJointTime = MAX_ALLOWABLE_TIME;
00722 
00723 //    ROS_INFO("Min time: %f",minJointTime);
00724 
00725     if(minTime < minJointTime)
00726       minTime = minJointTime;
00727 
00728   }
00729   return minTime;
00730 }
00731 
00732 double Trajectory::calculateMinTimeCubic(double q0, double q1, double v0, double v1, double vmax, int index)
00733 {
00734   double t1(MAX_ALLOWABLE_TIME), t2(MAX_ALLOWABLE_TIME), result(MAX_ALLOWABLE_TIME);
00735 //  double dq = q1 - q0;
00736   double dq = jointDiff(q0,q1,index);
00737 
00738   double v(0.0);
00739   if(dq > 0)
00740      v = vmax;
00741   else
00742      v = -vmax;
00743 
00744   double a = 3.0*(v0+v1)*v - 3.0* (v0+v1)*v0 + pow((2.0*v0+v1),2.0);
00745   double b = -6.0*dq*v + 6.0 * v0 *dq - 6.0*dq*(2.0*v0+v1);
00746   double c = 9.0 * pow(dq,2);
00747 
00748   if (fabs(a) > EPS_TRAJECTORY)
00749   {
00750     if((pow(b,2)-4.0*a*c) >= 0)
00751     {
00752       t1 = (-b + sqrt(pow(b,2)-4.0*a*c))/(2.0*a);
00753       t2 = (-b - sqrt(pow(b,2)-4.0*a*c))/(2.0*a);
00754     }
00755   }
00756   else
00757   {
00758     t1 = -c/b;
00759     t2 = t1;
00760   }
00761 
00762   if(t1 < 0)
00763     t1 = MAX_ALLOWABLE_TIME;
00764 
00765   if(t2 < 0)
00766     t2 = MAX_ALLOWABLE_TIME;
00767 
00768   result = std::min(t1,t2);
00769   return result;
00770 }
00771 
00772 double Trajectory::calculateMinimumTimeLSPB(const TPoint &start, const TPoint &end)
00773 {
00774   double minJointTime(MAX_ALLOWABLE_TIME);
00775   double minTime(0);
00776 
00777   for(int i = 0; i < start.dimension_; i++)
00778   {
00779     if(max_rate_[i] > 0)
00780       minJointTime = calculateMinTimeLSPB(start.q_[i],end.q_[i],max_rate_[i],max_acc_[i],i);
00781     else
00782       minJointTime = MAX_ALLOWABLE_TIME;
00783 
00784     if(minTime < minJointTime)
00785       minTime = minJointTime;
00786 
00787   }
00788 
00789   return minTime;
00790 }
00791 
00792 double Trajectory::calculateMinTimeLSPB(double q0, double q1, double vmax, double amax, int index)
00793 {
00794   double diff = jointDiff(q0,q1,index);
00795 
00796   double tb = std::min(fabs(vmax/amax),sqrt(fabs(diff)/amax));
00797   double acc(0);
00798   if(diff>0)
00799     acc = amax;
00800   else
00801     acc = -amax;
00802   double dist_tb = acc*tb*tb;
00803   double ts = (diff - dist_tb)/(acc*tb);
00804   if(ts < 0)
00805     ts = 0;
00806   return (2*tb+ts);
00807 }
00808 
00809 
00810 void Trajectory::setInterpolationMethod(std::string interp_method)
00811 {
00812   interp_method_ = interp_method;
00813   ROS_INFO("Trajectory:: interpolation type %s",interp_method_.c_str());
00814 }
00815 
00816 int Trajectory::parameterize()
00817 {
00818   int error_code = -1;
00819   if(interp_method_ == std::string("linear"))
00820      error_code = parameterizeLinear();
00821   else if(interp_method_ == std::string("cubic"))
00822      error_code = parameterizeCubic();
00823   else if(interp_method_ == std::string("blended_linear"))
00824      error_code = parameterizeBlendedLinear();
00825   else
00826   {
00827     ROS_WARN("Unrecognized interp_method type: %s\n",interp_method_.c_str());
00828   }
00829   return error_code;
00830 }
00831 
00832 
00833 int Trajectory::parameterizeLinear()
00834 {
00835   double dT(0);
00836 
00837 /*  TCoeff tc;
00838   std::vector<double> temp;
00839   temp.resize(2);
00840   tc.degree_ = 1;
00841   tc.dimension_ = dimension_;
00842   tc_.clear();
00843 */
00844   double temp[2];
00845 
00846   if(autocalc_timing_)
00847   {
00848     if(!max_rate_set_ || (int) max_rate_.size() < 0)
00849     {
00850       ROS_WARN("Trying to apply rate limits without setting max rate information. Use setMaxRate first.");
00851       return -1;
00852     }
00853   }
00854   for(int i=1; i < (int) num_points_ ; i++)
00855   {
00856 //    tc.coeff_.clear();
00857     dT = tp_[i].time_ - tp_[i-1].time_;
00858     if(autocalc_timing_) 
00859     {
00860       double dTMin = calculateMinimumTimeLinear(tp_[i-1],tp_[i]);
00861       if(dTMin > dT) // if minimum time required to satisfy limits is greater than time available, stretch this segment
00862         dT = dTMin;
00863     }
00864 
00865 //    tc.duration_ = dT;
00866     tc_[i-1].duration_ = dT;
00867 
00868     for(int j=0; j < dimension_; j++)
00869     {
00870       temp[0] = tp_[i-1].q_[j];
00871       temp[1] = jointDiff(tp_[i-1].q_[j],tp_[i].q_[j],j)/tc_[i-1].duration_;  
00872       if(std::isnan(temp[1]))
00873         {
00874          temp[1] = 0.0;
00875 //         ROS_WARN("Zero duration between two trajectory points");
00876         }
00877 //      tc.coeff_.push_back(temp);
00878       tc_[i-1].coeff_[j][0] = temp[0];
00879       tc_[i-1].coeff_[j][1] = temp[1];
00880       tc_[i-1].degree_ = 1;
00881       tc_[i-1].dimension_ = dimension_;
00882     }
00883 //    tc_.push_back(tc);
00884   }
00885 
00886 /*  for(int i=0; i<tc_.size(); i++)
00887   {
00888     for(int j=0; j<dimension_; j++)
00889       ROS_INFO("stored coeff: %d %d %f %f",i,j,tc_[i].coeff_[j][0],tc_[i].coeff_[j][1]);
00890   }
00891 */
00892   // Now modify all the times to bring them up to date
00893   for(int i=1; i < (int) num_points_; i++)
00894   {
00895     tp_[i].time_ = tp_[i-1].time_ + tc_[i-1].duration_;
00896 //    ROS_INFO("Times: %d %f",i,tp_[i].time_);
00897   }
00898   return 1;
00899 }
00900 
00901 
00902 
00903 int Trajectory::parameterizeCubic()
00904 {
00905   double dT(0);
00906 
00907 /*  TCoeff tc;
00908   std::vector<double> temp;
00909   temp.resize(4);
00910   tc.degree_ = 1;
00911   tc.dimension_ = dimension_;
00912   tc_.clear();
00913 */
00914 
00915   double temp[4];
00916 
00917   if(autocalc_timing_)
00918   {
00919     if(!max_rate_set_ || (int) max_rate_.size() < 0)
00920     {
00921       ROS_WARN("Trying to apply rate limits without setting max rate information. Use setMaxRate first.");
00922       return -1;
00923     }
00924   }
00925 
00926   for(int i=1; i < (int) num_points_ ; i++)
00927   {
00928 //    tc.coeff_.clear();
00929     dT = tp_[i].time_ - tp_[i-1].time_;
00930     if(autocalc_timing_) 
00931     {
00932       double dTMin = calculateMinimumTimeCubic(tp_[i-1],tp_[i]);
00933       if(dTMin > dT) // if minimum time required to satisfy limits is greater than time available, stretch this segment
00934         dT = dTMin;
00935     }
00936 
00937 //    tc.duration_ = dT;
00938     tc_[i-1].duration_ = dT;
00939 
00940     for(int j=0; j < dimension_; j++)
00941     {
00942       double diff  = jointDiff(tp_[i-1].q_[j],tp_[i].q_[j],j);
00943       temp[0] = tp_[i-1].q_[j];
00944       temp[1] = tp_[i-1].qdot_[j];
00945       temp[2] = (3*diff-(2*tp_[i-1].qdot_[j]+tp_[i].qdot_[j])*tc_[i-1].duration_)/(tc_[i-1].duration_*tc_[i-1].duration_);
00946       temp[3] = (-2*diff+(tp_[i-1].qdot_[j]+tp_[i].qdot_[j])*tc_[i-1].duration_)/(pow(tc_[i-1].duration_,3));
00947       if(std::isnan(temp[2]))
00948         temp[2] = 0.0;
00949       if(std::isnan(temp[3]))
00950         temp[3] = 0.0;
00951 
00952       tc_[i-1].coeff_[j][0] = temp[0];
00953       tc_[i-1].coeff_[j][1] = temp[1];
00954       tc_[i-1].coeff_[j][2] = temp[2];
00955       tc_[i-1].coeff_[j][3] = temp[3];
00956       tc_[i-1].degree_ = 1;
00957       tc_[i-1].dimension_ = dimension_;
00958 
00959 //      tc.coeff_.push_back(temp);
00960     }
00961 //    tc_.push_back(tc);
00962   }
00963 
00964   // Now modify all the times to bring them up to date
00965   for(int i=1; i < (int) num_points_; i++)
00966     tp_[i].time_ = tp_[i-1].time_ + tc_[i-1].duration_;
00967 
00968   return 1;
00969 }
00970 
00971 
00972 int Trajectory::parameterizeBlendedLinear()
00973 {
00974    double dT(0.0),acc(0.0),tb(0.0);
00975 /*
00976   TCoeff tc;
00977   std::vector<double> temp;
00978   temp.resize(5);
00979   tc.degree_ = 1;
00980   tc.dimension_ = dimension_;
00981   tc_.clear();
00982 */
00983    double temp[5];
00984 
00985   if(autocalc_timing_)
00986   {
00987     if(!max_rate_set_ || (int) max_rate_.size() != dimension_ || !max_acc_set_ || (int) max_acc_.size() != dimension_)
00988     {
00989       ROS_WARN("Trying to apply rate and acc limits without setting max rate or acc information. Use setMaxRate and setMaxAcc first.");
00990       return -1;
00991     }
00992   }
00993 
00994   for(int i=1; i < (int) num_points_ ; i++)
00995   {
00996 //    tc.coeff_.clear();
00997     dT = tp_[i].time_ - tp_[i-1].time_;
00998     if(autocalc_timing_) 
00999     {
01000       double dTMin = calculateMinimumTimeLSPB(tp_[i-1],tp_[i]);
01001       if(dTMin > dT)
01002         dT = dTMin;
01003     }
01004 
01005 //    tc.duration_ = dT;
01006     tc_[i-1].duration_ = dT;
01007 
01008     for(int j=0; j < dimension_; j++)
01009     {
01010       double diff  = jointDiff(tp_[i-1].q_[j],tp_[i].q_[j],j);
01011        if(diff > 0)
01012           acc = max_acc_[j];
01013        else
01014           acc = -max_acc_[j];
01015 
01016       tb =  blendTime(acc,-acc*tc_[i-1].duration_,diff);
01017 
01018       temp[0] = tp_[i-1].q_[j];
01019       temp[1] = 0;
01020       temp[2] = 0.5*acc;
01021       temp[3] = tb;
01022       temp[4] = std::max(tc_[i-1].duration_-2*tb,0.0);
01023 
01024       tc_[i-1].coeff_[j][0] = temp[0];
01025       tc_[i-1].coeff_[j][1] = temp[1];
01026       tc_[i-1].coeff_[j][2] = temp[2];
01027       tc_[i-1].coeff_[j][3] = temp[3];
01028       tc_[i-1].coeff_[j][4] = temp[4];
01029       tc_[i-1].degree_ = 1;
01030       tc_[i-1].dimension_ = dimension_;
01031 
01032 
01033       //ROS_DEBUG("coeff: %d %d %f %f %f %f %f %f\n", i,j,tc.duration_,temp[0],temp[1],temp[2],temp[3],temp[4]);
01034       //    tc.coeff_.push_back(temp);
01035     }
01036 //    tc_.push_back(tc);
01037   }
01038 
01039   // Now modify all the times to bring them up to date
01040   for(int i=1; i < (int) num_points_; i++)
01041     tp_[i].time_ = tp_[i-1].time_ + tc_[i-1].duration_;
01042 
01043   return 1;
01044 }
01045 
01046 int Trajectory::write(std::string filename, double dT)
01047 {
01048   FILE *f = fopen(filename.c_str(),"w");
01049   double time = tp_.front().time_;
01050   TPoint tp;
01051   tp.setDimension(dimension_);
01052 
01053   while(time < lastPoint().time_)
01054   {
01055     sample(tp,time);
01056     fprintf(f,"%f ",time);
01057     for(int j=0; j < dimension_; j++)
01058     {
01059       fprintf(f,"%f ",tp.q_[j]);
01060     }
01061     for(int j=0; j < dimension_; j++)
01062     {
01063       fprintf(f,"%f ",tp.qdot_[j]);
01064     }
01065     fprintf(f,"\n");
01066     time += dT;
01067 //    printf("%f \n",time);
01068   }
01069   fclose(f);
01070   return 1;
01071 }
01072 
01073 void Trajectory::getTrajectory(std::vector<trajectory::Trajectory::TPoint> &traj, double dT)
01074 {
01075   double time = tp_.front().time_;
01076   TPoint tp;
01077   tp.setDimension(dimension_);
01078   while(time < lastPoint().time_)
01079   {
01080     sample(tp,time);
01081     traj.push_back(tp);
01082     time += dT;
01083   }
01084 }


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Sat Jun 8 2019 20:49:34