spline_smoother: spline_smoother: spline_smoother

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   if(end_index == -1)
00063     end_index = spline.segments.size();
00064 
00065   double segment_start_time = 0.0;
00066   double segment_end_time = 0.0;
00067 
00068   for(int i=0; i < (int)spline.segments.size(); i++)
00069   {
00070     segment_start_time = segment_end_time;
00071     segment_end_time += spline.segments[i].duration.toSec();
00072     if(time <= segment_end_time)
00073     {
00074       segment_time = time-segment_start_time;
00075       spline_segment = spline.segments[i];
00076       ROS_DEBUG("Found spline segment: %d, trajectory time: %f, segment_time: %f, segment_end_time: %f",i,time,segment_time,segment_end_time);
00077       return true;
00078     }
00079   }
00080   if(time >= segment_end_time)
00081   {
00082     ROS_DEBUG("Did not find spline segment corresponding to input time: %f",time);
00083     segment_time = segment_end_time - segment_start_time;
00084     spline_segment = spline.segments.back();
00085     return true;
00086   }
00087   ROS_ERROR("Should not be here in findSplineSegment. Input time: %f is invalid",time);
00088   return false;
00089 }
00090 
00091 bool findSplineSegment(const spline_smoother::LSPBTrajectoryMsg &spline,
00092                        const double& time, 
00093                        spline_smoother::LSPBTrajectorySegmentMsg& spline_segment,
00094                        double& segment_time, 
00095                        int start_index, 
00096                        int end_index)
00097 {
00098   if(end_index == -1)
00099     end_index = spline.segments.size();
00100 
00101   double segment_start_time = 0.0;
00102   double segment_end_time = 0.0;
00103 
00104   for(int i=0; i < (int)spline.segments.size(); i++)
00105   {
00106     segment_start_time = segment_end_time;
00107     segment_end_time += spline.segments[i].duration.toSec();
00108     if(time <= segment_end_time)
00109     {
00110       segment_time = time-segment_start_time;
00111       spline_segment = spline.segments[i];
00112       ROS_DEBUG("Found spline segment: %d, trajectory time: %f, segment_time: %f, segment_end_time: %f",i,time,segment_time,segment_end_time);
00113       return true;
00114     }
00115   }
00116   if(time >= segment_end_time)
00117   {
00118     ROS_DEBUG("Did not find spline segment corresponding to input time: %f",time);
00119     segment_time = segment_end_time - segment_start_time;
00120     spline_segment = spline.segments.back();
00121     return true;
00122   }
00123   ROS_ERROR("Should not be here in findSplineSegment. Input time: %f is invalid",time);
00124   return false;
00125 }
00126 
00127 
00134 bool getTotalTime(const spline_smoother::SplineTrajectory &spline, 
00135                   double &t)
00136 {
00137   t = 0.0;
00138   for(int i=0; i < (int)spline.segments.size(); i++)
00139   {
00140     t += spline.segments[i].duration.toSec();
00141   }
00142   return true;
00143 }
00144 
00145 
00152 bool getTotalTime(const spline_smoother::LSPBTrajectoryMsg &spline, 
00153                   double &t)
00154 {
00155   t = 0.0;
00156   for(int i=0; i < (int)spline.segments.size(); i++)
00157   {
00158     t += spline.segments[i].duration.toSec();
00159   }
00160   return true;
00161 }
00162 
00163 
00164 bool sampleSplineTrajectory(const spline_smoother::SplineTrajectorySegment &spline, 
00165                             const double& input_time, 
00166                             trajectory_msgs::JointTrajectoryPoint &point_out)
00167 {
00168   double t = input_time;
00169   if(t > spline.duration.toSec())
00170   {
00171     t = spline.duration.toSec();
00172   }
00173   int joint_num = spline.joints.size();
00174   point_out.positions.resize(joint_num);
00175   point_out.velocities.resize(joint_num);
00176   point_out.accelerations.resize(joint_num);
00177   for(unsigned int i=0; i< spline.joints.size(); i++)
00178   {
00179     point_out.positions[i] = 0.0;
00180     point_out.velocities[i] = 0.0;
00181     point_out.accelerations[i] = 0.0;
00182 
00183     for(unsigned int j =0; j < spline.joints[i].coefficients.size(); j++)
00184     {
00185       point_out.positions[i] += spline.joints[i].coefficients[j]*pow(t,j);
00186       if(j > 0)
00187         point_out.velocities[i] += j*spline.joints[i].coefficients[j]*pow(t,j-1);
00188       if(j > 1)
00189         point_out.accelerations[i] += j*(j-1)*spline.joints[i].coefficients[j]*pow(t,j-2);
00190     }      
00191   }
00192   point_out.time_from_start = ros::Duration(t);
00193   return true;
00194 }
00195 
00196 bool sampleSplineTrajectory(const spline_smoother::LSPBTrajectorySegmentMsg &spline, 
00197                             const double& input_time, 
00198                             trajectory_msgs::JointTrajectoryPoint &point_out)
00199 {
00200   double t = input_time;
00201   if(t > spline.duration.toSec())
00202   {
00203     t = spline.duration.toSec();
00204   }
00205   int joint_num = spline.joints.size();
00206   point_out.positions.resize(joint_num);
00207   point_out.velocities.resize(joint_num);
00208   point_out.accelerations.resize(joint_num);
00209 
00210   double taccend(0.0), tvelend(0.0), tvel(0.0), acc(0.0), v0(0.0);
00211 
00212   for(int i=0; i< joint_num; i++)
00213   {
00214     taccend = spline.joints[i].quadratic_segment_duration;
00215     tvelend = spline.joints[i].linear_segment_duration + spline.joints[i].quadratic_segment_duration;
00216     tvel = spline.joints[i].linear_segment_duration;
00217     acc = spline.joints[i].coefficients[2]*2;
00218     v0 = spline.joints[i].coefficients[1];
00219     
00220     if(t <= taccend)
00221     {
00222       point_out.positions[i]  =  spline.joints[i].coefficients[0] + t * v0 + 0.5 * t * t * acc;
00223       point_out.velocities[i] =  spline.joints[i].coefficients[1] + t * acc;
00224       point_out.accelerations[i] = acc;
00225     }
00226     else if(t >= tvelend)
00227     {
00228       double dT = t - tvelend;
00229       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;
00230       point_out.velocities[i] = acc*taccend - acc*dT;
00231       point_out.accelerations[i] = -acc;
00232     }
00233     else
00234     {
00235       double dT = t - taccend;
00236       point_out.positions[i] = spline.joints[i].coefficients[0] +  v0 * taccend + 0.5 * acc * taccend * taccend + acc * taccend * dT;
00237       point_out.velocities[i] = acc * taccend;
00238       point_out.accelerations[i] = 0.0;
00239     }
00240   }
00241   point_out.time_from_start = ros::Duration(t);
00242   return true;
00243 }
00244 
00245 bool sampleSplineTrajectory(const spline_smoother::SplineTrajectory& spline, 
00246                             const std::vector<double> &times, 
00247                             trajectory_msgs::JointTrajectory& traj_out)
00248 {
00249   bool success = true;
00250   trajectory_msgs::JointTrajectoryPoint point_out;
00251   spline_smoother::SplineTrajectorySegment spline_segment;
00252   double segment_time;
00253   traj_out.points.clear();
00254   traj_out.points.resize(times.size());
00255   for(int i=0; i < (int)times.size(); i++)
00256   {
00257     ROS_DEBUG("Input time:%d %f",i,times[i]);
00258     success = success && findSplineSegment(spline,times[i],spline_segment,segment_time);
00259     success = success && sampleSplineTrajectory(spline_segment,segment_time,point_out);    
00260     point_out.time_from_start = ros::Duration(times[i]);
00261     traj_out.points[i] = point_out;
00262   }
00263   traj_out.joint_names = spline.names;
00264   return success;
00265 }
00266 
00267 bool sampleSplineTrajectory(const spline_smoother::LSPBTrajectoryMsg& spline, 
00268                             const std::vector<double> &times, 
00269                             trajectory_msgs::JointTrajectory& traj_out)
00270 {
00271   bool success = true;
00272   trajectory_msgs::JointTrajectoryPoint point_out;
00273   spline_smoother::LSPBTrajectorySegmentMsg spline_segment;
00274   double segment_time;
00275   traj_out.points.clear();
00276   traj_out.points.resize(times.size());
00277   for(int i=0; i < (int)times.size(); i++)
00278   {
00279     ROS_DEBUG("Input time:%d %f",i,times[i]);
00280     success = success && findSplineSegment(spline,times[i],spline_segment,segment_time);
00281     success = success && sampleSplineTrajectory(spline_segment,segment_time,point_out);    
00282     point_out.time_from_start = ros::Duration(times[i]);
00283     traj_out.points[i] = point_out;
00284   }
00285   traj_out.joint_names = spline.names;
00286   return success;
00287 }
00288 
00296 bool write(const spline_smoother::SplineTrajectory &spline, 
00297            const std::vector<double> &times, 
00298            const std::string &filename)
00299 {
00300   trajectory_msgs::JointTrajectory tp;
00301   if(!sampleSplineTrajectory(spline,times,tp))
00302   {
00303     ROS_ERROR("Input spline was not sampled successfully");
00304     return false;
00305   }
00306 
00307   if(tp.points.empty())
00308   {
00309     ROS_ERROR("Input trajectory is empty");
00310     return false;
00311   }
00312 
00313   FILE *f = fopen(filename.c_str(),"w");
00314   if(!f)
00315     return false;
00316 
00317   int num_joints = tp.points[0].positions.size();
00318   for(int i=0; i < (int) tp.points.size(); i++)
00319   {
00320     fprintf(f,"%f ",tp.points[i].time_from_start.toSec());
00321     for(int j=0; j < num_joints; j++)
00322     {
00323       fprintf(f,"%f ",tp.points[i].positions[j]);
00324     }
00325     for(int j=0; j < num_joints; j++)
00326     {
00327       fprintf(f,"%f ",tp.points[i].velocities[j]);
00328     }
00329     for(int j=0; j < num_joints; j++)
00330     {
00331       fprintf(f,"%f ",tp.points[i].accelerations[j]);
00332     }
00333     fprintf(f,"\n");
00334   }
00335   fclose(f);
00336   return true;
00337 }
00338 
00345 bool writeSpline(const spline_smoother::SplineTrajectory &spline, 
00346                  const std::string &filename)
00347 {
00348   if(spline.segments.empty())
00349     return false;
00350 
00351   int num_segments = spline.segments.size();
00352   int num_joints = spline.segments[0].joints.size();
00353 
00354   FILE *f = fopen(filename.c_str(),"w");
00355   if(!f)
00356     return false;
00357   for(int i=0; i < num_segments; i++)
00358   {
00359     fprintf(f,"%f ",spline.segments[i].duration.toSec());
00360     for(int j=0; j < num_joints; j++)
00361     {
00362       for(unsigned int k=0; k < spline.segments[i].joints[j].coefficients.size(); k++)
00363         fprintf(f,"%f ",spline.segments[i].joints[j].coefficients[k]);
00364     }
00365     fprintf(f,"\n");
00366   }
00367   fclose(f);
00368   return true;    
00369 }
00370 
00378 bool write(const spline_smoother::LSPBTrajectoryMsg &spline, 
00379            const std::vector<double> &times, 
00380            const std::string &filename)
00381 {
00382   trajectory_msgs::JointTrajectory tp;
00383   if(!sampleSplineTrajectory(spline,times,tp))
00384   {
00385     ROS_ERROR("Input spline was not sampled successfully");
00386     return false;
00387   }
00388 
00389   if(tp.points.empty())
00390   {
00391     ROS_ERROR("Input trajectory is empty");
00392     return false;
00393   }
00394 
00395   FILE *f = fopen(filename.c_str(),"w");
00396   if(!f)
00397     return false;
00398 
00399   int num_joints = tp.points[0].positions.size();
00400   for(int i=0; i < (int) tp.points.size(); i++)
00401   {
00402     fprintf(f,"%f ",tp.points[i].time_from_start.toSec());
00403     for(int j=0; j < num_joints; j++)
00404     {
00405       fprintf(f,"%f ",tp.points[i].positions[j]);
00406     }
00407     for(int j=0; j < num_joints; j++)
00408     {
00409       fprintf(f,"%f ",tp.points[i].velocities[j]);
00410     }
00411     for(int j=0; j < num_joints; j++)
00412     {
00413       fprintf(f,"%f ",tp.points[i].accelerations[j]);
00414     }
00415     fprintf(f,"\n");
00416   }
00417   fclose(f);
00418   return true;
00419 }
00420 
00427 bool writeSpline(const spline_smoother::LSPBTrajectoryMsg &spline, 
00428                  const std::string &filename)
00429 {
00430   if(spline.segments.empty())
00431     return false;
00432 
00433   int num_segments = spline.segments.size();
00434   int num_joints = spline.segments[0].joints.size();
00435 
00436   FILE *f = fopen(filename.c_str(),"w");
00437   if(!f)
00438     return false;
00439   for(int i=0; i < num_segments; i++)
00440   {
00441     fprintf(f,"%f ",spline.segments[i].duration.toSec());
00442     for(int j=0; j < num_joints; j++)
00443     {
00444       for(unsigned int k=0; k < spline.segments[i].joints[j].coefficients.size(); k++)
00445         fprintf(f,"%f ",spline.segments[i].joints[j].coefficients[k]);
00446     }
00447     fprintf(f,"\n");
00448   }
00449   fclose(f);
00450   return true;    
00451 }
00452 
00453 }
spline_smoother_utils.cpp
