spline_smoother: spline_smoother: clamped_cubic_spline

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 #ifndef CLAMPED_CUBIC_SPLINE_SMOOTHER_H_
00038 #define CLAMPED_CUBIC_SPLINE_SMOOTHER_H_
00039 
00040 #include "spline_smoother.h"
00041 #include "numerical_differentiation_spline_smoother.h"
00042 #include <spline_smoother/spline_smoother_utils.h>
00043 
00044 namespace spline_smoother
00045 {
00046 
00058 template <typename T>
00059 class ClampedCubicSplineSmoother: public SplineSmoother<T>
00060 {
00061 public:
00062   ClampedCubicSplineSmoother();
00063   virtual ~ClampedCubicSplineSmoother();
00064 
00065   static const int MAX_TRIDIAGONAL_SOLVER_ELEMENTS = 20;
00066 
00072   virtual bool smooth(const T& trajectory_in, 
00073                       T& trajectory_out) const;
00074 
00075 private:
00076   NumericalDifferentiationSplineSmoother<T> num_diff_spline_smoother_;
00077   bool smoothSegment(std::vector<trajectory_msgs::JointTrajectoryPoint>& waypoints) const;
00078 
00079 };
00080 
00081 
00082 template <typename T>
00083 ClampedCubicSplineSmoother<T>::ClampedCubicSplineSmoother()
00084 {
00085 }
00086 
00087 template <typename T>
00088 ClampedCubicSplineSmoother<T>::~ClampedCubicSplineSmoother()
00089 {
00090 }
00091 
00092 template <typename T>
00093 bool ClampedCubicSplineSmoother<T>::smooth(const T& trajectory_in, T& trajectory_out) const
00094 {
00095   int length = trajectory_in.trajectory.points.size();
00096   trajectory_out = trajectory_in;
00097 
00098   if (!checkTrajectoryConsistency(trajectory_out))
00099     return false;
00100 
00101   if (length<3)
00102     return true;
00103 
00104   if (length <= MAX_TRIDIAGONAL_SOLVER_ELEMENTS)
00105   {
00106     smoothSegment(trajectory_out.trajectory.points);
00107   }
00108   else
00109   {
00110     ROS_ERROR("ClampedCubicSplineSmoother: does not support trajectory lengths > %d due to numerical instability.", MAX_TRIDIAGONAL_SOLVER_ELEMENTS);
00111     return false;
00112   }
00113 
00114   return true;
00115 }
00116 
00117 template <typename T>
00118 bool ClampedCubicSplineSmoother<T>::smoothSegment(std::vector<trajectory_msgs::JointTrajectoryPoint>& wpts) const
00119 {
00120   int length = wpts.size();
00121   int num_joints = wpts[0].positions.size();
00122   if (length < 3)
00123     return true;
00124 
00125   std::vector<double> intervals(length-1);
00126 
00127   // generate time intervals:
00128   for (int i=0; i<length-1; i++)
00129     intervals[i] = (wpts[i+1].time_from_start - wpts[i].time_from_start).toSec();
00130 
00131   // arrays for tridiagonal matrix
00132   std::vector<double> a(length-2);
00133   std::vector<double> b(length-2);
00134   std::vector<double> c(length-2);
00135   std::vector<double> d(length-2);
00136   std::vector<double> x(length-2);
00137 
00138   // for each joint:
00139   for (int j=0; j<num_joints; j++)
00140   {
00141     a[0] = 0.0;
00142     c[length-3] = 0.0;
00143     for (int i=0; i<length-2; i++)
00144     {
00145       c[i] = intervals[i];
00146       if (i<length-3)
00147         a[i+1] = intervals[i+2];
00148       b[i] = 2.0*(intervals[i] + intervals[i+1]);
00149       d[i] = (3.0/(intervals[i]*intervals[i+1]))*
00150           ((intervals[i]*intervals[i])*(wpts[i+2].positions[j]-wpts[i+1].positions[j]) +
00151               (intervals[i+1]*intervals[i+1])*(wpts[i+1].positions[j]-wpts[i].positions[j]));
00152     }
00153     d[0] -= wpts[0].velocities[j]*intervals[1];
00154     d[length-3] -= wpts[length-1].velocities[j]*intervals[length-3];
00155 
00156     tridiagonalSolve(a, b, c, d, x);
00157     for (int i=0; i<length-2; i++)
00158       wpts[i+1].velocities[j] = x[i];
00159   }
00160   return true;
00161 }
00162 
00163 
00164 }
00165 
00166 #endif /* CLAMPED_CUBIC_SPLINE_SMOOTHER_H_ */
clamped_cubic_spline_smoother.h
