Go to the documentation of this file.00001
00026 #ifndef INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UPDATE_FILTERS_STOMP_UPDATE_FILTER_H_
00027 #define INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UPDATE_FILTERS_STOMP_UPDATE_FILTER_H_
00028
00029 #include <Eigen/Core>
00030 #include <XmlRpc.h>
00031 #include <stomp_core/utils.h>
00032 #include <moveit/robot_model/robot_model.h>
00033 #include <moveit/robot_trajectory/robot_trajectory.h>
00034 #include <moveit/planning_scene/planning_scene.h>
00035 #include <moveit_msgs/MotionPlanRequest.h>
00036
00037
00038 namespace stomp_moveit
00039 {
00040
00041 namespace update_filters
00042 {
00043
00052 class StompUpdateFilter;
00053 typedef boost::shared_ptr<StompUpdateFilter> StompUpdateFilterPtr;
00054
00055 class StompUpdateFilter
00056 {
00057 public:
00058 StompUpdateFilter(){}
00059 virtual ~StompUpdateFilter(){}
00060
00068 virtual bool initialize(moveit::core::RobotModelConstPtr robot_model_ptr,
00069 const std::string& group_name,const XmlRpc::XmlRpcValue& config) = 0;
00070
00076 virtual bool configure(const XmlRpc::XmlRpcValue& config) = 0;
00077
00086 virtual bool setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
00087 const moveit_msgs::MotionPlanRequest &req,
00088 const stomp_core::StompConfiguration &config,
00089 moveit_msgs::MoveItErrorCodes& error_code) = 0;
00090
00102 virtual bool filter(std::size_t start_timestep,
00103 std::size_t num_timesteps,
00104 int iteration_number,
00105 const Eigen::MatrixXd& parameters,
00106 Eigen::MatrixXd& updates,
00107 bool& filtered) = 0 ;
00108
00117 virtual void postIteration(std::size_t start_timestep,
00118 std::size_t num_timesteps,int iteration_number,double cost,const Eigen::MatrixXd& parameters){}
00119
00128 virtual void done(bool success,int total_iterations,double final_cost,const Eigen::MatrixXd& parameters){}
00129
00130
00131 virtual std::string getName() const
00132 {
00133 return "Not implemented";
00134 }
00135
00136
00137 virtual std::string getGroupName() const
00138 {
00139 return "Not implemented";
00140 }
00141
00142 };
00143
00144 }
00145
00146 }
00147
00148
00149
00150 #endif