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 FRITSCH_BUTLAND_SPLINE_SMOOTHER_H_
00038 #define FRITSCH_BUTLAND_SPLINE_SMOOTHER_H_
00039
00040 #include <spline_smoother/spline_smoother.h>
00041
00042 namespace spline_smoother
00043 {
00044
00053 template <typename T>
00054 class FritschButlandSplineSmoother: public SplineSmoother<T>
00055 {
00056 public:
00057 FritschButlandSplineSmoother();
00058 virtual ~FritschButlandSplineSmoother();
00059
00060 virtual bool smooth(const T& trajectory_in,
00061 T& trajectory_out) const;
00062 };
00063
00064 template <typename T>
00065 FritschButlandSplineSmoother<T>::FritschButlandSplineSmoother()
00066 {
00067 }
00068
00069 template <typename T>
00070 FritschButlandSplineSmoother<T>::~FritschButlandSplineSmoother()
00071 {
00072 }
00073
00074 template <typename T>
00075 bool FritschButlandSplineSmoother<T>::smooth(const T& trajectory_in,
00076 T& trajectory_out) const
00077 {
00078 bool success = true;
00079
00080 int size = trajectory_in.request.trajectory.points.size();
00081 int num_traj = trajectory_in.request.trajectory.joint_names.size();
00082 trajectory_out = trajectory_in;
00083
00084 if (!checkTrajectoryConsistency(trajectory_out))
00085 return false;
00086
00087
00088
00089
00090 for (int i=1; i<size-1; ++i)
00091 {
00092 double h1 = (trajectory_in.request.trajectory.points[i].time_from_start - trajectory_in.request.trajectory.points[i-1].time_from_start).toSec();
00093 double h2 = (trajectory_in.request.trajectory.points[i+1].time_from_start - trajectory_in.request.trajectory.points[i].time_from_start).toSec();
00094
00095
00096 for (int j=0; j<num_traj; ++j)
00097 {
00098 double s1 = (trajectory_in.request.trajectory.points[i].positions[j] - trajectory_in.request.trajectory.points[i-1].positions[j])/h1;
00099 double s2 = (trajectory_in.request.trajectory.points[i+1].positions[j] - trajectory_in.request.trajectory.points[i].positions[j])/h2;
00100
00101 double alpha = (1 + h2/(h1+h2))/3.0;
00102
00103 double vel = 0.0;
00104 if (s1*s2 > 0)
00105 {
00106 vel = (s1*s2)/(alpha*s2 + (1.0-alpha)*s1);
00107 }
00108 trajectory_out.request.trajectory.points[i].velocities[j] = vel;
00109 trajectory_out.request.trajectory.points[i].accelerations[j] = 0.0;
00110 }
00111 }
00112 return success;
00113 }
00114 }
00115
00116 #endif