Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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.request.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.request.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
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
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
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