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
00035
00036
00037 #include <industrial_trajectory_filters/smoothing_trajectory_filter.h>
00038 #include <console_bridge/console.h>
00039 #include <moveit/robot_state/conversions.h>
00040 #include <stdio.h>
00041
00042 #include <ros/ros.h>
00043 #include <ros/console.h>
00044
00045 namespace industrial_trajectory_filters
00046 {
00047
00048
00049 SmoothingTrajectoryFilter::SmoothingTrajectoryFilter()
00050 {
00051 initialized_ = false;
00052 }
00053
00054 bool SmoothingTrajectoryFilter::init(std::vector<double> &coef)
00055 {
00056 if(coef.size()%2 == 1) {
00057 initialized_ = true;
00058 num_coef_ = coef.size();
00059 double sum =0;
00060 for(int i=0; i<num_coef_; i++) {
00061 coef_.push_back(coef[i]);
00062 sum += coef[i];
00063 }
00064 gain_ = sum;
00065 return(true);
00066 }
00067 else{
00068 initialized_ = false;
00069 return(false);
00070 }
00071 }
00072
00073 SmoothingTrajectoryFilter::~SmoothingTrajectoryFilter()
00074 {
00075 coef_.clear();
00076 }
00077
00078 bool SmoothingTrajectoryFilter::applyFilter(robot_trajectory::RobotTrajectory& rob_trajectory) const
00079 {
00080 if(!initialized_) return(false);
00081
00082 const int num_points = rob_trajectory.getWayPointCount();
00083 if(num_points <=2) return(false);
00084 const int num_states = rob_trajectory.getWayPoint(0).getVariableCount();
00085 std::vector<double> xv;
00086
00087
00088 for(int i=0; i<num_states; i++){
00089 double start_value = rob_trajectory.getWayPoint(0).getVariablePosition(i);
00090 double start_slope = rob_trajectory.getWayPoint(1).getVariablePosition(i) - start_value;
00091 double end_value = rob_trajectory.getWayPoint(num_points-1).getVariablePosition(i);
00092 double end_slope = end_value - rob_trajectory.getWayPoint(num_points-2).getVariablePosition(i);
00093
00094
00095 xv.clear();
00096 double value = start_value - (num_coef_/2)*start_slope;
00097 for(int j=0; j<num_coef_; j++) {
00098 xv.push_back(value);
00099 value += start_slope;
00100 }
00101
00102
00103 for(int j=1; j<num_points-1; j++){
00104
00105 for(int k=0; k<num_coef_-1; k++){
00106 xv[k] = xv[k+1];
00107 }
00108
00109
00110 if(j+num_coef_/2 < num_points){
00111 xv[num_coef_ - 1] = rob_trajectory.getWayPoint(j+num_coef_/2).getVariablePosition(i);
00112 }
00113 else{
00114 end_value += end_slope;
00115 xv[num_coef_-1] = end_value;
00116 }
00117
00118 double sum = 0.0;
00119 for(int k=0; k<num_coef_; k++){
00120 sum += xv[k]*coef_[k];
00121 }
00122
00123
00124 rob_trajectory.getWayPointPtr(j)->setVariablePosition(i,sum/gain_);
00125
00126 }
00127
00128 }
00129
00130 return(true);
00131
00132 }
00133
00134 }