diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 18ffced5..336d8dec 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -55,15 +55,17 @@ public: bool plan(const planning_scene::PlanningSceneConstPtr from, const planning_scene::PlanningSceneConstPtr to, const moveit::core::JointModelGroup *jmg, - double timeout, - robot_trajectory::RobotTrajectoryPtr& result) override; + double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr from, const moveit::core::LinkModel &link, const Eigen::Affine3d& target, const moveit::core::JointModelGroup *jmg, - double timeout, - robot_trajectory::RobotTrajectoryPtr& result) override; + double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; }; } } } diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 804c800d..8c609c65 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -62,14 +62,16 @@ public: const planning_scene::PlanningSceneConstPtr to, const core::JointModelGroup *jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result) override; + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr from, const moveit::core::LinkModel &link, const Eigen::Affine3d& target, const core::JointModelGroup *jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result) override; + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override; protected: planning_pipeline::PlanningPipelinePtr planner_; diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 197f5ae0..91a872a6 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -39,6 +39,7 @@ #pragma once #include +#include #include #include @@ -77,16 +78,18 @@ public: virtual bool plan(const planning_scene::PlanningSceneConstPtr from, const planning_scene::PlanningSceneConstPtr to, const moveit::core::JointModelGroup *jmg, - double timeout, - robot_trajectory::RobotTrajectoryPtr& result) = 0; + double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; /// plan trajectory from current robot state to Cartesian target virtual bool plan(const planning_scene::PlanningSceneConstPtr from, const moveit::core::LinkModel &link, const Eigen::Affine3d& target, const moveit::core::JointModelGroup *jmg, - double timeout, - robot_trajectory::RobotTrajectoryPtr& result) = 0; + double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; }; std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg); diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index e030b56f..7a9ec275 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -41,12 +41,18 @@ #include #include +#include + namespace moveit { namespace task_constructor { namespace stages { class Connect : public Connecting { public: Connect(std::string name, const solvers::PlannerInterfacePtr &planner); + void setPathConstraints(moveit_msgs::Constraints path_constraints){ + setProperty("path_constraints", std::move(path_constraints)); + } + void init(const moveit::core::RobotModelConstPtr& robot_model); bool compute(const InterfaceState &from, const InterfaceState &to); diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index f0c98d01..a1afe1db 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -59,8 +59,9 @@ void CartesianPath::init(const core::RobotModelConstPtr &robot_model) bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from, const planning_scene::PlanningSceneConstPtr to, const moveit::core::JointModelGroup *jmg, - double timeout, - robot_trajectory::RobotTrajectoryPtr& result) + double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& /*path_constraints*/) { const moveit::core::LinkModel* link; const std::string& link_name = solvers::getEndEffectorLink(jmg); @@ -77,8 +78,9 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from, const moveit::core::LinkModel &link, const Eigen::Affine3d &target, const moveit::core::JointModelGroup *jmg, - double timeout, - robot_trajectory::RobotTrajectoryPtr &result) + double timeout, + robot_trajectory::RobotTrajectoryPtr &result, + const moveit_msgs::Constraints& /*path_constraints*/) { const auto& props = properties(); planning_scene::PlanningScenePtr sandbox_scene = from->diff(); diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index c5c7fa54..6f0bcd52 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -85,7 +85,8 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from, const planning_scene::PlanningSceneConstPtr to, const moveit::core::JointModelGroup *jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result) + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { const auto& props = properties(); moveit_msgs::MotionPlanRequest req; @@ -95,6 +96,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from, req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( to->getCurrentState(), jmg, props.get("goal_joint_tolerance")); + req.path_constraints = path_constraints; ::planning_interface::MotionPlanResponse res; if(!planner_->generatePlan(from, req, res)) @@ -109,8 +111,8 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from, const Eigen::Affine3d& target_eigen, const moveit::core::JointModelGroup *jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result) - + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { const auto& props = properties(); moveit_msgs::MotionPlanRequest req; @@ -125,6 +127,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from, link.getName(), target, props.get("goal_position_tolerance"), props.get("goal_orientation_tolerance")); + req.path_constraints = path_constraints; ::planning_interface::MotionPlanResponse res; if(!planner_->generatePlan(from, req, res)) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index b2805ec1..8e1c83ea 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -48,6 +48,8 @@ Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner) auto& p = properties(); p.declare("timeout", 10.0, "planning timeout"); p.declare("group", "name of planning group"); + p.declare("path_constraints", moveit_msgs::Constraints(), + "constraints to maintain during trajectory"); } void Connect::init(const core::RobotModelConstPtr& robot_model) @@ -63,7 +65,8 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { const moveit::core::JointModelGroup* jmg = from.scene()->getRobotModel()->getJointModelGroup(group); robot_trajectory::RobotTrajectoryPtr trajectory; - if (!planner_->plan(from.scene(), to.scene(), jmg, timeout, trajectory)) + if (!planner_->plan(from.scene(), to.scene(), jmg, timeout, trajectory, + props.get("path_constraints"))) return false; connect(from, to, trajectory);