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 NORMALIZE_JOINT_TRAJECTORY_H_
00038 #define NORMALIZE_JOINT_TRAJECTORY_H_
00039
00040 #include <spline_smoother/spline_smoother.h>
00041 #include <spline_smoother/spline_smoother_utils.h>
00042 #include <angles/angles.h>
00043
00044 namespace joint_normalization_filters
00045 {
00046
00050 template <typename T>
00051 class NormalizeJointTrajectory: public spline_smoother::SplineSmoother<T>
00052 {
00053 public:
00054 NormalizeJointTrajectory();
00055 virtual ~NormalizeJointTrajectory();
00056
00057 virtual bool smooth(const T& trajectory_in,
00058 T& trajectory_out) const;
00059
00060 };
00061
00062 template <typename T>
00063 bool NormalizeJointTrajectory<T>::smooth(const T& data_in,
00064 T& data_out) const
00065 {
00066 data_out = data_in;
00067
00068 int size = data_in.trajectory.points.size();
00069 int num_joints = data_in.trajectory.joint_names.size();
00070
00071 if (!spline_smoother::checkTrajectoryConsistency(data_out))
00072 return false;
00073
00074 for (int i=0; i<num_joints; ++i)
00075 {
00076 if (!data_out.limits[i].has_position_limits)
00077 {
00078 for (int j=1; j<size; ++j)
00079 {
00080 double& cur = data_out.trajectory.points[j].positions[i];
00081 double prev = data_out.trajectory.points[j-1].positions[i];
00082
00083 cur = prev + angles::shortest_angular_distance(prev, cur);
00084 }
00085 }
00086 }
00087 return true;
00088 }
00089
00090 template <typename T>
00091 NormalizeJointTrajectory<T>::NormalizeJointTrajectory()
00092 {
00093 }
00094
00095 template <typename T>
00096 NormalizeJointTrajectory<T>::~NormalizeJointTrajectory()
00097 {
00098 }
00099
00100 }
00101
00102 #endif