$search
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 %d 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 %d 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 %d 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 = %d 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: %d 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: %d 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 %d 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> ×tamps) 00677 { 00678 if((int) timestamps.size() != num_points_) 00679 { 00680 ROS_WARN("Size of timestamps vector %d 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 }