From cbd9b0c0eaf5ff756206c8a1b5ce8506a5e13e1f Mon Sep 17 00:00:00 2001 From: Mike Tobia Date: Tue, 9 Dec 2025 14:50:55 -0600 Subject: [PATCH 1/6] updated init and approach_bench_seat waypoints, need to work on traverse poses still --- clr_pick_and_place_demo/config/waypoints.yaml | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/clr_pick_and_place_demo/config/waypoints.yaml b/clr_pick_and_place_demo/config/waypoints.yaml index bb17132..42802ae 100644 --- a/clr_pick_and_place_demo/config/waypoints.yaml +++ b/clr_pick_and_place_demo/config/waypoints.yaml @@ -3,23 +3,23 @@ waypoints: 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" @@ -37,12 +37,12 @@ waypoints: relative: true stow: config: - - -2.11185 - - -2.6529 - - 2.44346 + - -2.11185 # -121 deg + - -2.6529 # -152 deg + - 2.44346 # 140 deg - 0.0 - - 1.0821 - - 0.1222 + - 1.0821 # 62 deg + - 0.1222 # 7 deg planning_group: "ur_manipulator" cartesian: false planner: "RRTstarkConfigDefault" From 5dcb00e080043e037610efd1133c7f1b30629766 Mon Sep 17 00:00:00 2001 From: Mike Tobia Date: Tue, 16 Dec 2025 12:56:33 -0600 Subject: [PATCH 2/6] Updating demo waypoints --- clr_pick_and_place_demo/config/waypoints.yaml | 19 +++++++++++++++- clr_pick_and_place_demo/src/run_demo.cpp | 22 +++++++++++++++++++ 2 files changed, 40 insertions(+), 1 deletion(-) diff --git a/clr_pick_and_place_demo/config/waypoints.yaml b/clr_pick_and_place_demo/config/waypoints.yaml index 42802ae..c19f630 100644 --- a/clr_pick_and_place_demo/config/waypoints.yaml +++ b/clr_pick_and_place_demo/config/waypoints.yaml @@ -1,4 +1,15 @@ 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 @@ -35,9 +46,15 @@ waypoints: planning_group: "ur_manipulator" cartesian: true relative: true + approach_stow: + config: + - 0.500 + planning_group: "rail" + cartesian: false + stow: config: - - -2.11185 # -121 deg + - 4.17133 # 238 deg - -2.6529 # -152 deg - 2.44346 # 140 deg - 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..95485b0 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."); @@ -285,6 +295,12 @@ class RunDemoNode : public rclcpp::Node 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 +366,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."); From 741228cdba97fa46032a1ab4ffb8f5df517385d4 Mon Sep 17 00:00:00 2001 From: Mike Tobia Date: Thu, 18 Dec 2025 16:06:32 -0600 Subject: [PATCH 3/6] WIP trying to stop the shoulder inversion when moving to the stow waypoint, struggling with ctb placement in bench --- clr_pick_and_place_demo/config/waypoints.yaml | 21 ++++++++++++------- clr_pick_and_place_demo/src/run_demo.cpp | 7 ++++++- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/clr_pick_and_place_demo/config/waypoints.yaml b/clr_pick_and_place_demo/config/waypoints.yaml index c19f630..cbfa6b7 100644 --- a/clr_pick_and_place_demo/config/waypoints.yaml +++ b/clr_pick_and_place_demo/config/waypoints.yaml @@ -53,19 +53,26 @@ waypoints: 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: - - 4.17133 # 238 deg - - -2.6529 # -152 deg - - 2.44346 # 140 deg - - 0.0 - - 1.0821 # 62 deg - - 0.1222 # 7 deg + - 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: diff --git a/clr_pick_and_place_demo/src/run_demo.cpp b/clr_pick_and_place_demo/src/run_demo.cpp index 95485b0..1454599 100644 --- a/clr_pick_and_place_demo/src/run_demo.cpp +++ b/clr_pick_and_place_demo/src/run_demo.cpp @@ -292,6 +292,11 @@ 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!"); } @@ -434,7 +439,7 @@ 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); - Waypoint blob_wp = Waypoint(global_pose, "chonkur_grasp", true); + Waypoint blob_wp = Waypoint(global_pose, "clr", true); return plan_and_execute(blob_wp); } From 0107cfa68baf5bfe7c3554f83bc2a6bbaf563004 Mon Sep 17 00:00:00 2001 From: Mike Tobia Date: Fri, 2 Jan 2026 16:17:17 -0600 Subject: [PATCH 4/6] sim works, need to test on hw, added mid-left-traverse waypoint to allow pre_drop_ctb space to plan, changed pre_drop_ctb to non-cartesian --- clr_pick_and_place_demo/config/waypoints.yaml | 14 +++++++---- clr_pick_and_place_demo/src/run_demo.cpp | 23 +++++++++++++++---- 2 files changed, 27 insertions(+), 10 deletions(-) diff --git a/clr_pick_and_place_demo/config/waypoints.yaml b/clr_pick_and_place_demo/config/waypoints.yaml index cbfa6b7..5e3ff24 100644 --- a/clr_pick_and_place_demo/config/waypoints.yaml +++ b/clr_pick_and_place_demo/config/waypoints.yaml @@ -51,7 +51,6 @@ waypoints: - 0.500 planning_group: "rail" cartesian: false - stow: # config: # - 4.17133 # 238 deg @@ -119,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 @@ -134,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 1454599..a25ba8c 100644 --- a/clr_pick_and_place_demo/src/run_demo.cpp +++ b/clr_pick_and_place_demo/src/run_demo.cpp @@ -262,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()) @@ -272,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."); @@ -456,10 +461,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() @@ -472,9 +477,17 @@ 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() { RCLCPP_INFO(LOGGER, "Dropping CTB."); From e90cf4d6b2d7b489cc7dfae03564fbfe5d3532a2 Mon Sep 17 00:00:00 2001 From: Mike Tobia Date: Fri, 2 Jan 2026 16:24:31 -0600 Subject: [PATCH 5/6] formatting --- clr_pick_and_place_demo/src/run_demo.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/clr_pick_and_place_demo/src/run_demo.cpp b/clr_pick_and_place_demo/src/run_demo.cpp index a25ba8c..aebd640 100644 --- a/clr_pick_and_place_demo/src/run_demo.cpp +++ b/clr_pick_and_place_demo/src/run_demo.cpp @@ -487,7 +487,6 @@ class RunDemoNode : public rclcpp::Node return plan_and_execute(wp_map.at("traverse_left_2")); } - bool drop_ctb() { RCLCPP_INFO(LOGGER, "Dropping CTB."); From eb696a6325dc83f4055aed56d06d133c3ee88100 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Fri, 9 Jan 2026 13:56:57 -0500 Subject: [PATCH 6/6] Only use chonkur for the grasp --- clr_pick_and_place_demo/src/run_demo.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/clr_pick_and_place_demo/src/run_demo.cpp b/clr_pick_and_place_demo/src/run_demo.cpp index aebd640..282d37e 100644 --- a/clr_pick_and_place_demo/src/run_demo.cpp +++ b/clr_pick_and_place_demo/src/run_demo.cpp @@ -396,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"; @@ -444,7 +444,9 @@ 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); - Waypoint blob_wp = Waypoint(global_pose, "clr", true); + // 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); }