diff --git a/clr_pick_and_place_demo/config/waypoints.yaml b/clr_pick_and_place_demo/config/waypoints.yaml index bb17132..5e3ff24 100644 --- a/clr_pick_and_place_demo/config/waypoints.yaml +++ b/clr_pick_and_place_demo/config/waypoints.yaml @@ -1,25 +1,36 @@ waypoints: + demo_home: + config: + - 3.1927 + - -2.3913 + - -1.7760 + - -0.4700 + - -1.6177 + - -1.6134 + planning_group: "ur_manipulator" + cartesian: false + planner: "RRTstarkConfigDefault" init: config: - 0.0 - 0.0 - - -3.71755 - - -3.0020 - - -0.9599 - - -0.6982 - - -1.5184 - - -1.0297 + - 2.5656 # 147 deg + - -3.0020 # -172 deg + - -0.9599 # -55 deg + - -0.6982 # -40 deg + - -1.5184 # -87 deg + - -1.0297 # -59 deg planning_group: "clr" cartesian: false planner: "RRTstarkConfigDefault" approach_bench_seat: config: - - -3.8235 - - -2.683256954185755 - - -1.5352487041273213 - - -0.44915294760331065 - - -1.5091231026904337 - - -0.9393066919906827 + - 2.46091 # 141 deg + - -2.6832 # -153.7 deg + - -1.5352 # -88 deg + - -0.4491 # -25.7 deg + - -1.5091 # -86.5 deg + - -0.9393 # -53.8 deg planning_group: "ur_manipulator" cartesian: false planner: "RRTstarkConfigDefault" @@ -35,20 +46,32 @@ waypoints: planning_group: "ur_manipulator" cartesian: true relative: true + approach_stow: + config: + - 0.500 + planning_group: "rail" + cartesian: false stow: + # config: + # - 4.17133 # 238 deg + # - -2.6529 # -152 deg + # - 2.44346 # 140 deg + # - 0.0 + # - 1.0821 # 62 deg + # - 0.1222 # 7 deg config: - - -2.11185 - - -2.6529 - - 2.44346 - - 0.0 - - 1.0821 - - 0.1222 + - 1.5010 # 86 deg + - -0.3316 # -19 deg + - -2.2515 # -129 deg + - -3.6477 # -209 deg + - -1.5533 # -89 deg + - 1.5708 # 90 deg planning_group: "ur_manipulator" cartesian: false planner: "RRTstarkConfigDefault" traverse_right: config: - - 1.748 + - 1.200 planning_group: "rail" cartesian: false ctb_offset: @@ -95,14 +118,14 @@ waypoints: planning_group: "ur_manipulator" cartesian: true relative: true - traverse_left: + traverse_left_1: config: - - 0.000 + - 0.700 planning_group: "rail" cartesian: false pre_drop_ctb: pose: - x: -1.708 + x: -1.006 y: 0.811 z: 1.580 qx: 0.725 @@ -110,7 +133,12 @@ waypoints: qz: 0.032 qw: -0.004 planning_group: "ur_manipulator" - cartesian: true + cartesian: false + traverse_left_2: + config: + - 0.000 + planning_group: "rail" + cartesian: false drop_arm: pose: x: 0.0 diff --git a/clr_pick_and_place_demo/src/run_demo.cpp b/clr_pick_and_place_demo/src/run_demo.cpp index bb6093c..282d37e 100644 --- a/clr_pick_and_place_demo/src/run_demo.cpp +++ b/clr_pick_and_place_demo/src/run_demo.cpp @@ -202,6 +202,11 @@ class RunDemoNode : public rclcpp::Node this->movegroup_visualtools_setup("ur_manipulator"); moveit_viz->trigger(); moveit_viz->prompt("Press next in RvizVisualToolsGUI to start the demo"); + if (!this->demo_home()) + { + RCLCPP_ERROR(LOGGER, "Failed to reach home pose. Exiting."); + return; + } if (!this->init()) { RCLCPP_ERROR(LOGGER, "Failed to reach initial pose. Exiting."); @@ -222,6 +227,11 @@ class RunDemoNode : public rclcpp::Node RCLCPP_ERROR(LOGGER, "Failed to back out from bench seat. Exiting."); return; } + if (!this->approach_stow()) + { + RCLCPP_ERROR(LOGGER, "Failed to approach stow waypoint. Exiting."); + return; + } if (!this->stow()) { RCLCPP_ERROR(LOGGER, "Failed to stow manipulator. Exiting."); @@ -252,9 +262,9 @@ class RunDemoNode : public rclcpp::Node RCLCPP_ERROR(LOGGER, "Failed to lift CTB. Exiting."); return; } - if (!this->traverse_left()) + if (!this->traverse_left_1()) { - RCLCPP_ERROR(LOGGER, "Failed to traverse to CTB dropoff location. Exiting."); + RCLCPP_ERROR(LOGGER, "Failed to traverse left. Exiting."); return; } if (!this->pre_drop_ctb()) @@ -262,6 +272,11 @@ class RunDemoNode : public rclcpp::Node RCLCPP_ERROR(LOGGER, "Failed to approach CTB dropoff location. Exiting."); return; } + if (!this->traverse_left_2()) + { + RCLCPP_ERROR(LOGGER, "Failed to traverse to CTB dropoff location. Exiting."); + return; + } if (!this->drop_ctb()) { RCLCPP_ERROR(LOGGER, "Failed to lower CTB. Exiting."); @@ -282,9 +297,20 @@ class RunDemoNode : public rclcpp::Node RCLCPP_ERROR(LOGGER, "Failed to stow manipulator. Exiting."); return; } + if (!this->demo_home()) + { + RCLCPP_ERROR(LOGGER, "Failed to return to home position. Exiting."); + return; + } RCLCPP_INFO(LOGGER, "Demo succeeded!"); } + bool demo_home() + { + RCLCPP_INFO(LOGGER, "Moving to home position."); + return plan_and_execute(wp_map.at("demo_home")); + } + bool init() { RCLCPP_INFO(LOGGER, "Moving to initial position."); @@ -350,6 +376,12 @@ class RunDemoNode : public rclcpp::Node return plan_and_execute(wp_map.at("back_out")); } + bool approach_stow() + { + RCLCPP_INFO(LOGGER, "Moving rail to approach stow position."); + return plan_and_execute(wp_map.at("approach_stow")); + } + bool stow() { RCLCPP_INFO(LOGGER, "Stowing manipulator."); @@ -364,7 +396,7 @@ class RunDemoNode : public rclcpp::Node bool approach_ctb_handle() { - RCLCPP_INFO(LOGGER, "Reaching for CTB handle."); + RCLCPP_INFO(LOGGER, "Detecting CTB handle."); // Find red blob in wrist camera image auto request = std::make_shared(); request->color = "red"; @@ -412,6 +444,8 @@ class RunDemoNode : public rclcpp::Node geometry_msgs::msg::Pose offset = wp_map.at("ctb_offset").pose; global_pose = this->relative_to_global(global_pose, offset); + // TODO: Consider replacing with a two phase grasp, one to align to fixed z-offset, and one to grasp. + RCLCPP_INFO(LOGGER, "Reaching for CTB handle."); Waypoint blob_wp = Waypoint(global_pose, "chonkur_grasp", true); return plan_and_execute(blob_wp); } @@ -429,10 +463,10 @@ class RunDemoNode : public rclcpp::Node plan_and_execute(wp_map.at("stow_ctb")); } - bool traverse_left() + bool traverse_left_1() { RCLCPP_INFO(LOGGER, "Traversing to bench seat location."); - return plan_and_execute(wp_map.at("traverse_left")); + return plan_and_execute(wp_map.at("traverse_left_1")); } bool pre_drop_ctb() @@ -445,7 +479,14 @@ class RunDemoNode : public rclcpp::Node } Waypoint approach_wp_1 = Waypoint(eef.transform.translation.x, eef.transform.translation.y, eef.transform.translation.z, 0.725, 0.688, 0.032, -0.004, "ur_manipulator", true); - return plan_and_execute({ approach_wp_1, wp_map.at("pre_drop_ctb") }); + // return plan_and_execute({ approach_wp_1, wp_map.at("pre_drop_ctb") }); + return plan_and_execute(approach_wp_1) && plan_and_execute(wp_map.at("pre_drop_ctb")); + } + + bool traverse_left_2() + { + RCLCPP_INFO(LOGGER, "Traversing to bench seat location."); + return plan_and_execute(wp_map.at("traverse_left_2")); } bool drop_ctb()