smoothing_trajectory_filter.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Southwest Research Institute nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Chris Lewis */
36 
38 #include <console_bridge/console.h>
40 #include <stdio.h>
41 
42 #include <ros/ros.h>
43 #include <ros/console.h>
44 
46 {
47 
48 
50  {
51  initialized_ = false;
52  }
53 
54  bool SmoothingTrajectoryFilter::init(std::vector<double> &coef)
55  {
56  if(coef.size()%2 == 1) { // smoothing filters must have an odd number of coefficients
57  initialized_ = true;
58  num_coef_ = coef.size();
59  double sum =0;
60  for(int i=0; i<num_coef_; i++) {
61  coef_.push_back(coef[i]);
62  sum += coef[i];
63  }
64  gain_ = sum; // set gain to be the sum of the coefficients because we need unity gain
65  return(true);
66  }
67  else{
68  initialized_ = false;
69  return(false);
70  }
71  }
72 
74  {
75  coef_.clear();
76  }
77 
79  {
80  if(!initialized_) return(false);
81 
82  const int num_points = rob_trajectory.getWayPointCount();
83  if(num_points <=2) return(false); // nothing to do here, can't change either first or last point
84  const int num_states = rob_trajectory.getWayPoint(0).getVariableCount();
85  std::vector<double> xv;
86 
87  // filter each variable independently
88  for(int i=0; i<num_states; i++){
89  double start_value = rob_trajectory.getWayPoint(0).getVariablePosition(i);
90  double start_slope = rob_trajectory.getWayPoint(1).getVariablePosition(i) - start_value; // slope at start
91  double end_value = rob_trajectory.getWayPoint(num_points-1).getVariablePosition(i);
92  double end_slope = end_value - rob_trajectory.getWayPoint(num_points-2).getVariablePosition(i); // slope at end
93 
94  // initialize the filter to have initial slope
95  xv.clear();
96  double value = start_value - (num_coef_/2)*start_slope;
97  for(int j=0; j<num_coef_; j++) {
98  xv.push_back(value);
99  value += start_slope;
100  }
101 
102  // cycle through every waypoint, and apply the filter, NOTE, 1st and last waypoints should not be changed
103  for(int j=1; j<num_points-1; j++){
104  // shift backwards
105  for(int k=0; k<num_coef_-1; k++){
106  xv[k] = xv[k+1];
107  }
108 
109  // get next input to filter which is num_coef/2 in front of current point being smoothed
110  if(j+num_coef_/2 < num_points){
111  xv[num_coef_ - 1] = rob_trajectory.getWayPoint(j+num_coef_/2).getVariablePosition(i); // i'th state of j'th waypoint
112  }
113  else{
114  end_value += end_slope;
115  xv[num_coef_-1] = end_value; // fill by continuing with final slope
116  }
117  // apply the filter
118  double sum = 0.0;
119  for(int k=0; k<num_coef_; k++){
120  sum += xv[k]*coef_[k];
121  }
122 
123  // save the results
124  rob_trajectory.getWayPointPtr(j)->setVariablePosition(i,sum/gain_); // j'th waypoint, i'th variable set to output value
125 
126  }// end for every waypoint
127 
128  } // end for every state
129 
130  return(true);
131 
132 }// end SmoothingTrajectoryFilter::applyfilter()
133 
134 } // end namespace
robot_state::RobotStatePtr & getWayPointPtr(std::size_t index)
bool applyFilter(robot_trajectory::RobotTrajectory &rob_trajectory) const
const robot_state::RobotState & getWayPoint(std::size_t index) const
std::size_t getWayPointCount() const


open_manipulator_with_tb3_waffle_pi_moveit
Author(s): Darby Lim
autogenerated on Mon Jun 10 2019 14:13:54