Implemented MoveFromMouth actions on robot side

ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py

@@ -11,17 +11,23 @@
 
 # Third-party imports
 import py_trees
+from py_trees.blackboard import Blackboard
 from rclpy.node import Node
 
 # Local imports
+from ada_feeding.behaviors import ToggleCollisionObject
 from ada_feeding.idioms import pre_moveto_config
 from ada_feeding.trees import MoveToTree, MoveToConfigurationTree
 
 
 class MoveToConfigurationWithFTThresholdsTree(MoveToTree):
     """
     A behavior tree that moves the robot to a specified configuration, after
-    re-taring the FT sensor and setting specific FT thresholds.
+    re-taring the FT sensor and setting specific FT thresholds. This tree also
+    provides the option to specify a list of collision object IDs that should
+    be ignored during the motion (e.g., collisions between the robot and those
+    objects will be allowed before the motion begins and disallowed after the
+    motion ends or if it fails).
     """
 
     # pylint: disable=too-many-instance-attributes, too-many-arguments
@@ -49,6 +55,7 @@ def __init__(
         t_x: float = 0.0,
         t_y: float = 0.0,
         t_z: float = 0.0,
+        collision_object_ids: List[str] = [],
         keys_to_not_write_to_blackboard: Set[str] = set(),
     ):
         """
@@ -73,6 +80,10 @@ def __init__(
         t_x: The magnitude of the x component of the torque threshold. No threshold if 0.0.
         t_y: The magnitude of the y component of the torque threshold. No threshold if 0.0.
         t_z: The magnitude of the z component of the torque threshold. No threshold if 0.0.
+        collision_object_ids: The collision object IDs to ignore during the motion.
+            Collisions between the robot and these objects will be allowed in the
+            planning scene before the robot moves, and disallowed after the robot
+            stops moving or if the motion failed.
         keys_to_not_write_to_blackboard: the keys to not write to the blackboard.
             Note that the keys need to be exact e.g., "move_to.cartesian,"
             "position_goal_constraint.tolerance," "orientation_goal_constraint.tolerance,"
@@ -105,6 +116,9 @@ def __init__(
         self.t_y = t_y
         self.t_z = t_z
 
+        # Optional parameter to ignore collisions with certain objects
+        self.collision_object_ids = collision_object_ids
+
         self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard
 
     def create_move_to_tree(
@@ -128,22 +142,15 @@ def create_move_to_tree(
         -------
         tree: The behavior tree that moves the robot above the plate.
         """
-        # First, create the MoveToConfiguration behavior tree, in the same
-        # namespace as this tree
-        move_to_configuration_root = (
-            MoveToConfigurationTree(
-                joint_positions=self.joint_positions,
-                tolerance=self.tolerance_joint,
-                weight=self.weight_joint,
-                planner_id=self.planner_id,
-                allowed_planning_time=self.allowed_planning_time,
-                keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard,
-            )
-            .create_tree(name, self.action_type, tree_root_name, logger, node)
-            .root
-        )
 
-        # Add the pose path constraints
+        # pylint: disable=too-many-locals
+        # Unfortunately, many local variables are required here since this tree
+        # is exposing many different options.
+
+        children = []
+
+        # Create the re-taring and setting FT threshold behaviors that should
+        # run before the MoveTo action
         pre_moveto_behavior = pre_moveto_config(
             name=name,
             re_tare=self.re_tare,
@@ -158,13 +165,85 @@ def create_move_to_tree(
             t_z=self.t_z,
             logger=logger,
         )
+        children.append(pre_moveto_behavior)
+
+        # Create the behavior to allow collisions with certain objects
+        if len(self.collision_object_ids) > 0:
+            for collision_object_id in self.collision_object_ids:
+                allow_collision_prefix = f"allow_collision_with_{collision_object_id}"
+                allow_collision = ToggleCollisionObject(
+                    name=Blackboard.separator.join([name, allow_collision_prefix]),
+                    node=node,
+                    collision_object_id=collision_object_id,
+                    allow=True,
+                )
+                allow_collision.logger = logger
+                children.append(allow_collision)
+
+        # Create the MoveToConfiguration behavior tree, in the same
+        # namespace as this tree
+        move_to_configuration_root = (
+            MoveToConfigurationTree(
+                joint_positions=self.joint_positions,
+                tolerance=self.tolerance_joint,
+                weight=self.weight_joint,
+                planner_id=self.planner_id,
+                allowed_planning_time=self.allowed_planning_time,
+                keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard,
+            )
+            .create_tree(name, self.action_type, tree_root_name, logger, node)
+            .root
+        )
+        children.append(move_to_configuration_root)
+
+        # Create the behavior to disallow collisions with certain objects
+        disallow_collision_children = []
+        if len(self.collision_object_ids) > 0:
+            for collision_object_id in self.collision_object_ids:
+                disallow_collision_prefix = (
+                    f"disallow_collision_with_{collision_object_id}"
+                )
+                disallow_collision = ToggleCollisionObject(
+                    name=Blackboard.separator.join([name, disallow_collision_prefix]),
+                    node=node,
+                    collision_object_id=collision_object_id,
+                    allow=False,
+                )
+                disallow_collision.logger = logger
+                children.append(disallow_collision)
+                disallow_collision_children.append(disallow_collision)
 
         # Combine them in a sequence with memory
-        root = py_trees.composites.Sequence(
+        main_behavior = py_trees.composites.Sequence(
             name=name,
             memory=True,
-            children=[pre_moveto_behavior, move_to_configuration_root],
+            children=children,
         )
+        main_behavior.logger = logger
+
+        # If any part of the main behavior fails, we need to clean it up by still
+        # allowing the collision.
+        if len(self.collision_object_ids) > 0:
+            root = py_trees.composites.Selector(
+                name=name,
+                memory=True,
+                children=[
+                    main_behavior,
+                    # Even though we are cleaning up the tree, it should still
+                    # pass the failure up.
+                    py_trees.decorators.SuccessIsFailure(
+                        name + " Cleanup",
+                        py_trees.composites.Sequence(
+                            name=name,
+                            memory=True,
+                            children=disallow_collision_children,
+                        ),
+                    ),
+                ],
+            )
+            root.logger = logger
+        else:
+            root = main_behavior
 
         tree = py_trees.trees.BehaviourTree(root)
         return tree

ada_feeding/config/ada_feeding_action_servers.yaml

@@ -45,7 +45,7 @@ ada_feeding_action_servers:
         dummy_motion_time: 10.0 # secs
       tick_rate: 10 # Hz, optional, default: 30
 
-   # Parameters for the MoveToRestingPosition action
+    # Parameters for the MoveToRestingPosition action
     MoveToRestingPosition: # required
       action_type: ada_feeding_msgs.action.MoveTo # required
       tree_class: ada_feeding.trees.MoveToConfigurationWithFTThresholdsTree # required
@@ -95,6 +95,48 @@ ada_feeding_action_servers:
         allowed_planning_time_to_staging_configuration: 1.0 # secs
       tick_rate: 10 # Hz, optional, default: 30
 
+    # Parameters for the MoveFromMouthToAbovePlate action
+    MoveFromMouthToAbovePlate: # required
+      action_type: ada_feeding_msgs.action.MoveTo # required
+      tree_class: ada_feeding.trees.MoveToConfigurationWithFTThresholdsTree # required
+      tree_kws: # optional
+        - joint_positions
+        - f_mag
+        - collision_object_ids
+      tree_kwargs: # optional
+        joint_positions: # required
+          - -2.11666 # j2n6s200_joint_1
+          -  3.34967 # j2n6s200_joint_2
+          -  2.04129 # j2n6s200_joint_3
+          - -2.30031 # j2n6s200_joint_4
+          - -2.34026 # j2n6s200_joint_5
+          -  2.95450 # j2n6s200_joint_6
+        f_mag: 4.0 # N
+        collision_object_ids: # list of objects to allow collisions with during the motion
+          - wheelchair_collision
+      tick_rate: 10 # Hz, optional, default: 30
+
+    # Parameters for the MoveFromMouthToRestingPosition action
+    MoveFromMouthToRestingPosition: # required
+      action_type: ada_feeding_msgs.action.MoveTo # required
+      tree_class: ada_feeding.trees.MoveToConfigurationWithFTThresholdsTree # required
+      tree_kws: # optional
+        - joint_positions
+        - f_mag
+        - collision_object_ids
+      tree_kwargs: # optional
+        joint_positions: # required
+          - -1.94672 # j2n6s200_joint_1
+          -  2.51268 # j2n6s200_joint_2
+          -  0.35653 # j2n6s200_joint_3
+          - -4.76501 # j2n6s200_joint_4
+          -  5.99991 # j2n6s200_joint_5
+          -  4.99555 # j2n6s200_joint_6
+        f_mag: 4.0 # N
+        collision_object_ids: # list of objects to allow collisions with during the motion
+          - wheelchair_collision
+      tick_rate: 10 # Hz, optional, default: 30
+
     # Parameters for the MoveToStowLocation action
     # TODO: We may be able to get a stow location that actually
     # tucks to the side of the user's wheelchair.