WIP trying to stop the shoulder inversion when moving to the stow waypoint, struggling with ctb placement in bench

Author: mtobia-nasa

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:

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);
   }