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/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