Lrainb/articutool spoon resting

.gitignore

@@ -66,3 +66,35 @@ trees/debug.xml
 ada_feeding_perception/test/food_img/
 ada_feeding_perception/test/output/
 ada_feeding_action_select/data
+
+# Debug/scratch tree
+trees/debug.xml
+
+# ===============================================================
+# Python Artifacts (Consolidated and Added)
+# ===============================================================
+__pycache__/
+*.pyc
+.ada_venv/
+venv/
+.venv/
+env/
+
+# ===============================================================
+# Project-Specific Data, Plots, and Benchmark Outputs (Added)
+# ===============================================================
+# Ignore generated data, plots, and saved trajectories
+ada_feeding/data/
+
+# Ignore all benchmark and analysis output directories
+ada_feeding/scripts/benchmark_analysis_plots/
+ada_feeding/scripts/e2e_benchmark_output/
+ada_feeding/scripts/holistic_benchmark_output/
+ada_feeding/scripts/manifold_data/
+ada_feeding/scripts/visualization_output/
+
+# Ignore specific generated files in the scripts directory
+ada_feeding/scripts/camera_poses.json
+
+# Ignore screenshots directory at the root
+screenshots/

ada_feeding/CMakeLists.txt

@@ -37,6 +37,9 @@ install(PROGRAMS
   scripts/dummy_ft_sensor.py
   scripts/joint_state_latency.py
   scripts/planner_benchmark.py
+  scripts/planner_benchmark_analysis.py
+  scripts/orientation_constrained_planner_benchmark.py
+  scripts/visualize_trajectory.py
   scripts/robot_state_service.py
   scripts/save_image.py
   DESTINATION lib/${PROJECT_NAME}

ada_feeding/ada_feeding/behaviors/acquisition/__init__.py

@@ -10,3 +10,7 @@
     ComputeActionConstraints,
     ComputeActionTwist,
 )
+from .rotate_local_approach_poses import (
+    RotateLocalApproachPoses,
+)
+from .conditionally_rotate_food_frame import ConditionallyRotateFoodFrame

ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py

@@ -5,12 +5,12 @@
 """
 This module defines behaviors that take an AcquisitionSchema.msg object
 (or return from AcquisitionSelect.srv) and computes the outputs
-needed to send MoveIt2Plan calls.
+needed to send MoveIt2Plan calls and execute primitive actions.
 """
 
 # Standard imports
 from copy import deepcopy
-from typing import Union, Optional
+from typing import Union, Optional, List
 
 # Third-party imports
 from geometry_msgs.msg import (
@@ -45,7 +45,7 @@ class ComputeActionConstraints(BlackboardBehavior):
     """
     Checks AcquisitionSelect response, implements stochastic
     policy choice, and then decomposes into individual
-    BlackboardKey objects for MoveIt2 Behaviors.
+    BlackboardKey objects for MoveIt2 Behaviors and Primitive Actions.
 
     Also sets static TF from food -> approach frame
     """
@@ -93,6 +93,11 @@ def blackboard_outputs(
         ext_thresh: Optional[BlackboardKey],  # SetParameters.Request
         action: Optional[BlackboardKey],  # AcquisitionSchema.msg
         action_index: Optional[BlackboardKey],  # int
+        post_move_into_primitive_name: Optional[BlackboardKey],
+        post_move_into_primitive_params: Optional[BlackboardKey],
+        post_acquisition_primitive_name: Optional[BlackboardKey],
+        post_acquisition_primitive_params: Optional[BlackboardKey],
+        should_align_to_base: Optional[BlackboardKey],
     ) -> None:
         """
         Blackboard Outputs
@@ -106,6 +111,10 @@ def blackboard_outputs(
         grasp_thresh: SetParameters request to set thresholds in grasp
         ext_thresh: SetParameters request to set thresholds for extraction
         action: AcquisitionSchema object to use in later computations
+        post_move_into_primitive_name: The name of the primitive to run after MoveInto.
+        post_move_into_primitive_params: The parameters for that primitive.
+        post_acquisition_primitive_name: The name of the primitive to run after acquisition.
+        post_acquisition_primitive_params: The parameters for that primitive.
         """
         # pylint: disable=unused-argument, duplicate-code
         # Arguments are handled generically in base class.
@@ -206,7 +215,41 @@ def update(self) -> py_trees.common.Status:
                 create_ft_thresh_request(action.ext_force, action.ext_torque),
             )
 
