Merge pull request ros-industrial#77 from drchrislewis/issue_76

Issue 76

industrial_trajectory_filters/CMakeLists.txt

@@ -15,7 +15,12 @@ include_directories(include
 )
 
 
-add_library(${PROJECT_NAME} src/n_point_filter.cpp src/uniform_sample_filter.cpp)
+add_library(${PROJECT_NAME} 
+  src/n_point_filter.cpp 
+  src/uniform_sample_filter.cpp
+  src/add_smoothing_filter.cpp
+  src/smoothing_trajectory_filter.cpp
+)
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 
 

industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h

@@ -0,0 +1,74 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, Southwest Research Institute
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Chris Lewis */
+
+#ifndef MOVEIT_TRAJECTORY_PROCESSING_SMOOTHING_TRAJECTORY_FILTER_
+#define MOVEIT_TRAJECTORY_PROCESSING_SMOOTHING_TRAJECTORY_FILTER_
+
+#include <moveit/robot_trajectory/robot_trajectory.h>
+
+namespace industrial_trajectory_filters
+{
+
+  /*! \brief This class  filters the trajectory using a Finite Impluse Response filter */
+class SmoothingTrajectoryFilter
+{
+public:
+  /*!  \brief Constructor */
+  SmoothingTrajectoryFilter();
+
+  /*! \brief Destructor */
+  ~SmoothingTrajectoryFilter();
+
+  /*!  \brief Constructor
+   *    @param coef   a vector of Smoothing coeficients with an odd number of values
+   *    @return  true if the number of coeficients is odd, otherwise, false. All smoothing filters have odd number of coef
+   */
+  bool init(std::vector<double> &coef);
+
+  /* \brief action of filter depends on the coefficients, intended to be a low pass filter
+   *  @param rob_trajectory   A robot_trajectory::RobotTrajectory to be filtered
+   */
+  bool applyFilter(robot_trajectory::RobotTrajectory& rob_trajectory) const;  
+
+private:
+  double gain_; /*!< gain_ is the sum of the coeficients to achieve unity gain overall */
+  int num_coef_; /*< the number of coefficients  */
+  std::vector<double> coef_; /*!< Vector of coefficients  */ 
+  bool initialized_; /*!< was the init() function called sucessfully? */
+};
+
+}
+#endif

industrial_trajectory_filters/planning_request_adapters_plugin_description.xml

@@ -20,5 +20,21 @@
 	- sample_duration (default = 0.050)
     </description>
   </class>
+  <class name="industrial_trajectory_filters/AddSmoothingFilter" type="industrial_trajectory_filters::AddSmoothingFilter" base_class_type="planning_request_adapter::PlanningRequestAdapter">
+    <description>
+           This filter replaces each point with a weighted average of the adjacent 
+           waypoints/states in the trajectory. Odd number of weights are provided
+           via a yaml file loaded upon launch onto the ROS parameter server.
+           ROS Parameters Example:
+                  smoothing_filter_name: /move_group/smoothing_5_coef
+                  smoothing_5_coef:
+                            - 0.25
+                            - 0.50
+                            - 1.00
+                            - 0.50
+                            - 0.25
+    </description>
+  </class>
+
 
 </library>

industrial_trajectory_filters/src/add_smoothing_filter.cpp

@@ -0,0 +1,141 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, Southwest Research Institute
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Southwest Research Institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Chris Lewis */
+
+#include <moveit/planning_request_adapter/planning_request_adapter.h>
+#include <class_loader/class_loader.h>
+
+#include <industrial_trajectory_filters/smoothing_trajectory_filter.h>
+#include <ros/ros.h>  // required for NodeHandle
+#include <ros/console.h>
+
+namespace industrial_trajectory_filters
+{
+
+class AddSmoothingFilter : public planning_request_adapter::PlanningRequestAdapter
+{
+public:
+
+  static const std::string FILTER_PARAMETER_NAME_; // base name for filter parameters
+
+  /*!  \brief Constructor AddSmoothingFilter is a planning request adapter plugin which post-processes
+  *             The robot's trajectory to round out corners
+  *             The filter name and then its coefficients are obtained from the ROS parameter server
+  */
+  AddSmoothingFilter() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
+  {
+      int num_coef;
+
+      // set of default filter coefficients in case paramter, or its associated file, or its syntax is wrong
+      filter_coef_.push_back(0.25);
+      filter_coef_.push_back(0.5);
+      filter_coef_.push_back(1.0);
+      filter_coef_.push_back(0.5);
+      filter_coef_.push_back(0.25);
+
+    // see if a new set of filter parameters is on the parameter server, if so, install them
+    if (!nh_.getParam(FILTER_PARAMETER_NAME_, filter_name_)){
+      ROS_INFO_STREAM("Param '" << FILTER_PARAMETER_NAME_ << "' was not set. Using default filter values " );
+    }
+    else{ // base filter name exists
+      std::vector<double> temp_coef;
+      nh_.getParam(filter_name_.c_str(),temp_coef); // no need to check for success
+      if(temp_coef.size()%2 == 1 && temp_coef.size()>2){ // need to have odd number of coefficients greater than 2
+	filter_coef_.clear();
+	for(int i=0; i< (int) temp_coef.size(); i++){ // install the filter coefficients
+	  filter_coef_.push_back(temp_coef[i]);
+	}
+      }
+      else{
+	ROS_INFO_STREAM("Could not read filter, using default filter coefficients");
+      }
+    }
+    if(!smoothing_filter_.init(filter_coef_))
+      ROS_ERROR("Initialization error on smoothing filter. Requires an odd number of coeficients");
+
+  };
+
+  /*!  \brief Destructor */
+  ~AddSmoothingFilter(){ };
+
+  /*!  \brief Returns a short description of this plugin */
+  virtual std::string getDescription() const { return "Add Smoothing Trajectory Filter"; }
+
+  /*!  \brief The work hourse of planning request adapters
+   *   \param planner A function called somewhere within this subroutine
+   *   \param planning_scene  an object describing the objects and kinematics
+   *   \param req   the request, includes the starting config, the goal, constraints, etc
+   *   \param res    the response, includes the robot trajectory and other info
+   *   \param added_path_index, a index of the points added by this adapter, which in this case will be empty
+   */
+  virtual bool adaptAndPlan(const PlannerFn &planner,
+                            const planning_scene::PlanningSceneConstPtr& planning_scene,
+                            const planning_interface::MotionPlanRequest &req,
+                            planning_interface::MotionPlanResponse &res,
+                            std::vector<std::size_t> &added_path_index) const
+  {
+    // do anything prior to calling the planner here
+    // ....
+    // IN this case nothing was done
+
+    // call the nested planner or adapter
+    bool result = planner(planning_scene, req, res); 
+
+    // do anything after calling the nested planner or adapters here
+    if (result && res.trajectory_) // successful plan
+    {
+      ROS_DEBUG("Running '%s'", getDescription().c_str()); // inform the user 
+      if (!smoothing_filter_.applyFilter(*res.trajectory_)) {// do the smoothing
+	ROS_ERROR("Smoothing filter of the solution path failed. Filter Not Initialized ");
+      }
+    }// end of if successful plan
+    return result;
+  };
+
+
+private:
+  ros::NodeHandle nh_;
+  industrial_trajectory_filters::SmoothingTrajectoryFilter smoothing_filter_;
+  std::string filter_name_;
+  std::vector<double> filter_coef_;
+
+};
+
+const std::string  AddSmoothingFilter::FILTER_PARAMETER_NAME_ = "/move_group/smoothing_filter_name";
+
+}
+
+CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::AddSmoothingFilter,
+                            planning_request_adapter::PlanningRequestAdapter);

industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp

@@ -0,0 +1,134 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, Southwest Research Institute
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Southwest Research Institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Chris Lewis */
+
+#include <industrial_trajectory_filters/smoothing_trajectory_filter.h>
+#include <console_bridge/console.h>
+#include <moveit/robot_state/conversions.h>
+#include <stdio.h>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+
+namespace industrial_trajectory_filters
+{
+
+
+  SmoothingTrajectoryFilter::SmoothingTrajectoryFilter()
+  {
+      initialized_ = false;
+  }
+
+  bool SmoothingTrajectoryFilter::init(std::vector<double> &coef)
+  {
+    if(coef.size()%2 == 1) {		// smoothing filters must have an odd number of coefficients
+      initialized_ = true;
+      num_coef_ = coef.size();
+      double sum =0;
+      for(int i=0; i<num_coef_; i++) {
+	coef_.push_back(coef[i]);
+	sum += coef[i];
+      }
+      gain_ = sum;		// set gain to be the sum of the coefficients because we need unity gain
+      return(true);
+    }
+    else{
+      initialized_ = false;
+      return(false);
+    }
+  }
+
+  SmoothingTrajectoryFilter::~SmoothingTrajectoryFilter()
+  {
+    coef_.clear();
+  }
+
+ bool SmoothingTrajectoryFilter::applyFilter(robot_trajectory::RobotTrajectory& rob_trajectory) const
+  {
+    if(!initialized_) return(false);
+
+    const int num_points = rob_trajectory.getWayPointCount(); 
+    if(num_points <=2) return(false); // nothing to do here, can't change either first or last point
+    const int num_states = rob_trajectory.getWayPoint(0).getVariableCount();
+    std::vector<double> xv;
+
+    // filter each variable independently
+    for(int i=0; i<num_states; i++){ 
+      double start_value     = rob_trajectory.getWayPoint(0).getVariablePosition(i);
+      double start_slope     = rob_trajectory.getWayPoint(1).getVariablePosition(i) - start_value; // slope at start
+      double end_value      = rob_trajectory.getWayPoint(num_points-1).getVariablePosition(i);
+      double end_slope      = end_value - rob_trajectory.getWayPoint(num_points-2).getVariablePosition(i); // slope at end
+
+      // initialize the filter to have initial slope
+      xv.clear();
+      double value = start_value - (num_coef_/2)*start_slope;
+      for(int j=0; j<num_coef_; j++) { 
+	xv.push_back(value);
+	value += start_slope;
+      }
+
+      // cycle through every waypoint, and apply the filter, NOTE, 1st and last waypoints should not be changed
+      for(int j=1; j<num_points-1; j++){
+	// shift backwards
+	for(int k=0; k<num_coef_-1; k++){
+	  xv[k] = xv[k+1];  
+	}
+
+	// get next input to filter which is num_coef/2 in front of current point being smoothed
+	if(j+num_coef_/2 < num_points){	
+	  xv[num_coef_ - 1] = rob_trajectory.getWayPoint(j+num_coef_/2).getVariablePosition(i); // i'th state of j'th waypoint
+	}
+	else{
+	  end_value += end_slope;
+	  xv[num_coef_-1] = end_value; // fill by continuing with final slope
+	}
+	// apply the filter
+	double sum = 0.0;
+	for(int k=0; k<num_coef_; k++){ 
+	  sum += xv[k]*coef_[k];
+	}
+
+	// save the results
+	rob_trajectory.getWayPointPtr(j)->setVariablePosition(i,sum/gain_); // j'th waypoint, i'th variable set to output value
+
+      }// end for every waypoint
+
+    } // end for every state
+
+    return(true);
+
+}// end SmoothingTrajectoryFilter::applyfilter()
+
+}  // end namespace