From 7f3bdaa607d114f662d591c0ca4de405536ec56a Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Nov 2019 17:32:48 +0100 Subject: [PATCH] Add parameter struct --- .../src/plan_pick_place_capability.cpp | 10 +- .../task_constructor/tasks/pick_place_task.h | 83 ++- core/src/tasks/pick_place_task.cpp | 637 ++++++++---------- 3 files changed, 338 insertions(+), 392 deletions(-) diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp index 0f0ffd07..4ecdced3 100644 --- a/capabilities/src/plan_pick_place_capability.cpp +++ b/capabilities/src/plan_pick_place_capability.cpp @@ -51,17 +51,21 @@ void PlanPickPlaceCapability::initialize() { std::bind(&PlanPickPlaceCapability::goalCallback, this, std::placeholders::_1), false)); as_->registerPreemptCallback(std::bind(&PlanPickPlaceCapability::preemptCallback, this)); as_->start(); - ros::NodeHandle nh("~"); - pick_place_task_ = std::make_unique("", nh); + pick_place_task_ = std::make_unique("pick_place_task"); } void PlanPickPlaceCapability::goalCallback( const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal) { moveit_task_constructor_msgs::PlanPickPlaceResult result; + // TODO: fill parameters + PickPlaceTask::Parameters parameters; + // initialize task + pick_place_task_->init(parameters); // run plan - // return result + pick_place_task_->plan(); + // retrieve and return result } void PlanPickPlaceCapability::preemptCallback() { diff --git a/core/include/moveit/task_constructor/tasks/pick_place_task.h b/core/include/moveit/task_constructor/tasks/pick_place_task.h index 71a0a32d..5a50ef38 100644 --- a/core/include/moveit/task_constructor/tasks/pick_place_task.h +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -73,55 +73,54 @@ using namespace moveit::task_constructor; class PickPlaceTask { public: - PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); + struct Parameters + { + // planning group properties + std::string arm_group_name_; + std::string eef_name_; + std::string hand_group_name_; + std::string hand_frame_; + + // object + surface + std::vector support_surfaces_; + std::string object_reference_frame_; + std::string surface_link_; + std::string object_name_; + std::string world_frame_; + std::string grasp_frame_; + std::vector object_dimensions_; + + // Predefined pose targets + std::string hand_open_pose_; + std::string hand_close_pose_; + std::string arm_home_pose_; + + // Pick metrics + Eigen::Isometry3d grasp_frame_transform_; + double approach_object_min_dist_; + double approach_object_max_dist_; + double lift_object_min_dist_; + double lift_object_max_dist_; + double place_object_min_dist_; + double place_object_max_dist_; + double retreat_object_min_dist_; + double retreat_object_max_dist_; + + // Place metrics + geometry_msgs::PoseStamped place_pose_; + double place_surface_offset_; + }; + + PickPlaceTask(const std::string& task_name); ~PickPlaceTask() = default; - void loadParameters(); - - void init(); + void init(const Parameters& parameters); bool plan(); - bool execute(); - private: - ros::NodeHandle nh_; - - std::string task_name_; moveit::task_constructor::TaskPtr task_; - - // planning group properties - std::string arm_group_name_; - std::string eef_name_; - std::string hand_group_name_; - std::string hand_frame_; - - // object + surface - std::vector support_surfaces_; - std::string object_reference_frame_; - std::string surface_link_; - std::string object_name_; - std::string world_frame_; - std::vector object_dimensions_; - - // Predefined pose targets - std::string hand_open_pose_; - std::string hand_close_pose_; - std::string arm_home_pose_; - - // Execution - actionlib::SimpleActionClient execute_; - - // Pick metrics - Eigen::Isometry3d grasp_frame_transform_; - double approach_object_min_dist_; - double approach_object_max_dist_; - double lift_object_min_dist_; - double lift_object_max_dist_; - - // Place metrics - geometry_msgs::Pose place_pose_; - double place_surface_offset_; + const std::string task_name_; }; } // namespace tasks diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index 99b52aa9..4400ab47 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -42,390 +42,349 @@ namespace task_constructor { namespace tasks { constexpr char LOGNAME[] = "pick_place_task"; -PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh) - : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) {} +PickPlaceTask::PickPlaceTask(const std::string& task_name) + : task_name_(task_name) {} -void PickPlaceTask::loadParameters() { - /**************************************************** - * * - * Load Parameters * - * * - ***************************************************/ - ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); - ros::NodeHandle pnh("~"); +void PickPlaceTask::init(const Parameters& parameters) +{ + ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); + // Reset ROS introspection before constructing the new object + // TODO(henningkayser): verify this is a bug, fix if possible + task_.reset(); + task_.reset(new moveit::task_constructor::Task(task_name_)); + Task& t = *task_; + t.loadRobotModel(); - // Planning group properties - size_t errors = 0; - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_); + // Sampling planner + // TODO(henningkayser): Setup and parameterize alternative planners + auto sampling_planner = std::make_shared(); + sampling_planner->setProperty("goal_joint_tolerance", 1e-5); - // Predefined pose targets - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_); + // Cartesian planner + auto cartesian_planner = std::make_shared(); + cartesian_planner->setMaxVelocityScaling(1.0); + cartesian_planner->setMaxAccelerationScaling(1.0); + cartesian_planner->setStepSize(.01); - // Target object - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_); - support_surfaces_ = { surface_link_ }; + // Set task properties + t.setProperty("group", parameters.arm_group_name_); + t.setProperty("eef", parameters.eef_name_); + t.setProperty("hand", parameters.hand_group_name_); + t.setProperty("ik_frame", parameters.hand_frame_); - // Pick/Place metrics - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_); - rosparam_shortcuts::shutdownIfError(LOGNAME, errors); -} + /**************************************************** + * * + * Current State * + * * + ***************************************************/ + Stage* current_state = nullptr; // Forward current_state on to grasp pose generator + { + auto _current_state = std::make_unique("current state"); + _current_state->setTimeout(10); -void PickPlaceTask::init() { - ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); - const std::string object = "object"; + // Verify that object is not attachd + auto applicability_filter = + std::make_unique("applicability test", std::move(_current_state)); + applicability_filter->setPredicate([&](const SolutionBase& s, std::string& comment) { + s.start()->scene()->printKnownObjects(std::cout); + if (s.start()->scene()->getCurrentState().hasAttachedBody(parameters.object_name_)) + { + comment = "object with id '" + parameters.object_name_ + "' is already attached and cannot be picked"; + return false; + } + return true; + }); - // Reset ROS introspection before constructing the new object - // TODO(henningkayser): verify this is a bug, fix if possible - task_.reset(); - task_.reset(new moveit::task_constructor::Task()); + current_state = applicability_filter.get(); + t.add(std::move(applicability_filter)); + } - Task& t = *task_; - t.stages()->setName(task_name_); - t.loadRobotModel(); + /**************************************************** + * * + * Open Hand * + * * + ***************************************************/ + { // Open Hand + auto stage = std::make_unique("open hand", sampling_planner); + stage->setGroup(parameters.hand_group_name_); + stage->setGoal(parameters.hand_open_pose_); + t.add(std::move(stage)); + } - // Sampling planner - auto sampling_planner = std::make_shared(); - sampling_planner->setProperty("goal_joint_tolerance", 1e-5); + /**************************************************** + * * + * Move to Pick * + * * + ***************************************************/ + { // Move-to pre-grasp + auto stage = std::make_unique( + "move to pick", stages::Connect::GroupPlannerVector{ { parameters.arm_group_name_, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } - // Cartesian planner - auto cartesian_planner = std::make_shared(); - cartesian_planner->setMaxVelocityScaling(1.0); - cartesian_planner->setMaxAccelerationScaling(1.0); - cartesian_planner->setStepSize(.01); + /**************************************************** + * * + * Pick Object * + * * + ***************************************************/ + Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator + { + auto grasp = std::make_unique("pick object"); + t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); + grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); - // Set task properties - t.setProperty("group", arm_group_name_); - t.setProperty("eef", eef_name_); - t.setProperty("hand", hand_group_name_); - t.setProperty("hand_grasping_frame", hand_frame_); - t.setProperty("ik_frame", hand_frame_); - - /**************************************************** - * * - * Current State * - * * - ***************************************************/ - Stage* current_state = nullptr; // Forward current_state on to grasp pose generator - { - auto _current_state = std::make_unique("current state"); - - // Verify that object is not attachd - auto applicability_filter = - std::make_unique("applicability test", std::move(_current_state)); - applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) { - if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) { - comment = "object with id '" + object + "' is already attached and cannot be picked"; - return false; - } - return true; - }); - - current_state = applicability_filter.get(); - t.add(std::move(applicability_filter)); - } - - /**************************************************** - * * - * Open Hand * - * * - ***************************************************/ - { // Open Hand - auto stage = std::make_unique("open hand", sampling_planner); - stage->setGroup(hand_group_name_); - stage->setGoal(hand_open_pose_); - t.add(std::move(stage)); - } - - /**************************************************** - * * - * Move to Pick * - * * - ***************************************************/ - { // Move-to pre-grasp - auto stage = std::make_unique( - "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); - stage->setTimeout(5.0); - stage->properties().configureInitFrom(Stage::PARENT); - t.add(std::move(stage)); - } - - /**************************************************** - * * - * Pick Object * - * * - ***************************************************/ - Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator - { - auto grasp = std::make_unique("pick object"); - t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); - grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); - - /**************************************************** + /**************************************************** ---- * Approach Object * - ***************************************************/ - { - auto stage = std::make_unique("approach object", cartesian_planner); - stage->properties().set("marker_ns", "approach_object"); - stage->properties().set("link", hand_frame_); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); + ***************************************************/ + { + auto stage = std::make_unique("approach object", cartesian_planner); + stage->properties().set("marker_ns", "approach_object"); + stage->properties().set("link", parameters.hand_frame_); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(parameters.approach_object_min_dist_, parameters.approach_object_max_dist_); - // Set hand forward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; - vec.vector.z = 1.0; - stage->setDirection(vec); - grasp->insert(std::move(stage)); - } + // Set hand forward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = parameters.object_name_; + vec.vector.z = -1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } - /**************************************************** + /**************************************************** ---- * Generate Grasp Pose * - ***************************************************/ - { - // Sample grasp pose - auto stage = std::make_unique("generate grasp pose"); - stage->properties().configureInitFrom(Stage::PARENT); - stage->properties().set("marker_ns", "grasp_pose"); - stage->setPreGraspPose(hand_open_pose_); - stage->setObject(object); - stage->setAngleDelta(M_PI / 12); - stage->setMonitoredStage(current_state); // Hook into current state + ***************************************************/ + { + // Sample grasp pose + auto stage = std::make_unique("generate grasp pose"); + stage->properties().configureInitFrom(Stage::PARENT); + stage->properties().set("marker_ns", "grasp_pose"); + stage->setPreGraspPose(parameters.hand_open_pose_); + stage->setObject(parameters.object_name_); + stage->setAngleDelta(M_PI / 12); + stage->setMonitoredStage(current_state); // Hook into current state - // Compute IK - auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); - wrapper->setMaxIKSolutions(8); - wrapper->setMinSolutionDistance(1.0); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); - wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); - wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); - grasp->insert(std::move(wrapper)); - } + // Compute IK + auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(2); + wrapper->setMinSolutionDistance(1.0); + wrapper->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); + //wrapper->setIgnoreCollisions(true); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + grasp->insert(std::move(wrapper)); + } - /**************************************************** + /**************************************************** ---- * Allow Collision (hand object) * - ***************************************************/ - { - auto stage = std::make_unique("allow collision (hand,object)"); - stage->allowCollisions( - object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), - true); - grasp->insert(std::move(stage)); - } + ***************************************************/ + { + auto stage = std::make_unique("allow collision (hand,object)"); + stage->allowCollisions( + parameters.object_name_, + t.getRobotModel()->getJointModelGroup(parameters.hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), + true); + grasp->insert(std::move(stage)); + } - /**************************************************** + /**************************************************** ---- * Close Hand * - ***************************************************/ - { - auto stage = std::make_unique("close hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); - stage->setGoal(hand_close_pose_); - grasp->insert(std::move(stage)); - } + ***************************************************/ + { + auto stage = std::make_unique("close hand", sampling_planner); + stage->properties().property("group").configureInitFrom(Stage::PARENT, parameters.hand_group_name_); + stage->setGoal(parameters.hand_close_pose_); + grasp->insert(std::move(stage)); + } - /**************************************************** + /**************************************************** .... * Attach Object * - ***************************************************/ - { - auto stage = std::make_unique("attach object"); - stage->attachObject(object, hand_frame_); - attach_object_stage = stage.get(); - grasp->insert(std::move(stage)); - } + ***************************************************/ + { + auto stage = std::make_unique("attach object"); + stage->attachObject(parameters.object_name_, parameters.hand_frame_); + attach_object_stage = stage.get(); + grasp->insert(std::move(stage)); + } - /**************************************************** + /**************************************************** .... * Allow collision (object support) * - ***************************************************/ - { - auto stage = std::make_unique("allow collision (object,support)"); - stage->allowCollisions({ object }, support_surfaces_, true); - grasp->insert(std::move(stage)); - } + ***************************************************/ + { + auto stage = std::make_unique("allow collision (object,support)"); + stage->allowCollisions({ parameters.object_name_ }, "source_container", true); // TODO(henningkayser): parameterize + grasp->insert(std::move(stage)); + } - /**************************************************** + /**************************************************** .... * Lift object * - ***************************************************/ - { - auto stage = std::make_unique("lift object", cartesian_planner); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_); - stage->setIKFrame(hand_frame_); - stage->properties().set("marker_ns", "lift_object"); + ***************************************************/ + { + auto stage = std::make_unique("lift object", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(parameters.lift_object_min_dist_, parameters.lift_object_max_dist_); + stage->setIKFrame(parameters.hand_frame_); + stage->properties().set("marker_ns", "lift_object"); - // Set upward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = world_frame_; - vec.vector.z = 1.0; - stage->setDirection(vec); - grasp->insert(std::move(stage)); - } + // Set upward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = parameters.world_frame_; + vec.vector.z = 1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } - /**************************************************** + /**************************************************** .... * Forbid collision (object support) * - ***************************************************/ - { - auto stage = std::make_unique("forbid collision (object,surface)"); - stage->allowCollisions({ object }, support_surfaces_, false); - grasp->insert(std::move(stage)); - } + ***************************************************/ + { + auto stage = std::make_unique("forbid collision (object,surface)"); + stage->allowCollisions({ parameters.object_name_ }, "source_container", false); // TODO(henningkayser): parameterize + grasp->insert(std::move(stage)); + } - // Add grasp container to task - t.add(std::move(grasp)); - } + // Add grasp container to task + t.add(std::move(grasp)); + } - /****************************************************** - * * - * Move to Place * - * * - *****************************************************/ - { - auto stage = std::make_unique( - "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); - stage->setTimeout(5.0); - stage->properties().configureInitFrom(Stage::PARENT); - t.add(std::move(stage)); - } + /****************************************************** + * * + * Move to Place * + * * + *****************************************************/ + { + auto stage = std::make_unique( + "move to place", stages::Connect::GroupPlannerVector{ { parameters.arm_group_name_, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } - /****************************************************** - * * - * Place Object * - * * - *****************************************************/ - { - auto place = std::make_unique("place object"); - t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); - place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); + /****************************************************** + * * + * Place Object * + * * + *****************************************************/ + { + auto place = std::make_unique("place object"); + t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); + place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); - /****************************************************** + /****************************************************** ---- * Lower Object * - *****************************************************/ - { - auto stage = std::make_unique("lower object", cartesian_planner); - stage->properties().set("marker_ns", "lower_object"); - stage->properties().set("link", hand_frame_); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(.03, .13); + *****************************************************/ + { + auto stage = std::make_unique("lower object", cartesian_planner); + stage->properties().set("marker_ns", "lower_object"); + stage->properties().set("link", parameters.hand_frame_); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(parameters.place_object_min_dist_, parameters.place_object_max_dist_); - // Set downward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = world_frame_; - vec.vector.z = -1.0; - stage->setDirection(vec); - place->insert(std::move(stage)); - } + // Set downward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = parameters.world_frame_; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } - /****************************************************** + /****************************************************** ---- * Generate Place Pose * - *****************************************************/ - { - // Generate Place Pose - auto stage = std::make_unique("generate place pose"); - stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); - stage->properties().set("marker_ns", "place_pose"); - stage->setObject(object); + *****************************************************/ + { + // Generate Place Pose + auto stage = std::make_unique("generate place pose"); + stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); + stage->properties().set("marker_ns", "place_pose"); + stage->setObject(parameters.object_name_); + stage->setPose(parameters.place_pose_); + // Hook into attach_object_stage which allows us to use the attached object as IK frame + stage->setMonitoredStage(attach_object_stage); - // Set target pose - geometry_msgs::PoseStamped p; - p.header.frame_id = object_reference_frame_; - p.pose = place_pose_; - p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; - stage->setPose(p); - stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage + // Compute IK + auto wrapper = std::make_unique("place pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(2); + wrapper->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); + // TODO(henningkayser): Enable collisions + wrapper->setIgnoreCollisions(true); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + place->insert(std::move(wrapper)); + } - // Compute IK - auto wrapper = std::make_unique("place pose IK", std::move(stage)); - wrapper->setMaxIKSolutions(2); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); - wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); - wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); - place->insert(std::move(wrapper)); - } - - /****************************************************** + /****************************************************** ---- * Open Hand * - *****************************************************/ - { - auto stage = std::make_unique("open hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); - stage->setGoal(hand_open_pose_); - place->insert(std::move(stage)); - } + *****************************************************/ + { + auto stage = std::make_unique("open hand", sampling_planner); + stage->properties().property("group").configureInitFrom(Stage::PARENT, parameters.hand_group_name_); + stage->setGoal(parameters.hand_open_pose_); + place->insert(std::move(stage)); + } - /****************************************************** + /****************************************************** ---- * Forbid collision (hand, object) * - *****************************************************/ - { - auto stage = std::make_unique("forbid collision (hand,object)"); - stage->allowCollisions( - object_name_, - t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), false); - place->insert(std::move(stage)); - } + *****************************************************/ + { + // TODO(henningkayser): Forbid collision after retreat? + auto stage = std::make_unique("forbid collision (hand,object)"); + stage->allowCollisions( + parameters.object_name_, + t.getRobotModel()->getJointModelGroup(parameters.hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), + false); + place->insert(std::move(stage)); + } - /****************************************************** + /****************************************************** ---- * Detach Object * - *****************************************************/ - { - auto stage = std::make_unique("detach object"); - stage->detachObject(object_name_, hand_frame_); - place->insert(std::move(stage)); - } + *****************************************************/ + { + auto stage = std::make_unique("detach object"); + stage->detachObject(parameters.object_name_, parameters.hand_frame_); + place->insert(std::move(stage)); + } - /****************************************************** + /****************************************************** ---- * Retreat Motion * - *****************************************************/ - { - auto stage = std::make_unique("retreat after place", cartesian_planner); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(.12, .25); - stage->setIKFrame(hand_frame_); - stage->properties().set("marker_ns", "retreat"); - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; - vec.vector.z = -1.0; - stage->setDirection(vec); - place->insert(std::move(stage)); - } + *****************************************************/ + { + // TODO(henningkayser): Do we need this if items are dropped? + auto stage = std::make_unique("retreat after place", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(parameters.retreat_object_min_dist_, parameters.retreat_object_max_dist_); + stage->setIKFrame(parameters.hand_frame_); + stage->properties().set("marker_ns", "retreat"); + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = parameters.hand_frame_; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } - // Add place container to task - t.add(std::move(place)); - } + // Add place container to task + t.add(std::move(place)); + } - /****************************************************** - * * - * Move to Home * - * * - *****************************************************/ - { - auto stage = std::make_unique("move home", sampling_planner); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setGoal(arm_home_pose_); - stage->restrictDirection(stages::MoveTo::FORWARD); - t.add(std::move(stage)); - } + /****************************************************** + * * + * Move to Home * + * * + *****************************************************/ + { + auto stage = std::make_unique("move home", sampling_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setGoal(parameters.arm_home_pose_); + stage->restrictDirection(stages::MoveTo::FORWARD); + t.add(std::move(stage)); + } } bool PickPlaceTask::plan() { ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); - ros::NodeHandle pnh("~"); - int planning_attempts = pnh.param("planning_attempts", 10); - try { - task_->plan(planning_attempts); + task_->plan(10); // TODO: parameterize } catch (InitStageException& e) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e); return false; @@ -437,22 +396,6 @@ bool PickPlaceTask::plan() { return true; } -bool PickPlaceTask::execute() { - ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); - moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; - task_->solutions().front()->fillMessage(execute_goal.solution); - execute_.sendGoal(execute_goal); - execute_.waitForResult(); - moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code; - - if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_.getState().toString()); - return false; - } - - return true; -} - } // namespace tasks } // namespace task_constructor } // namespace moveit