-            ### Final write to Blackboard
+            # Post-Move-Into Primitive
+            # post_move_into_name = (
+            #     action.post_move_into_primitive_name or "NONE"
+            # )  # Default to NONE
+            post_move_into_name = action.post_move_into_primitive_name
+            post_move_into_params = list(action.post_move_into_primitive_params)
+            self.blackboard_set("post_move_into_primitive_name", post_move_into_name)
+            self.blackboard_set(
+                "post_move_into_primitive_params", post_move_into_params
+            )
+            self.logger.info(
+                f"[{self.name}] Set Post-MoveInto Primitive to: '{post_move_into_name}' with params: {post_move_into_params}"
+            )
+
+            # Post-Acquisition Primitive
+            # post_acquisition_name = (
+            #     action.post_acquisition_primitive_name or "NONE"
+            # )  # Default to NONE
+            post_acquisition_name = action.post_acquisition_primitive_name
+            post_acquisition_params = list(action.post_acquisition_primitive_params)
+            self.blackboard_set(
+                "post_acquisition_primitive_name", post_acquisition_name
+            )
+            self.blackboard_set(
+                "post_acquisition_primitive_params", post_acquisition_params
+            )
+            self.logger.info(
+                f"[{self.name}] Set Post-Acquisition Primitive to: '{post_acquisition_name}' with params: {post_acquisition_params}"
+            )
+
+            self.blackboard_set("should_align_to_base", action.align_to_robot_base)
+            self.logger.info(
+                f"[{self.name}] Set 'should_align_to_base' flag to: {action.align_to_robot_base}"
+            )
+
             self.blackboard_set("action", action)
             self.blackboard_set("action_index", index)
         return py_trees.common.Status.SUCCESS
@@ -232,6 +275,7 @@ def blackboard_inputs(
         action: Union[BlackboardKey, AcquisitionSchema],
         is_grasp: Union[BlackboardKey, bool] = True,
         approach_frame_id: Union[BlackboardKey, str] = "approach",
+        group_name: Union[BlackboardKey, str] = "jaco_arm_with_articutool",
     ) -> None:
         """
         Blackboard Inputs
@@ -241,6 +285,7 @@ def blackboard_inputs(
         action: AcquisitionSchema msg object
         is_grasp: if true, use the grasp action elements, else use extraction
         approach_frame_id: approach frame defined in AcquisitionSchema.msg
+        group_name: The name of the MoveIt2 planning group
         """
         # pylint: disable=unused-argument, duplicate-code
         # Arguments are handled generically in base class.
@@ -279,6 +324,9 @@ def setup(self, **kwargs):
         # Get Node from Kwargs
         self.node = kwargs["node"]
 
+        # Get group name from blackboard
+        self.group_name = self.blackboard_get("group_name")
+
         # Get TF Listener from blackboard
         # For transform approach -> end_effector_frame
         self.tf_buffer, _, self.tf_lock = get_tf_object(self.blackboard, self.node)
@@ -288,6 +336,7 @@ def setup(self, **kwargs):
         self.moveit2, self.moveit2_lock = get_moveit2_object(
             self.blackboard,
             self.node,
+            self.group_name,
         )
 
     @override

ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py

@@ -4,14 +4,23 @@
 
 """
 This module defines the ComputeFoodFrame behavior, which computes the
-food frame from the Mask provided from a perception algorithm.
+food frame from the Mask provided from a perception algorithm, with an
+option to align it to the robot's base.
 """
 # Standard imports
+import math
 from typing import Optional, Tuple, Union
 
 # Third-party imports
 import cv2 as cv
-from geometry_msgs.msg import PointStamped, TransformStamped, Vector3Stamped
+from geometry_msgs.msg import (
+    PointStamped,
+    TransformStamped,
+    Vector3Stamped,
+    Pose,
+    Quaternion,
+    Vector3,
+)
 import numpy as np
 import numpy.typing as npt
 from overrides import override
@@ -20,6 +29,8 @@
 import rclpy
 from sensor_msgs.msg import CameraInfo
 import tf2_ros
+from scipy.spatial.transform import Rotation as R
+import ros2_numpy
 
 # Local imports
 from ada_feeding_msgs.msg import Mask
@@ -36,8 +47,10 @@
 
 class ComputeFoodFrame(BlackboardBehavior):
     """
-    Computes the food reference frame.
-    See definition in AcquisitionSchema.msg
+    Computes the food reference frame from a perception mask.
+    Optionally rotates this frame around its Z-axis to align its
+    local X-axis with the vector from the robot base to the food origin.
+    See definition in AcquisitionSchema.msg.
     """
 
     # pylint: disable=arguments-differ
@@ -48,6 +61,7 @@ class ComputeFoodFrame(BlackboardBehavior):
     # pylint: disable=too-many-arguments
     # These are effectively config definitions
     # They require a lot of arguments.
