Finalize learning capabilities (not including posthoc) (#195)

* Enabled calling all different policy types, added policy flag to start script

* Fixed spacing error in dummy launch

* Fixed linalg mispelling bug

* Commit local changes on lovelace

* Added full acquisition library, switched to SPANet context

* Cleaned up code, added angled extraction, removed unwanted actions

---------

Co-authored-by: Taylor <taylorkf@tiktok.personalrobotics.cs.washington.edu>
Co-authored-by: Taylor Kessler Faulkner <taylorkf@cs.washington.edu>

Author: 3 people

ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py

@@ -63,6 +63,7 @@ def blackboard_inputs(
         move_above_dist_m: Union[BlackboardKey, float] = 0.05,
         food_frame_id: Union[BlackboardKey, str] = "food",
         approach_frame_id: Union[BlackboardKey, str] = "approach",
+        action: Union[BlackboardKey, Optional[AcquisitionSchema]] = None,
     ) -> None:
         """
         Blackboard Inputs
@@ -73,6 +74,7 @@ def blackboard_inputs(
         move_above_dist_m: how far from the food to start
         food_frame_id: food frame defined in AcquisitionSchema.msg
         approach_frame_id: approach frame defined in AcquisitionSchema.msg
+        action: which action has been chosen in the initial pi-symmetry break
         """
         # pylint: disable=unused-argument, duplicate-code
         # Arguments are handled generically in base class.
@@ -141,64 +143,67 @@ def update(self) -> py_trees.common.Status:
             self.logger.error(f"Malformed action select response: {response}")
             return py_trees.common.Status.FAILURE
 
-        # Sample Action
-        index = np.random.choice(np.arange(prob.size), p=prob)
-        action = response.actions[index]
-
-        ### Construct Constraints
-
-        # Re-scale pre-transform to move_above_dist_m
-        position = ros2_numpy.numpify(action.pre_transform.position)
-        if np.isclose(np.linalg.norm(position), 0.0):
-            self.logger.error(
-                f"Malformed action pre_transform: {action.pre_transform.position}"
+        action_set = self.blackboard_get("action")
+        if action_set is None:
+            # Sample Action
+            index = np.random.choice(np.arange(prob.size), p=prob)
+            action = response.actions[index]
+            self.logger.info(f"Chosen action index: {index}")
+
+            ### Construct Constraints
+
+            # Re-scale pre-transform to move_above_dist_m
+            position = ros2_numpy.numpify(action.pre_transform.position)
+            if np.isclose(np.linalg.norm(position), 0.0):
+                self.logger.error(
+                    f"Malformed action pre_transform: {action.pre_transform.position}"
+                )
+                return py_trees.common.Status.FAILURE
+            position = (
+                position
+                * self.blackboard_get("move_above_dist_m")
+                / np.linalg.norm(position)
+            )
+            action.pre_transform.position = ros2_numpy.msgify(Point, position)
+            self.blackboard_set("move_above_pose", action.pre_transform)
+
+            # Calculate Approach Target (in food frame)
+            move_into_pose = Pose()
+            move_into_pose.orientation = deepcopy(action.pre_transform.orientation)
+            offset = ros2_numpy.numpify(action.pre_offset)
+            move_into_pose.position = ros2_numpy.msgify(Point, offset)
+            self.blackboard_set("move_into_pose", move_into_pose)
+
+            ### Calculate Approach Frame
+            approach_vec = offset - position
+            approach_frame = TransformStamped()
+            approach_mat = np.eye(4)
+            if not np.all(np.isclose(approach_vec[:2], np.zeros(2))):
+                approach_mat[:3, :3] = R.from_rotvec(
+                    np.arctan2(approach_vec[1], approach_vec[0]) * np.array([0, 0, 1])
+                ).as_matrix()
+            approach_frame.transform = ros2_numpy.msgify(Transform, approach_mat)
+            approach_frame.header.stamp = self.node.get_clock().now().to_msg()
+            approach_frame.header.frame_id = "food"
+            approach_frame.child_frame_id = "approach"
+            set_static_tf(approach_frame, self.blackboard, self.node)
+
+            ### Calculate F/T Thresholds
+            self.blackboard_set(
+                "approach_thresh",
+                create_ft_thresh_request(action.pre_force, action.pre_torque),
+            )
+            self.blackboard_set(
+                "grasp_thresh",
+                create_ft_thresh_request(action.grasp_force, action.grasp_torque),
+            )
+            self.blackboard_set(
+                "ext_thresh",
+                create_ft_thresh_request(action.ext_force, action.ext_torque),
             )
-            return py_trees.common.Status.FAILURE
-        position = (
-            position
-            * self.blackboard_get("move_above_dist_m")
-            / np.linalg.norm(position)
-        )
-        action.pre_transform.position = ros2_numpy.msgify(Point, position)
-        self.blackboard_set("move_above_pose", action.pre_transform)
-
-        # Calculate Approach Target (in food frame)
-        move_into_pose = Pose()
-        move_into_pose.orientation = deepcopy(action.pre_transform.orientation)
-        offset = ros2_numpy.numpify(action.pre_offset)
-        move_into_pose.position = ros2_numpy.msgify(Point, offset)
-        self.blackboard_set("move_into_pose", move_into_pose)
-
-        ### Calculate Approach Frame
-        approach_vec = offset - position
-        approach_frame = TransformStamped()
-        approach_mat = np.eye(4)
-        if not np.all(np.isclose(approach_vec[:2], np.zeros(2))):
-            approach_mat[:3, :3] = R.from_rotvec(
-                np.arctan2(approach_vec[1], approach_vec[0]) * np.array([0, 0, 1])
-            ).as_matrix()
-        approach_frame.transform = ros2_numpy.msgify(Transform, approach_mat)
-        approach_frame.header.stamp = self.node.get_clock().now().to_msg()
-        approach_frame.header.frame_id = "food"
-        approach_frame.child_frame_id = "approach"
-        set_static_tf(approach_frame, self.blackboard, self.node)
-
-        ### Calculate F/T Thresholds
-        self.blackboard_set(
-            "approach_thresh",
-            create_ft_thresh_request(action.pre_force, action.pre_torque),
-        )
-        self.blackboard_set(
-            "grasp_thresh",
-            create_ft_thresh_request(action.grasp_force, action.grasp_torque),
-        )
-        self.blackboard_set(
-            "ext_thresh",
-            create_ft_thresh_request(action.ext_force, action.ext_torque),
-        )
 
-        ### Final write to Blackboard
-        self.blackboard_set("action", action)
+            ### Final write to Blackboard
+            self.blackboard_set("action", action)
         return py_trees.common.Status.SUCCESS
 
 

ada_feeding/ada_feeding/trees/acquire_food_tree.py

@@ -323,7 +323,7 @@ def create_tree(
         )  # End RecoverySequence
 
         def move_above_plan(
-            flip_food_frame: bool = False,
+            flip_food_frame: bool = False, action: Optional[BlackboardKey] = None, 
         ) -> py_trees.behaviour.Behaviour:
             return py_trees.composites.Sequence(
                 name="MoveAbovePlanningSeq",
@@ -380,7 +380,7 @@ def move_above_plan(
                             inputs={
                                 "action_select_response": BlackboardKey(
                                     "action_response"
-                                ),
+                                ), "action": action,
                                 # Default move_above_dist_m = 0.05
                                 # Default food_frame_id = "food"
                                 # Default approach_frame_id = "approach"
@@ -523,7 +523,7 @@ def move_above_plan(
                                     memory=True,
                                     children=[
                                         move_above_plan(False),
-                                        move_above_plan(True),
+                                        move_above_plan(True, BlackboardKey("action")),
                                     ],
                                 ),
                                 MoveIt2Execute(

ada_feeding/launch/ada_feeding_launch.xml

@@ -18,7 +18,7 @@
   </node>
 
   <!-- Launch the action servers necessary to move the robot -->
-  <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_fastrtps_cpp:=INFO">
+  <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"/>
     <remap from="~/watchdog" to="/ada_watchdog/watchdog" />

ada_feeding_action_select/ada_feeding_action_select/policies/__init__.py

@@ -11,6 +11,9 @@
 
 # Linear Policies
 from .linear_policies import RandomLinearPolicy
+from .linear_policies import GreedyLinearPolicy
+from .linear_policies import EpsilonGreedyLinearPolicy
+from .linear_policies import LinUCBPolicy
 
 # Color Policy
 from .color_policy import ColorPolicy

ada_feeding_action_select/ada_feeding_action_select/policies/linear_policies.py

@@ -207,7 +207,7 @@ def _solve(self, arm=None):
             solve_out = self.checkpoint.linear_model[arm, :]
 
         # Solve for linear policy vector
-        solve_out[:] = np.linalge.solve(solve_a, solve_b)
+        solve_out[:] = np.linalg.solve(solve_a, solve_b)
 
     @override
     def get_checkpoint(self) -> Any:

ada_feeding_action_select/ada_feeding_action_select/policy_service.py

@@ -349,6 +349,7 @@ def select_callback(
             self.get_logger().info(
                 f"AcquisitionSelect Success! Sending Response with ID: '{response.id}'"
             )
+
         return response
 
     def report_callback(
@@ -382,6 +383,8 @@ def report_callback(
             select.actions[request.action_index],
         )
 
+        self.get_logger().info(f"Executed Action: '{request.action_index}'")
+
         # Run the posthoc adapter
         posthoc = self.posthoc_adapter.get_posthoc(np.array(request.posthoc))
         if posthoc.size != self.posthoc_adapter.dim: