spline_smoother: spline_smoother_utils.cpp Source 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/spline_smoother_utils.h>
00038 #include <spline_smoother/LSPBTrajectoryMsg.h>
00039 #include <spline_smoother/SplineTrajectory.h>
00040 #include <angles/angles.h>
00041 
00042 namespace spline_smoother
00043 {
00044 
00055 bool findSplineSegment(const spline_smoother::SplineTrajectory &spline,
00056                        const double& time, 
00057                        spline_smoother::SplineTrajectorySegment& spline_segment,
00058                        double& segment_time, 
00059                        int start_index, 
00060                        int end_index)
00061 {
00062 
00063   if(spline.segments.empty())
00064   {
00065     ROS_ERROR("Spline segments are empty: this should not have happened");
00066     return false;
00067   }
00068   if(end_index == -1)
00069     end_index = spline.segments.size();
00070 
00071   double segment_start_time = 0.0;
00072   double segment_end_time = 0.0;
00073 
00074   for(int i=0; i < (int)spline.segments.size(); i++)
00075   {
00076     segment_start_time = segment_end_time;
00077     segment_end_time += spline.segments[i].duration.toSec();
00078     if(time <= segment_end_time)
00079     {
00080       segment_time = time-segment_start_time;
00081       spline_segment = spline.segments[i];
00082       ROS_DEBUG("Found spline segment: %d, trajectory time: %f, segment_time: %f, segment_end_time: %f",i,time,segment_time,segment_end_time);
00083       return true;
00084     }
00085   }
00086   if(time >= segment_end_time)
00087   {
00088     ROS_DEBUG("Did not find spline segment corresponding to input time: %f",time);
00089     segment_time = segment_end_time - segment_start_time;
00090     spline_segment = spline.segments.back();
00091     return true;
00092   }
00093   ROS_ERROR("Should not be here in findSplineSegment. Input time: %f is invalid",time);
00094   return false;
00095 }
00096 
00097 bool findSplineSegment(const spline_smoother::LSPBTrajectoryMsg &spline,
00098                        const double& time, 
00099                        spline_smoother::LSPBTrajectorySegmentMsg& spline_segment,
00100                        double& segment_time, 
00101                        int start_index, 
00102                        int end_index)
00103 {
00104   if(end_index == -1)
00105     end_index = spline.segments.size();
00106 
00107   double segment_start_time = 0.0;
00108   double segment_end_time = 0.0;
00109 
00110   for(int i=0; i < (int)spline.segments.size(); i++)
00111   {
00112     segment_start_time = segment_end_time;
00113     segment_end_time += spline.segments[i].duration.toSec();
00114     if(time <= segment_end_time)
00115     {
00116       segment_time = time-segment_start_time;
00117       spline_segment = spline.segments[i];
00118       ROS_DEBUG("Found spline segment: %d, trajectory time: %f, segment_time: %f, segment_end_time: %f",i,time,segment_time,segment_end_time);
00119       return true;
00120     }
00121   }
00122   if(time >= segment_end_time)
00123   {
00124     ROS_DEBUG("Did not find spline segment corresponding to input time: %f",time);
00125     segment_time = segment_end_time - segment_start_time;
00126     spline_segment = spline.segments.back();
00127     return true;
00128   }
00129   ROS_ERROR("Should not be here in findSplineSegment. Input time: %f is invalid",time);
00130   return false;
00131 }
00132 
00133 
00140 bool getTotalTime(const spline_smoother::SplineTrajectory &spline, 
00141                   double &t)
00142 {
00143   t = 0.0;
00144   for(int i=0; i < (int)spline.segments.size(); i++)
00145   {
00146     t += spline.segments[i].duration.toSec();
00147   }
00148   return true;
00149 }
00150 
00151 
00158 bool getTotalTime(const spline_smoother::LSPBTrajectoryMsg &spline, 
00159                   double &t)
00160 {
00161   t = 0.0;
00162   for(int i=0; i < (int)spline.segments.size(); i++)
00163   {
00164     t += spline.segments[i].duration.toSec();
00165   }
00166   return true;
00167 }
00168 
00169 
00170 bool sampleSplineTrajectory(const spline_smoother::SplineTrajectorySegment &spline, 
00171                             const double& input_time, 
00172                             trajectory_msgs::JointTrajectoryPoint &point_out)
00173 {
00174   double t = input_time;
00175   if(t > spline.duration.toSec())
00176   {
00177     t = spline.duration.toSec();
00178   }
00179   int joint_num = spline.joints.size();
00180   point_out.positions.resize(joint_num);
00181   point_out.velocities.resize(joint_num);
00182   point_out.accelerations.resize(joint_num);
00183   for(unsigned int i=0; i< spline.joints.size(); i++)
00184   {
00185     point_out.positions[i] = 0.0;
00186     point_out.velocities[i] = 0.0;
00187     point_out.accelerations[i] = 0.0;
00188 
00189     for(unsigned int j =0; j < spline.joints[i].coefficients.size(); j++)
00190     {
00191       point_out.positions[i] += spline.joints[i].coefficients[j]*pow(t,j);
00192       if(j > 0)
00193         point_out.velocities[i] += j*spline.joints[i].coefficients[j]*pow(t,j-1);
00194       if(j > 1)
00195         point_out.accelerations[i] += j*(j-1)*spline.joints[i].coefficients[j]*pow(t,j-2);
00196     }      
00197   }
00198   point_out.time_from_start = ros::Duration(t);
00199   return true;
00200 }
00201 
00202 bool sampleSplineTrajectory(const spline_smoother::LSPBTrajectorySegmentMsg &spline, 
00203                             const double& input_time, 
00204                             trajectory_msgs::JointTrajectoryPoint &point_out)
00205 {
00206   double t = input_time;
00207   if(t > spline.duration.toSec())
00208   {
00209     t = spline.duration.toSec();
00210   }
00211   int joint_num = spline.joints.size();
00212   point_out.positions.resize(joint_num);
00213   point_out.velocities.resize(joint_num);
00214   point_out.accelerations.resize(joint_num);
00215 
00216   double taccend(0.0), tvelend(0.0), tvel(0.0), acc(0.0), v0(0.0);
00217 
00218   for(int i=0; i< joint_num; i++)
00219   {
00220     taccend = spline.joints[i].quadratic_segment_duration;
00221     tvelend = spline.joints[i].linear_segment_duration + spline.joints[i].quadratic_segment_duration;
00222     tvel = spline.joints[i].linear_segment_duration;
00223     acc = spline.joints[i].coefficients[2]*2;
00224     v0 = spline.joints[i].coefficients[1];
00225     
00226     if(t <= taccend)
00227     {
00228       point_out.positions[i]  =  spline.joints[i].coefficients[0] + t * v0 + 0.5 * t * t * acc;
00229       point_out.velocities[i] =  spline.joints[i].coefficients[1] + t * acc;
00230       point_out.accelerations[i] = acc;
00231     }
00232     else if(t >= tvelend)
00233     {
00234       double dT = t - tvelend;
00235       point_out.positions[i] = spline.joints[i].coefficients[0] +  v0 * taccend + 0.5 * acc * taccend * taccend + acc * taccend * tvel + acc * taccend * dT - 0.5 * acc * dT * dT;
00236       point_out.velocities[i] = acc*taccend - acc*dT;
00237       point_out.accelerations[i] = -acc;
00238     }
00239     else
00240     {
00241       double dT = t - taccend;
00242       point_out.positions[i] = spline.joints[i].coefficients[0] +  v0 * taccend + 0.5 * acc * taccend * taccend + acc * taccend * dT;
00243       point_out.velocities[i] = acc * taccend;
00244       point_out.accelerations[i] = 0.0;
00245     }
00246   }
00247   point_out.time_from_start = ros::Duration(t);
00248   return true;
00249 }
00250 
00251 bool sampleSplineTrajectory(const spline_smoother::SplineTrajectory& spline, 
00252                             const std::vector<double> &times, 
00253                             trajectory_msgs::JointTrajectory& traj_out)
00254 {
00255   bool success = true;
00256   trajectory_msgs::JointTrajectoryPoint point_out;
00257   spline_smoother::SplineTrajectorySegment spline_segment;
00258   double segment_time;
00259   traj_out.points.clear();
00260   traj_out.points.resize(times.size());
00261   for(int i=0; i < (int)times.size(); i++)
00262   {
00263     ROS_DEBUG("Input time:%d %f",i,times[i]);
00264     success = success && findSplineSegment(spline,times[i],spline_segment,segment_time);
00265     if(success)
00266       success = success && sampleSplineTrajectory(spline_segment,segment_time,point_out);    
00267     else
00268       return false;
00269     point_out.time_from_start = ros::Duration(times[i]);
00270     traj_out.points[i] = point_out;
00271   }
00272   traj_out.joint_names = spline.names;
00273   return success;
00274 }
00275 
00276 bool sampleSplineTrajectory(const spline_smoother::LSPBTrajectoryMsg& spline, 
00277                             const std::vector<double> &times, 
00278                             trajectory_msgs::JointTrajectory& traj_out)
00279 {
00280   bool success = true;
00281   trajectory_msgs::JointTrajectoryPoint point_out;
00282   spline_smoother::LSPBTrajectorySegmentMsg spline_segment;
00283   double segment_time;
00284   traj_out.points.clear();
00285   traj_out.points.resize(times.size());
00286   for(int i=0; i < (int)times.size(); i++)
00287   {
00288     ROS_DEBUG("Input time:%d %f",i,times[i]);
00289     success = success && findSplineSegment(spline,times[i],spline_segment,segment_time);
00290     if(success)
00291       success = success && sampleSplineTrajectory(spline_segment,segment_time,point_out);    
00292     else
00293       return false;
00294     point_out.time_from_start = ros::Duration(times[i]);
00295     traj_out.points[i] = point_out;
00296   }
00297   traj_out.joint_names = spline.names;
00298   return success;
00299 }
00300 
00308 bool write(const spline_smoother::SplineTrajectory &spline, 
00309            const std::vector<double> &times, 
00310            const std::string &filename)
00311 {
00312   trajectory_msgs::JointTrajectory tp;
00313   if(!sampleSplineTrajectory(spline,times,tp))
00314   {
00315     ROS_ERROR("Input spline was not sampled successfully");
00316     return false;
00317   }
00318 
00319   if(tp.points.empty())
00320   {
00321     ROS_ERROR("Input trajectory is empty");
00322     return false;
00323   }
00324 
00325   FILE *f = fopen(filename.c_str(),"w");
00326   if(!f)
00327     return false;
00328 
00329   int num_joints = tp.points[0].positions.size();
00330   for(int i=0; i < (int) tp.points.size(); i++)
00331   {
00332     fprintf(f,"%f ",tp.points[i].time_from_start.toSec());
00333     for(int j=0; j < num_joints; j++)
00334     {
00335       fprintf(f,"%f ",tp.points[i].positions[j]);
00336     }
00337     for(int j=0; j < num_joints; j++)
00338     {
00339       fprintf(f,"%f ",tp.points[i].velocities[j]);
00340     }
00341     for(int j=0; j < num_joints; j++)
00342     {
00343       fprintf(f,"%f ",tp.points[i].accelerations[j]);
00344     }
00345     fprintf(f,"\n");
00346   }
00347   fclose(f);
00348   return true;
00349 }
00350 
00357 bool writeSpline(const spline_smoother::SplineTrajectory &spline, 
00358                  const std::string &filename)
00359 {
00360   if(spline.segments.empty())
00361     return false;
00362 
00363   int num_segments = spline.segments.size();
00364   int num_joints = spline.segments[0].joints.size();
00365 
00366   FILE *f = fopen(filename.c_str(),"w");
00367   if(!f)
00368     return false;
00369   for(int i=0; i < num_segments; i++)
00370   {
00371     fprintf(f,"%f ",spline.segments[i].duration.toSec());
00372     for(int j=0; j < num_joints; j++)
00373     {
00374       for(unsigned int k=0; k < spline.segments[i].joints[j].coefficients.size(); k++)
00375         fprintf(f,"%f ",spline.segments[i].joints[j].coefficients[k]);
00376     }
00377     fprintf(f,"\n");
00378   }
00379   fclose(f);
00380   return true;    
00381 }
00382 
00390 bool write(const spline_smoother::LSPBTrajectoryMsg &spline, 
00391            const std::vector<double> &times, 
00392            const std::string &filename)
00393 {
00394   trajectory_msgs::JointTrajectory tp;
00395   if(!sampleSplineTrajectory(spline,times,tp))
00396   {
00397     ROS_ERROR("Input spline was not sampled successfully");
00398     return false;
00399   }
00400 
00401   if(tp.points.empty())
00402   {
00403     ROS_ERROR("Input trajectory is empty");
00404     return false;
00405   }
00406 
00407   FILE *f = fopen(filename.c_str(),"w");
00408   if(!f)
00409     return false;
00410 
00411   int num_joints = tp.points[0].positions.size();
00412   for(int i=0; i < (int) tp.points.size(); i++)
00413   {
00414     fprintf(f,"%f ",tp.points[i].time_from_start.toSec());
00415     for(int j=0; j < num_joints; j++)
00416     {
00417       fprintf(f,"%f ",tp.points[i].positions[j]);
00418     }
00419     for(int j=0; j < num_joints; j++)
00420     {
00421       fprintf(f,"%f ",tp.points[i].velocities[j]);
00422     }
00423     for(int j=0; j < num_joints; j++)
00424     {
00425       fprintf(f,"%f ",tp.points[i].accelerations[j]);
00426     }
00427     fprintf(f,"\n");
00428   }
00429   fclose(f);
00430   return true;
00431 }
00432 
00439 bool writeSpline(const spline_smoother::LSPBTrajectoryMsg &spline, 
00440                  const std::string &filename)
00441 {
00442   if(spline.segments.empty())
00443     return false;
00444 
00445   int num_segments = spline.segments.size();
00446   int num_joints = spline.segments[0].joints.size();
00447 
00448   FILE *f = fopen(filename.c_str(),"w");
00449   if(!f)
00450     return false;
00451   for(int i=0; i < num_segments; i++)
00452   {
00453     fprintf(f,"%f ",spline.segments[i].duration.toSec());
00454     for(int j=0; j < num_joints; j++)
00455     {
00456       for(unsigned int k=0; k < spline.segments[i].joints[j].coefficients.size(); k++)
00457         fprintf(f,"%f ",spline.segments[i].joints[j].coefficients[k]);
00458     }
00459     fprintf(f,"\n");
00460   }
00461   fclose(f);
00462   return true;    
00463 }
00464 
00465 }