+    EPSILON = 1e-6
 
     def blackboard_inputs(
         self,
@@ -56,6 +70,8 @@ def blackboard_inputs(
         timestamp: Union[BlackboardKey, rclpy.time.Time] = rclpy.time.Time(),
         food_frame_id: Union[BlackboardKey, str] = "food",
         world_frame: Union[BlackboardKey, str] = "world",
+        robot_base_frame_id: Union[BlackboardKey, str] = "j2n6s200_link_base",
+        align_to_robot_base: Union[BlackboardKey, bool] = True,
         flip_food_frame: Union[BlackboardKey, bool] = False,
     ) -> None:
         """
@@ -70,6 +86,9 @@ def blackboard_inputs(
         food_frame_id (string): If len>0, TF frame to publish static transform
                                    (relative to world_frame)
         world_frame (string): ID of the TF frame to represent the food frame in
+        robot_base_frame_id (string): ID of the robot's base frame for alignment.
+        align_to_robot_base (bool): If true, rotates the computed food_frame
+                                    around its Z-axis to align with the robot.
         flip_food_frame (bool): whether to rotate the food frame 180 about Z
         """
         # pylint: disable=unused-argument, duplicate-code
@@ -217,6 +236,10 @@ def get_mask_center(
 
         return (mask.roi.x_offset + c_x, mask.roi.y_offset + c_y, median_depth)
 
+    def _normalize_angle(self, angle: float) -> float:
+        """Normalize an angle to [-pi, pi]."""
+        return (angle + math.pi) % (2 * math.pi) - math.pi
+
     @override
     def update(self) -> py_trees.common.Status:
         # Docstring copied from @override
@@ -327,15 +350,71 @@ def update(self) -> py_trees.common.Status:
         # Project to world x-y plane
         x_pos.vector.z = 0.0
 
+        world_food_initial_quat_xyzw = quat_between_vectors(
+            Vector3(x=1.0), x_pos.vector
+        )
+
+        R_world_food_initial = R.from_quat(
+            [
+                world_food_initial_quat_xyzw.x,
+                world_food_initial_quat_xyzw.y,
+                world_food_initial_quat_xyzw.z,
+                world_food_initial_quat_xyzw.w,
+            ]
+        )
+
+        # Conditionally rotate the food frame to align with the robot base
+        R_correction_z = R.identity()  # Start with no correction
+        if self.blackboard_get("align_to_robot_base"):
+            P_food_in_rb = ros2_numpy.numpify(center.point)
+            L_xy_rb_numpy = np.array([P_food_in_rb[0], P_food_in_rb[1]])
+
+            if np.linalg.norm(L_xy_rb_numpy) < self.EPSILON:
+                self.logger.info(
+                    f"[{self.name}] Food origin too close to robot base. No alignment rotation applied."
+                )
+            else:
+                desired_global_approach_yaw_rb = math.atan2(
+                    L_xy_rb_numpy[1], L_xy_rb_numpy[0]
+                )
+
+                X_axis_in_food_frame = np.array([1.0, 0.0, 0.0])
+                X_axis_in_rb_frame = R_world_food_initial.apply(X_axis_in_food_frame)
+                X_axis_xy_in_rb_frame = np.array(
+                    [X_axis_in_rb_frame[0], X_axis_in_rb_frame[1]]
+                )
+
+                if np.linalg.norm(X_axis_xy_in_rb_frame) < self.EPSILON:
+                    self.logger.warn(
+                        f"[{self.name}] Food frame X-axis has no projection in robot base XY plane. Cannot align."
+                    )
+                else:
+                    current_food_x_yaw_rb = math.atan2(
+                        X_axis_xy_in_rb_frame[1], X_axis_xy_in_rb_frame[0]
+                    )
+                    delta_psi_food_z = self._normalize_angle(
+                        desired_global_approach_yaw_rb - current_food_x_yaw_rb
+                    )
+
+                    self.logger.info(
+                        f"[{self.name}] Applying alignment rotation of {math.degrees(delta_psi_food_z):.1f} deg to food frame."
+                    )
+                    R_correction_z = R.from_euler("z", delta_psi_food_z)
+
+        # --- Final Transform Calculation ---
+        R_world_food_final = R_world_food_initial * R_correction_z
+
         # Convert to TransformStamped
         world_to_food_transform.transform.translation.x = center.point.x
         world_to_food_transform.transform.translation.y = center.point.y
         world_to_food_transform.transform.translation.z = center.point.z
 
-        x_unit = Vector3Stamped()
-        x_unit.vector.x = 1.0
-        world_to_food_transform.transform.rotation = quat_between_vectors(
-            x_unit.vector, x_pos.vector
+        final_quat_xyzw = R_world_food_final.as_quat()
+        world_to_food_transform.transform.rotation = Quaternion(
+            x=final_quat_xyzw[0],
+            y=final_quat_xyzw[1],
+            z=final_quat_xyzw[2],
+            w=final_quat_xyzw[3],
         )
 
         # # If you need to send a fixed food frame to the robot arm, e.g., to