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..566d3489 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -41,12 +41,22 @@ #include #include +#include + namespace moveit { namespace task_constructor { namespace stages { class Connect : public Connecting { public: Connect(std::string name, const solvers::PlannerInterfacePtr &planner); + void setTimeout(const ros::Duration& timeout){ + setProperty("timeout", timeout.toSec()); + } + + 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/include/moveit/task_constructor/stages/move_relative.h b/core/include/moveit/task_constructor/stages/move_relative.h index ab09eef6..82d403f9 100644 --- a/core/include/moveit/task_constructor/stages/move_relative.h +++ b/core/include/moveit/task_constructor/stages/move_relative.h @@ -40,6 +40,7 @@ #include #include +#include #include #include @@ -62,6 +63,10 @@ public: void setMaxDistance(double distance); void setMinMaxDistance(double min_distance, double max_distance); + void setPathConstraints(moveit_msgs::Constraints path_constraints){ + setProperty("path_constraints", std::move(path_constraints)); + } + /// perform twist motion on specified link void along(const geometry_msgs::TwistStamped& twist); /// translate link along given direction diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index c96a2b94..1a44686d 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -40,6 +40,7 @@ #include #include +#include #include #include @@ -63,6 +64,10 @@ public: /// move joint model group to given named pose void setGoal(const std::string& joint_pose); + void setPathConstraints(moveit_msgs::Constraints path_constraints){ + setProperty("path_constraints", std::move(path_constraints)); + } + protected: bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene, SubTrajectory &trajectory, Direction dir); diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index f0c98d01..e49032cb 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); @@ -70,25 +71,31 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from, } // reach pose of forward kinematics - return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result); + return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), + jmg, timeout, result, path_constraints); } 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(); - auto isValid = [&sandbox_scene](moveit::core::RobotState* state, + kinematic_constraints::KinematicConstraintSet kcs(sandbox_scene->getRobotModel()); + kcs.add(path_constraints, sandbox_scene->getTransforms()); + + auto isValid = [&sandbox_scene, &kcs](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, const double* joint_positions) { state->setJointGroupPositions(jmg, joint_positions); state->update(); - return !sandbox_scene->isStateColliding(const_cast(*state), jmg->getName()); + return !sandbox_scene->isStateColliding(const_cast(*state), jmg->getName()) + && kcs.decide(*state).satisfied; }; std::vector trajectory; 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); diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index b27668fd..b8273f62 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -57,6 +57,9 @@ MoveRelative::MoveRelative(std::string name, const solvers::PlannerInterfacePtr& p.declare("twist", "Cartesian twist transform"); p.declare("direction", "Cartesian translation direction"); + + p.declare("path_constraints", moveit_msgs::Constraints(), + "constraints to maintain during trajectory"); } void MoveRelative::setGroup(const std::string& group){ @@ -204,7 +207,8 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning } robot_trajectory::RobotTrajectoryPtr robot_trajectory; - bool success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory); + bool success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, + props.get("path_constraints")); // min_distance reached? if (success && min_distance > 0.0) { diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index b77f8cc8..a6e4ac98 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -55,6 +55,9 @@ MoveTo::MoveTo(std::string name, const solvers::PlannerInterfacePtr& planner) p.declare("pose", "Cartesian target pose"); p.declare("point", "Cartesian target point"); p.declare("joint_pose", "named joint pose"); + + p.declare("path_constraints", moveit_msgs::Constraints(), + "constraints to maintain during trajectory"); } void MoveTo::setGroup(const std::string& group){ @@ -108,6 +111,8 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP return false; } + const auto& path_constraints = props.get("path_constraints"); + robot_trajectory::RobotTrajectoryPtr robot_trajectory; bool success = false; @@ -118,7 +123,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP ROS_WARN("MoveTo: unknown joint pose '%s' for jmg '%s'", named_joint_pose.c_str(), group.c_str()); return false; } - success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory); + success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); } else { // Cartesian targets require the link name std::string link_name = props.get("link"); @@ -167,7 +172,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP tf::poseEigenToMsg(target_eigen, pose_msg.pose); rviz_marker_tools::appendFrame(solution.markers(), pose_msg, 0.1, "ik frame"); - success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory); + success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); } // store result