cubic_trajectory.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00037 #include <spline_smoother/cubic_trajectory.h>
00038 
00039 namespace spline_smoother
00040 {
00041   CubicTrajectory::CubicTrajectory()
00042   {
00043     apply_limits_ = true;
00044   }
00045 
00046   double CubicTrajectory::calculateMinimumTime(const trajectory_msgs::JointTrajectoryPoint &start, 
00047                                                const trajectory_msgs::JointTrajectoryPoint &end, 
00048                                                const std::vector<arm_navigation_msgs::JointLimits> &limits)
00049   {
00050     double minJointTime(MAX_ALLOWABLE_TIME);
00051     double segmentTime(0);
00052     int num_joints = (int) start.positions.size();
00053 
00054     for(int i = 0; i < num_joints; i++)
00055     {
00056       minJointTime = minSegmentTime(start.positions[i],end.positions[i],start.velocities[i],end.velocities[i],limits[i]);
00057       if(segmentTime < minJointTime)
00058         segmentTime = minJointTime;
00059     }
00060     return segmentTime;
00061   }
00062 
00063   double CubicTrajectory::minSegmentTime(const double &q0, 
00064                                          const double &q1, 
00065                                          const double &v0, 
00066                                          const double &v1, 
00067                                          const arm_navigation_msgs::JointLimits &limit)
00068   {
00069     //    double dq = jointDiff(q0,q1,limit);
00070     double dq = q1-q0;
00071     double vmax = limit.max_velocity;
00072     if( q0 == q1 && fabs(v0-v1) == 0.0)
00073     {
00074       return 0.0;
00075     }
00076     dq = q1-q0;
00077     double v = vmax;
00078     double solution;
00079     std::vector<double> solution_vec;
00080 
00081     double a1 = 3*(v0+v1)*v - 3* (v0+v1)*v0 + (2*v0+v1)*(2*v0+v1);
00082     double b1 = -6*dq*v + 6 * v0 *dq - 6*dq*(2*v0+v1);
00083     double c1 = 9*dq*dq;
00084 
00085     double a2 = 3*(v0+v1)*v + 3* (v0+v1)*v0 - (2*v0+v1)*(2*v0+v1);
00086     double b2 = -6*dq*v - 6 * v0 *dq + 6*dq*(2*v0+v1);
00087     double c2 = -9*dq*dq;
00088 
00089     std::vector<double> t1,t2,t3,t4,t5,t6;
00090 
00091     if(quadSolve(a1,b1,c1,t1))
00092       for(unsigned int i=0; i < t1.size(); i++)
00093         solution_vec.push_back(t1[i]);
00094     
00095     if(quadSolve(a2,b2,c2,t2))
00096       for(unsigned int i=0; i < t2.size(); i++)
00097         solution_vec.push_back(t2[i]);
00098     double amax = -1.0;
00099     
00100     if(limit.has_acceleration_limits)
00101     {
00102       amax = limit.max_acceleration;
00103       double a3 = amax/2.0;
00104       double b3 = 2*v0+v1;
00105       double c3 = -3*dq;
00106       if(quadSolve(a3,b3,c3,t3))
00107         for(unsigned int i=0; i < t3.size(); i++)
00108           solution_vec.push_back(t3[i]);
00109 
00110       double a4 = amax/2.0;
00111       double b4 = -(2*v0+v1);
00112       double c4 = 3*dq;
00113       if(quadSolve(a4,b4,c4,t4))
00114         for(unsigned int i=0; i < t4.size(); i++)
00115           solution_vec.push_back(t4[i]);
00116 
00117 
00118       double a5 = amax;
00119       double b5 = (-2*v0-4*v1);
00120       double c5 = 6*dq;
00121       if(quadSolve(a5,b5,c5,t5))
00122         for(unsigned int i=0; i < t5.size(); i++)
00123           solution_vec.push_back(t5[i]);
00124 
00125       double a6 = amax;
00126       double b6 = (2*v0+4*v1);
00127       double c6 = -6*dq;
00128       if(quadSolve(a6,b6,c6,t6))
00129         for(unsigned int i=0; i < t6.size(); i++)
00130           solution_vec.push_back(t6[i]);
00131     }
00132     std::vector<double> positive_durations, valid_durations;
00133     for(unsigned int i=0; i < solution_vec.size(); i++)
00134     {
00135       if(solution_vec[i] > 0)
00136         positive_durations.push_back(solution_vec[i]);        
00137     }
00138 
00139     for(unsigned int i=0; i < positive_durations.size(); i++)
00140     {
00141       ROS_DEBUG("Positive duration: %f",positive_durations[i]);
00142       if(validSolution(q0,q1,v0,v1,positive_durations[i],vmax,amax))
00143         valid_durations.push_back(positive_durations[i]);        
00144     }
00145 
00146     ROS_DEBUG("valid size: %d",(int)valid_durations.size());       
00147     std::sort(valid_durations.begin(),valid_durations.end());
00148     if(!valid_durations.empty())
00149       solution = valid_durations.front();
00150     else
00151       solution = 0.025;
00152 
00153     ROS_DEBUG(" ");
00154     ROS_DEBUG(" ");
00155     ROS_DEBUG(" ");
00156     return solution;
00157  }
00158 
00159 bool CubicTrajectory::validSolution(const double &q0, 
00160                                     const double &q1,
00161                                     const double &v0,
00162                                     const double &v1,
00163                                     const double &dT,
00164                                     const double &vmax,
00165                                     const double &amax)
00166 {
00167   if (dT == 0.0)
00168     return false;
00169   //  double a0 = q0;
00170   double a1 = v0;
00171   double a2 = (3*(q1-q0)-(2*v0+v1)*dT)/(dT*dT);
00172   double a3 = (2*(q0-q1)+(v0+v1)*dT)/(dT*dT*dT);
00173 
00174   double max_accn = fabs(2*a2);
00175   if(fabs(2*a2+6*a3*dT) > max_accn)
00176     max_accn = fabs(2*a2+6*a3*dT);
00177 
00178   bool max_vel_exists = false;
00179   double max_vel = 0.0;
00180 
00181   if(fabs(a3) > 0.0)
00182   {
00183     double max_vel_time = (-2*a2)/(6*a3); 
00184     if (max_vel_time >= 0 && max_vel_time < dT)
00185     {
00186       max_vel_exists = true;
00187       max_vel = a1-(a2*a2)/(a3*3.0);
00188     }
00189   }
00190 
00191   if(amax > 0 && max_accn-amax > 1e-2)
00192   {
00193     ROS_DEBUG("amax allowed: %f, max_accn: %f",amax,max_accn);
00194     return false;
00195   }
00196   if(max_vel_exists)
00197     if(fabs(max_vel)-vmax > 1e-2)
00198     {
00199       ROS_DEBUG("vmax allowed: %f, max_vel: %f",vmax,max_vel);
00200       return false;
00201     }
00202   return true;
00203 }
00204 
00205   bool CubicTrajectory::parameterize(const trajectory_msgs::JointTrajectory& trajectory_in, 
00206                                      const std::vector<arm_navigation_msgs::JointLimits> &limits,
00207                                      spline_smoother::SplineTrajectory& spline)
00208   {
00209     int num_traj = trajectory_in.points.size();
00210     int num_joints = trajectory_in.joint_names.size();
00211     spline.names = trajectory_in.joint_names;
00212     spline.segments.resize(num_traj-1);
00213 
00214     for(int i=0; i<num_joints; i++)
00215     {
00216       if(!limits[i].has_velocity_limits)
00217       {
00218         ROS_ERROR("Trying to apply velocity limits without supplying them. Set velocity_limits in the message and set has_velocity_limits flag to true.");
00219         return false;
00220       }
00221     }
00222     for (int i=1; i< num_traj; ++i)
00223     {
00224       spline.segments[i-1].joints.resize(num_joints);
00225 
00226       for(int j =0; j < num_joints; j++)
00227         spline.segments[i-1].joints[j].coefficients.resize(4);
00228       double dT = (trajectory_in.points[i].time_from_start - trajectory_in.points[i-1].time_from_start).toSec();
00229       if(apply_limits_)
00230       {
00231       double dTMin = calculateMinimumTime(trajectory_in.points[i-1],trajectory_in.points[i],limits);
00232       if(dTMin > dT) // if minimum time required to satisfy limits is greater than time available, stretch this segment
00233         dT = dTMin;      
00234       }
00235       spline.segments[i-1].duration = ros::Duration(dT);
00236       for(int j=0; j<num_joints; j++)
00237       {
00238         //        double diff = jointDiff(trajectory_in.points[i-1].positions[j],trajectory_in.points[i].positions[j],limits[j]);
00239         double diff = trajectory_in.points[i].positions[j] - trajectory_in.points[i-1].positions[j];
00240         spline.segments[i-1].joints[j].coefficients[0] = trajectory_in.points[i-1].positions[j];
00241         spline.segments[i-1].joints[j].coefficients[1] = trajectory_in.points[i-1].velocities[j];
00242         spline.segments[i-1].joints[j].coefficients[2] = (3*diff-(2*trajectory_in.points[i-1].velocities[j]+trajectory_in.points[i].velocities[j])*spline.segments[i-1].duration.toSec())/(spline.segments[i-1].duration.toSec()*spline.segments[i-1].duration.toSec());;
00243         spline.segments[i-1].joints[j].coefficients[3] = (-2*diff+(trajectory_in.points[i-1].velocities[j]+trajectory_in.points[i].velocities[j])*spline.segments[i-1].duration.toSec())/pow(spline.segments[i-1].duration.toSec(),3);
00244       }
00245     }
00246     return true;
00247   }
00248 
00249 bool CubicTrajectory::quadSolve(const double &a, 
00250                                 const double &b, 
00251                                 const double &c, 
00252                                 double &solution)
00253 {
00254   double t1(0.0), t2(0.0);
00255   //  double eps = 2.2e-16;
00256   if (fabs(a) > 0.0)
00257   {
00258     double discriminant = b*b-4*a*c;
00259     if (discriminant >= 0)
00260     {
00261       t1 = (-b + sqrt(discriminant))/(2*a);
00262       t2 = (-b - sqrt(discriminant))/(2*a);
00263       ROS_DEBUG("t1:%f t2:%f",t1,t2);
00264       solution = std::max(t1,t2);
00265       ROS_DEBUG("Solution: %f",solution);
00266       return true;
00267     }
00268     else
00269       return false;
00270   }
00271   else
00272   {
00273     if(fabs(b) == 0.0)
00274       return false;
00275     t1 = -c/b;
00276     t2 = t1;
00277     solution = t1;
00278     ROS_DEBUG("Solution: %f",solution);
00279     return true;
00280   }
00281 }
00282 
00283 bool CubicTrajectory::quadSolve(const double &a, 
00284                                 const double &b, 
00285                                 const double &c, 
00286                                 std::vector<double> &solution)
00287 {
00288   double t1(0.0), t2(0.0);
00289   double eps = 2.2e-16;
00290   if (fabs(a) > eps)
00291   {
00292     double discriminant = b*b-4*a*c;
00293     if (discriminant >= 0)
00294     {
00295       t1 = (-b + sqrt(discriminant))/(2*a);
00296       t2 = (-b - sqrt(discriminant))/(2*a);
00297       ROS_DEBUG("t1:%f t2:%f",t1,t2);
00298       solution.push_back(t1);
00299       solution.push_back(t2);
00300       return true;
00301     }
00302     else
00303     {
00304       ROS_DEBUG("Discriminant: %f",discriminant);
00305       return false;
00306     }
00307   }
00308   else
00309   {
00310     if(fabs(b) == 0.0)
00311       return false;
00312     t1 = -c/b;
00313     t2 = t1;
00314     solution.push_back(t1);
00315     solution.push_back(t2);
00316     return true;
00317   }
00318 }
00319 
00320 
00321 }


spline_smoother
Author(s): Mrinal Kalakrishnan / mail@mrinal.net
autogenerated on Thu Dec 12 2013 11:09:50