00001 00026 #ifndef STOMP_MOVEIT_STOMP_PLANNER_H_ 00027 #define STOMP_MOVEIT_STOMP_PLANNER_H_ 00028 00029 #include <moveit/planning_interface/planning_interface.h> 00030 #include <stomp_core/stomp.h> 00031 #include <stomp_moveit/stomp_optimization_task.h> 00032 #include <boost/thread.hpp> 00033 #include <ros/ros.h> 00034 00035 namespace stomp_moveit 00036 { 00037 00038 using StompOptimizationTaskPtr = boost::shared_ptr<StompOptimizationTask>; 00039 00047 class StompPlanner: public planning_interface::PlanningContext 00048 { 00049 public: 00056 StompPlanner(const std::string& group,const XmlRpc::XmlRpcValue& config,const moveit::core::RobotModelConstPtr& model); 00057 virtual ~StompPlanner(); 00058 00064 virtual bool solve(planning_interface::MotionPlanResponse &res) override; 00065 00071 virtual bool solve(planning_interface::MotionPlanDetailedResponse &res) override; 00072 00077 virtual bool terminate() override; 00078 00082 virtual void clear() override; 00083 00091 static bool getConfigData(ros::NodeHandle &nh, std::map<std::string, XmlRpc::XmlRpcValue> &config, std::string param = std::string("stomp")); 00092 00097 bool canServiceRequest(const moveit_msgs::MotionPlanRequest &req) const; 00098 00106 static moveit_msgs::TrajectoryConstraints encodeSeedTrajectory(const trajectory_msgs::JointTrajectory& seed); 00107 00108 protected: 00109 00113 void setup(); 00114 00121 bool getStartAndGoal(Eigen::VectorXd& start, Eigen::VectorXd& goal); 00122 00131 bool getSeedParameters(Eigen::MatrixXd& parameters) const; 00132 00139 bool parametersToJointTrajectory(const Eigen::MatrixXd& parameters, trajectory_msgs::JointTrajectory& traj); 00140 00147 bool jointTrajectorytoParameters(const trajectory_msgs::JointTrajectory& traj, Eigen::MatrixXd& parameters) const; 00148 00156 bool extractSeedTrajectory(const moveit_msgs::MotionPlanRequest& req, trajectory_msgs::JointTrajectory& seed) const; 00157 00158 protected: 00159 00160 // stomp optimization 00161 boost::shared_ptr< stomp_core::Stomp> stomp_; 00162 StompOptimizationTaskPtr task_; 00163 XmlRpc::XmlRpcValue config_; 00164 stomp_core::StompConfiguration stomp_config_; 00165 00166 // robot environment 00167 moveit::core::RobotModelConstPtr robot_model_; 00168 00169 // ros tasks 00170 ros::NodeHandlePtr ph_; 00171 }; 00172 00173 00174 } /* namespace stomp_moveit */ 00175 #endif /* STOMP_MOVEIT_STOMP_PLANNER_H_ */