00001 00032 #ifndef CARTESIAN_PLANNER_H 00033 #define CARTESIAN_PLANNER_H 00034 00035 #include <moveit/planning_interface/planning_interface.h> 00036 #include <moveit/planning_scene/planning_scene.h> 00037 #include <constrained_ik/moveit_interface/constrained_ik_planning_context.h> 00038 #include <boost/atomic.hpp> 00039 #include <constrained_ik/constrained_ik.h> 00040 00041 namespace constrained_ik 00042 { 00052 class CartesianPlanner : public constrained_ik::CLIKPlanningContext 00053 { 00054 public: 00055 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00056 00062 CartesianPlanner(const std::string &name, const std::string &group); 00063 00068 CartesianPlanner(const CartesianPlanner &other) : constrained_ik::CLIKPlanningContext(other), 00069 terminate_(false), 00070 robot_description_(robot_description_), 00071 solver_(solver_) {} 00072 00074 void clear() override { terminate_ = false; } 00075 00080 bool terminate() override 00081 { 00082 terminate_ = true; 00083 return true; 00084 } 00085 00091 bool solve(planning_interface::MotionPlanResponse &res) override; 00092 00098 bool solve(planning_interface::MotionPlanDetailedResponse &res) override; 00099 00104 bool initializeSolver(); 00105 00110 void setSolverConfiguration(const ConstrainedIKConfiguration &config); 00111 00113 void resetSolverConfiguration(); 00114 00115 private: 00124 std::vector<Eigen::Affine3d,Eigen::aligned_allocator<Eigen::Affine3d> > 00125 interpolateCartesian(const Eigen::Affine3d& start, const Eigen::Affine3d& stop, double ds, double dt) const; 00126 00127 boost::atomic<bool> terminate_; 00128 std::string robot_description_; 00129 robot_model::RobotModelConstPtr robot_model_; 00130 boost::shared_ptr<Constrained_IK> solver_; 00131 boost::mutex mutex_; 00132 }; 00133 } //namespace constrained_ik 00134 00135 #endif // CARTESIAN_PLANNER_H