Configure ada_feeding for Articulable Fork

ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py

@@ -531,18 +531,30 @@ def get_path_len(
             return total_len, joint_lens
 
         j6_i = None
+        af1_i = None
+        af2_i = None
         if exclude_j6 and "j2n6s200_joint_6" in path.joint_names:
             j6_i = path.joint_names.index("j2n6s200_joint_6")
+        if exclude_j6 and "af_joint_1" in path.joint_names:
+            af1_i = path.joint_names.index("af_joint_1")
+        if exclude_j6 and "af_joint_2" in path.joint_names:
+            af2_i = path.joint_names.index("af_joint_2")
 
         prev_pos = np.array(path.points[0].positions)
         for point in path.points:
             curr_pos = np.array(point.positions)
             seg_len = np.abs(curr_pos - prev_pos)
             if j6_i is not None:
                 j6_len = seg_len[j6_i]
+                af1_len = seg_len[af1_i]
+                af2_len = seg_len[af2_i]
                 seg_len[j6_i] = 0.0
+                seg_len[af1_i] = 0.0
+                seg_len[af2_i] = 0.0
                 total_len += np.linalg.norm(seg_len)
                 seg_len[j6_i] = j6_len
+                seg_len[af1_i] = af1_len
+                seg_len[af2_i] = af2_len
             else:
                 total_len += np.linalg.norm(seg_len)
             for index, name in enumerate(path.joint_names):

ada_feeding/ada_feeding/helpers.py

@@ -9,7 +9,7 @@
 # Standard imports
 import logging
 from threading import Lock
-from typing import Any, Optional, Set, Tuple
+from typing import Any, Optional, Set, Tuple, List
 
 # Third-party imports
 import numpy as np
@@ -324,19 +324,30 @@ def get_moveit2_object(
     Raises
     -------
     KeyError: if the MoveIt2 object does not exist and node is None.
+
+    Expects
+    -------
+    The blackboard key `/end_effector_tool` to be set.  If this key is not set,
+    the default value "fork" is used.  Other possible values are "none" and
+    "articulable_fork".  It is crucial that the `get_tool_joints` function
+    can handle these string values.  If `/end_effector_tool` is not set, a
+    warning message will be logged.
     """
     # These Blackboard keys are used to store the single, global MoveIt2 object
     # and its corresponding lock. Note that it is important that these keys start with
     # a "/" because to indicate it is an absolute path, so all behaviors can access
     # the same object.
     moveit2_blackboard_key = "/moveit2"
     moveit2_lock_blackboard_key = "/moveit2_lock"
+    end_effector_tool_blackboard_key = "/end_effector_tool"
 
     # First, register the MoveIt2 object and its corresponding lock for READ access
     if not blackboard.is_registered(moveit2_blackboard_key, Access.READ):
         blackboard.register_key(moveit2_blackboard_key, Access.READ)
     if not blackboard.is_registered(moveit2_lock_blackboard_key, Access.READ):
         blackboard.register_key(moveit2_lock_blackboard_key, Access.READ)
+    if not blackboard.is_registered(end_effector_tool_blackboard_key, Access.READ):
+        blackboard.register_key(end_effector_tool_blackboard_key, Access.READ)
 
     # Second, check if the MoveIt2 object and its corresponding lock exist on the
     # blackboard. If they do not, register the blackboard for WRITE access to those
@@ -355,14 +366,30 @@ def get_moveit2_object(
         )
         blackboard.register_key(moveit2_blackboard_key, Access.WRITE)
         blackboard.register_key(moveit2_lock_blackboard_key, Access.WRITE)
+        blackboard.register_key(end_effector_tool_blackboard_key, Access.WRITE)
+        end_effector_tool = "fork"  # Assign to default value
+        if blackboard.get(end_effector_tool_blackboard_key) is None:  # Check if not set
+            node.get_logger().warn(
+                f"end_effector_tool not set, using default: {end_effector_tool}"
+            )
+            blackboard.set(
+                end_effector_tool_blackboard_key, end_effector_tool
+            )  # Set the value on the blackboard
+        else:
+            end_effector_tool = blackboard.get(
+                end_effector_tool_blackboard_key
+            )  # Get the value from the blackboard
         # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2.
         callback_group = ReentrantCallbackGroup()
+        tool_joints = get_tool_joints(blackboard.get(end_effector_tool_blackboard_key))
+        joint_names = kinova.joint_names() + tool_joints
+        node.get_logger().info(f"Creating MoveIt2 object with: {joint_names}")
         moveit2 = MoveIt2(
             node=node,
-            joint_names=kinova.joint_names(),
+            joint_names=joint_names,
             base_link_name=kinova.base_link_name(),
             end_effector_name="forkTip",
-            group_name="jaco_arm",
+            group_name="ada",
             callback_group=callback_group,
         )
         lock = Lock()
@@ -454,3 +481,30 @@ def import_from_string(import_string: str) -> Any:
         )
     except Exception as exc:
         raise ImportError(f"Error importing {import_string}") from exc
+
+
+def get_tool_joints(end_effector_tool: str) -> List[str]:
+    """
+    Returns a list of joint names associated with the given end-effector tool.
+
+    Args:
+        end_effector_tool: The name of the end-effector tool.
+
+    Returns:
+        A list of joint names, or an empty list if no joints are associated
+        with the tool.
+
+    Raises:
+        ValueError: If an unsupported tool is provided.
+    """
+
+    tool_joints = {
+        "articulable_fork": ["af_joint_1", "af_joint_2"],
+        "none": [],
+        "fork": [],
+    }
+
+    if end_effector_tool in tool_joints:
+        return tool_joints[end_effector_tool]
+
+    raise ValueError(f"Unknown end_effector_tool: {end_effector_tool}")

ada_feeding/ada_feeding/trees/activate_controller.py

@@ -60,6 +60,8 @@ def __init__(
                 "jaco_arm_cartesian_controller",
                 "jaco_arm_controller",
                 "jaco_arm_servo_controller",
+                "af_controller",
+                "af_servo_controller",
             ]
         self.re_tare = re_tare
 

ada_feeding/ada_feeding/trees/move_from_mouth_tree.py

@@ -152,8 +152,6 @@ def __init__(
         self.staging_configuration_position = staging_configuration_position
         self.staging_configuration_quat_xyzw = staging_configuration_quat_xyzw
         self.end_configuration = end_configuration
-        if self.end_configuration is not None:
-            assert len(self.end_configuration) == 6, "Must provide 6 joint positions"
         self.staging_configuration_tolerance_position = (
             staging_configuration_tolerance_position
         )

ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py

@@ -264,9 +264,6 @@ def send_goal(self, tree: py_trees.trees.BehaviourTree, goal: object) -> bool:
             assert (
                 self.joint_positions is not None
             ), "For action MoveTo, must provide hardcoded joint_positions"
-            assert (
-                len(self.joint_positions) == 6
-            ), "For action MoveTo, must provide 6 joint positions"
 
         # Adds MoveToVisitor for Feedback
         return super().send_goal(tree, goal)

ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py

@@ -95,7 +95,6 @@ def __init__(
 
         # Store the parameters
         self.goal_configuration = goal_configuration
-        assert len(self.goal_configuration) == 6, "Must provide 6 joint positions"
         self.goal_configuration_tolerance = goal_configuration_tolerance
         self.orientation_constraint_quaternion = orientation_constraint_quaternion
         self.orientation_constraint_tolerances = orientation_constraint_tolerances

ada_feeding/config/ada_feeding_action_servers_custom.yaml

@@ -118,12 +118,12 @@ ada_feeding_action_servers:
       - -2.0289974534007733
       - -3.1847696853949206
       MoveAbovePlate.tree_kwargs.joint_positions:
-      - 6.794832881070045
-      - 3.031285852819256
-      - 4.479355460783922
-      - 7.187106922258792
-      - -1.9369287787234262
-      - -3.5496749526931417
+      - -2.4538579336877304
+      - 3.07974419938212
+      - 1.8320725365979
+      - 4.096143890468605
+      - -2.003422584820525
+      - -3.2123560395465063
       MoveFromMouth.tree_kwargs.staging_configuration_position:
       - -0.6081959669757366
       - 0.07835060871598665

ada_feeding/config/ada_feeding_action_servers_default.yaml

@@ -39,12 +39,12 @@ ada_feeding_action_servers:
           - max_velocity_scaling_factor
         tree_kwargs: # optional
           joint_positions: # required
-            - -2.3149168248766614 # j2n6s200_joint_1
-            - 3.1444595465032634 # j2n6s200_joint_2
-            - 1.7332586075115999 # j2n6s200_joint_3
-            - -2.3609596843308234 # j2n6s200_joint_4
-            - 4.43936623280362 # j2n6s200_joint_5
-            - 3.06866544924739 # j2n6s200_joint_6
+            - -2.4538579336877304 # j2n6s200_joint_1
+            - 3.07974419938212 # j2n6s200_joint_2
+            - 1.8320725365979 # j2n6s200_joint_3
+            - 4.096143890468605 # j2n6s200_joint_4
+            - -2.003422584820525 # j2n6s200_joint_5
+            - -3.2123560395465063 # j2n6s200_joint_6
           toggle_watchdog_listener: false # optional, default: true
           f_mag: 4.0 # N
           max_velocity_scaling_factor: 1.0 # optional in (0.0, 1.0], default: 0.1

ada_feeding/launch/ada_feeding_launch.xml

@@ -9,6 +9,7 @@
   <arg name="log_level" default="info" description="Log Level to pass to create_action_servers: debug, info, warn" />
   <arg name="policy" default="constant" description="Which policy to use" />
   <arg name="action" default="0" description="Which action to use with constant policy" />
+  <arg name="end_effector_tool" default="fork" description="Which end-effector tool to use"/>
 
   <group if="$(var run_web_bridge)">
     <!-- The ROSBridge Node -->
@@ -26,6 +27,7 @@
   <node pkg="ada_feeding" exec="create_action_servers.py" name="ada_feeding_action_servers" respawn="true" args="--ros-args --log-level $(var log_level) --log-level rcl:=INFO --log-level rmw_cyclonedds_cpp:=INFO">
     <param from="$(find-pkg-share ada_feeding)/config/ada_feeding_action_servers_default.yaml"/>
     <param from="$(find-pkg-share ada_feeding)/config/ada_feeding_action_servers_custom.yaml"/>
+    <param name="end_effector_tool" value="$(var end_effector_tool)"/>
     <remap from="~/watchdog" to="/ada_watchdog/watchdog" />
     <remap from="~/toggle_watchdog_listener" to="/ada_watchdog_listener/toggle_watchdog_listener" />
     <remap from="~/re_tare_ft" to="/wireless_ft/set_bias" />

ada_feeding/scripts/create_action_servers.py

@@ -20,6 +20,7 @@
 from ada_watchdog_listener import ADAWatchdogListener
 from ament_index_python.packages import get_package_share_directory
 import py_trees
+from py_trees.common import Access
 from rcl_interfaces.msg import (
     Parameter as ParameterMsg,
     ParameterDescriptor,
@@ -33,13 +34,14 @@
 from rclpy.action.server import ServerGoalHandle
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
+from rclpy.exceptions import ParameterNotDeclaredException
 from rclpy.node import Node
 from rclpy.parameter import Parameter
 import yaml
 
 # Local imports
 from ada_feeding import ActionServerBT
-from ada_feeding.helpers import import_from_string, register_logger
+from ada_feeding.helpers import import_from_string, register_logger, get_tool_joints
 from ada_feeding.visitors import DebugVisitor
 
 
@@ -115,7 +117,7 @@ def __init__(self) -> None:
         super().__init__("create_action_servers", allow_undeclared_parameters=True)
         register_logger(self.get_logger())
 
-    def initialize(self) -> None:
+    def initialize(self, blackboard: py_trees.blackboard.Client) -> None:
         """
         Initialize the node. This is a separate function from above so rclpy can
         be spinning while this function is called.
@@ -134,6 +136,26 @@ def initialize(self) -> None:
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
+        try:
+            # Get the end_effector_tool parameter
+            self.declare_parameter("end_effector_tool")  # Declaring the parameter first
+            self.end_effector_tool = self.get_parameter("end_effector_tool").value
+            self.get_logger().info(f"End effector tool: {self.end_effector_tool}")
+        except ParameterNotDeclaredException:
+            self.get_logger().warn(
+                "Parameter 'end_effector_tool' not declared. Using default value."
+            )
+            self.end_effector_tool = "fork"  # Provide a sensible default
+
+        self.tool_joints = get_tool_joints(self.end_effector_tool)
+        self.get_logger().info(f"Tool joints: {self.tool_joints}")
+
+        # Set the end_effector_tool on the global blackboard using the provided client
+        blackboard.register_key("/end_effector_tool", Access.WRITE)
+        blackboard.set(
+            "/end_effector_tool", self.end_effector_tool
+        )  # Set the global key
+
         # Read the parameters that specify what action servers to create.
         self.namespace_to_use = CreateActionServers.DEFAULT_PARAMETER_NAMESPACE
         self.action_server_params = self.read_params()
@@ -175,7 +197,7 @@ def read_params(self) -> Tuple[Parameter, Parameter, Dict[str, ActionServerParam
         -------
         action_server_params: A dict mapping server names to ActionServerParams objects.
         """
-        # pylint: disable=too-many-locals
+        # pylint: disable=too-many-locals, too-many-branches
         # Okay because we are providing a lot of generic capabilities through parameters
 
         default_namespace = CreateActionServers.DEFAULT_PARAMETER_NAMESPACE
@@ -351,11 +373,27 @@ def read_params(self) -> Tuple[Parameter, Parameter, Dict[str, ActionServerParam
                             full_name
                         ] = CreateActionServers.get_parameter_value(custom_value)
                     if self.parameters[self.namespace_to_use][full_name] is not None:
-                        tree_kwargs[kw] = self.parameters[self.namespace_to_use][
-                            full_name
-                        ]
+                        value = self.parameters[self.namespace_to_use][full_name]
                     else:
-                        tree_kwargs[kw] = self.parameters[default_namespace][full_name]
+                        value = self.parameters[default_namespace][full_name]
+
+                    if kw.endswith("joint_positions") or kw.endswith(
+                        "goal_configuration"
+                    ):
+                        if isinstance(value, list):  # Check if the value is a list
+                            zeros_to_append = [0.0] * len(self.tool_joints)
+                            value.extend(zeros_to_append)  # Append tool joints
+                            self.get_logger().info(
+                                f"Appending tool joints to {full_name}: {self.tool_joints}"
+                            )
+                            self.get_logger().info(
+                                f"Value of {full_name} after appending: {value}"
+                            )
+                        else:
+                            self.get_logger().warn(
+                                f"tree_kwarg {full_name} is not a list, cannot append tool joints"
+                            )
+                    tree_kwargs[kw] = value
 
             action_server_params[server_name] = ActionServerParams(
                 server_name=server_name,
@@ -1159,6 +1197,8 @@ def main(args: List = None) -> None:
 
     create_action_servers = CreateActionServers()
 
+    blackboard = py_trees.blackboard.Client()
+
     # Use a MultiThreadedExecutor to enable processing goals concurrently
     executor = MultiThreadedExecutor(num_threads=multiprocessing.cpu_count() * 2)
 
@@ -1175,7 +1215,7 @@ def main(args: List = None) -> None:
     # All exceptions need printing at shutdown
     try:
         # Initialize the node
-        create_action_servers.initialize()
+        create_action_servers.initialize(blackboard)
 
         # Spin in the foreground
         spin_thread.join()

ada_planning_scene/ada_planning_scene/ada_planning_scene.py

@@ -15,6 +15,7 @@
 from rcl_interfaces.msg import ParameterDescriptor, ParameterType, SetParametersResult
 from rcl_interfaces.srv import GetParameters
 import rclpy
+from rclpy.exceptions import ParameterNotDeclaredException
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.duration import Duration
 from rclpy.executors import MultiThreadedExecutor
@@ -31,6 +32,7 @@
 from ada_planning_scene.update_from_face_detection import UpdateFromFaceDetection
 from ada_planning_scene.update_from_table_detection import UpdateFromTableDetection
 from ada_planning_scene.workspace_walls import WorkspaceWalls
+from ada_feeding.helpers import get_tool_joints
 
 
 class ADAPlanningScene(Node):
@@ -53,13 +55,29 @@ def __init__(self):
         """
         super().__init__("ada_planning_scene")
 
+        try:
+            # Get the end_effector_tool parameter
+            self.declare_parameter("end_effector_tool")  # Declaring the parameter first
+            self.end_effector_tool = self.get_parameter("end_effector_tool").value
+            self.get_logger().info(f"End effector tool: {self.end_effector_tool}")
+        except ParameterNotDeclaredException:
+            self.get_logger().warn(
+                "Parameter 'end_effector_tool' not declared. Using default value."
+            )
+            self.end_effector_tool = "fork"  # Provide a sensible default
+
+        self.tool_joints = get_tool_joints(self.end_effector_tool)
+        self.get_logger().info(f"Tool joints: {self.tool_joints}")
+
         # Load the parameters. Note that each of the other classes below may
         # initialize their own parameters, all of which will be in the same
         # namespace as the parameters loaded here.
         self.__load_parameters()
 
         # Create an object to add collision objects to the planning scene
-        self.__collision_object_manager = CollisionObjectManager(node=self)
+        self.__collision_object_manager = CollisionObjectManager(
+            node=self, tool_joints=self.tool_joints
+        )
 
         # Create the initializer
         self.__initializer = PlanningSceneInitializer(
@@ -86,6 +104,7 @@ def __init__(self):
             tf_buffer=self.__tf_buffer,
             namespaces=self.__namespaces,
             namespace_to_use=self.__namespace_to_use,
+            tool_joints=self.tool_joints,
         )
 
         # Add a callback to update the namespace to use