diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index 10c2c8db..9d566154 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -88,6 +88,8 @@ public: double min_distance, double max_distance); void setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); + + void setRelativeJointSpaceGoal(const std::map& relative_joint_space_goal); }; @@ -107,6 +109,10 @@ public: double min_distance, double max_distance) { setLiftPlace(motion, min_distance, max_distance); } + + void setRelativeJointSpaceLiftGoal(const std::map& relative_joint_space_goal) { + setRelativeJointSpaceGoal(relative_joint_space_goal); + } }; diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index b098749f..ac7f6eac 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -83,4 +83,10 @@ void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, doub p.set("max_distance", max_distance); } +void PickPlaceBase::setRelativeJointSpaceGoal(const std::map& relative_joint_space_goal) +{ + auto& p = lift_stage_->properties(); + p.set("relative_joint_space_goal", relative_joint_space_goal); +} + } } }