Go to the documentation of this file.00001
00027 #ifndef INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UPDATE_FILTERS_CONSTRAINED_CARTESIAN_GOAL_H_
00028 #define INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UPDATE_FILTERS_CONSTRAINED_CARTESIAN_GOAL_H_
00029
00030 #include <stomp_moveit/update_filters/stomp_update_filter.h>
00031 #include <stomp_moveit/utils/kinematics.h>
00032
00033 namespace stomp_moveit
00034 {
00035 namespace update_filters
00036 {
00037
00046 class ConstrainedCartesianGoal : public StompUpdateFilter
00047 {
00048 public:
00049 ConstrainedCartesianGoal();
00050 virtual ~ConstrainedCartesianGoal();
00051
00059 virtual bool initialize(moveit::core::RobotModelConstPtr robot_model_ptr,
00060 const std::string& group_name,const XmlRpc::XmlRpcValue& config) override;
00061
00067 virtual bool configure(const XmlRpc::XmlRpcValue& config) override;
00068
00077 virtual bool setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
00078 const moveit_msgs::MotionPlanRequest &req,
00079 const stomp_core::StompConfiguration &config,
00080 moveit_msgs::MoveItErrorCodes& error_code) override;
00081
00093 virtual bool filter(std::size_t start_timestep,
00094 std::size_t num_timesteps,
00095 int iteration_number,
00096 const Eigen::MatrixXd& parameters,
00097 Eigen::MatrixXd& updates,
00098 bool& filtered) override;
00099
00100 virtual std::string getGroupName() const
00101 {
00102 return group_name_;
00103 }
00104
00105 virtual std::string getName() const
00106 {
00107 return name_ + "/" + group_name_;
00108 }
00109
00110 protected:
00111
00112 std::string name_;
00113 std::string group_name_;
00114
00115
00116 utils::kinematics::KinematicConfig kc_;
00117
00118
00119 moveit::core::RobotModelConstPtr robot_model_;
00120 moveit::core::RobotStatePtr state_;
00121 std::string tool_link_;
00122 };
00123
00124 }
00125 }
00126
00127 #endif