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