Update waypoints

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,22 +118,27 @@ 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
       qy: 0.688
       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

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,16 +262,21 @@ 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())
     {
       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.");
@@ -412,7 +444,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);
   }
 
@@ -429,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()
@@ -445,7 +477,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()