Improve Bite Transfer (#72)

* Reverted to in-front staging location, added a cartesian motion to mouth

* MoveToMouth and MoveFromMouth works well in sim

* Add cleanup to turn watchdog listener on to MoveToRestingPosition and MoveAbovePlate

* Added ability to specific separate orientaiton constraints for staging location vs end config for MoveFromMouth

* Added velocity scaling to MoveFromMouth

* Made cartesian motions avoid collision

* Updated README.md

* Move the staging location down, adjust the planning scene based on the real table location

* Added ability to set pipeline id

* Added ability to set acceleration scaling

* Add velocity scaling to cartesian planning

* Formatting and linting

* Added more granular control for cartesian planning

* Fix cartesian velocity scaling

* Linting

* Fix the MoveFromMouth staging location to be the new lower staging location

* Added a min distance to use a detected face

* Final polishing

* Fix face detection out-of-bounds bug

* Remove unnecessary ReentrantCallbackGroup

* Make toggle collision object asynch

* Linting

ada_feeding/README.md

@@ -8,7 +8,7 @@ This code has been developed and tested with the Kinova JACO Gen2 Arm, on comput
 - Install [ROS2 Humble](https://docs.ros.org/en/humble/Installation.html)
 - Install Python dependencies: `python3 -m pip install pyyaml py_trees pymongo tornado trimesh`
 - Install the code to command the real robot ([instructions here](https://github.com/personalrobotics/ada_ros2/blob/main/README.md))
-- Git clone the [PRL fork of pymoveit (branch: `amaln/allowed_collision_matrix`)](https://github.com/personalrobotics/pymoveit2) and the [PRL fork of py_trees_ros (branch: `amaln/service_client`)](https://github.com/personalrobotics/py_trees_ros/tree/amaln/service_client) into your ROS2 workspace's `src` folder.
+- Git clone the [PRL fork of pymoveit (branch: `amaln/cartesian_allow_collision`)](https://github.com/personalrobotics/pymoveit2/tree/amaln/cartesian_avoid_collision) and the [PRL fork of py_trees_ros (branch: `amaln/service_client`)](https://github.com/personalrobotics/py_trees_ros/tree/amaln/service_client) into your ROS2 workspace's `src` folder.
 - Install additional dependencies: `sudo apt install ros-humble-py-trees-ros-interfaces`.
 - Install the web app into your workspace ([instructions here](https://github.com/personalrobotics/feeding_web_interface/tree/main/feedingwebapp)).
 
@@ -21,7 +21,7 @@ This code has been developed and tested with the Kinova JACO Gen2 Arm, on comput
 4. Launch the RealSense & Perception:
     1. Dummy nodes: `ros2 launch feeding_web_app_ros2_test feeding_web_app_dummy_nodes_launch.xml run_motion:=false run_web_bridge:=false`
     2. Real nodes: Follow the instructions in the [`ada_feeding_perception` README](https://github.com/personalrobotics/ada_feeding/blob/ros2-devel/ada_feeding_perception/README.md#usage)
-5. Run the action servers: `ros2 launch ada_feeding ada_feeding_launch.xml`
+5. Ensure the e-stop is plugged in. Run the action servers: `ros2 launch ada_feeding ada_feeding_launch.xml`
 6. Launch MoveIt2:
     1. Sim (RVIZ): `ros2 launch ada_moveit demo_feeding.launch.py sim:=mock`
     2. Real Robot: `ros2 launch ada_moveit demo_feeding.launch.py`
@@ -31,7 +31,9 @@ This code has been developed and tested with the Kinova JACO Gen2 Arm, on comput
         2. `ros2 action send_goal /AcquireFood ada_feeding_msgs/action/AcquireFood "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''}, detected_food: {roi: {x_offset: 0, y_offset: 0, height: 0, width: 0, do_rectify: false}, mask: {header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''}, format: '', data: []}, item_id: '', confidence: 0.0}}" --feedback`
         3. `ros2 action send_goal /MoveToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback`
         4. `ros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveTo "{}" --feedback`
-        5. `ros2 action send_goal /MoveToStowLocation ada_feeding_msgs/action/MoveTo "{}" --feedback`
+        5. `ros2 action send_goal /MoveFromMouthToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback`
+        6. `ros2 action send_goal /MoveFromMouthToAbovePlate ada_feeding_msgs/action/MoveTo "{}" --feedback`
+        7. `ros2 action send_goal /MoveToStowLocation ada_feeding_msgs/action/MoveTo "{}" --feedback`
     2. Test the individual actions with the web app:
         1. Launch the web app ([instructions here](https://github.com/personalrobotics/feeding_web_interface/tree/main/feedingwebapp))
         2. Go through the web app, ensure the expected actions happen on the robot.

ada_feeding/ada_feeding/behaviors/move_to.py

@@ -84,6 +84,10 @@ def __init__(
         self.move_to_blackboard.register_key(
             key="cartesian", access=py_trees.common.Access.READ
         )
+        # Add the ability to set a pipeline_id
+        self.move_to_blackboard.register_key(
+            key="pipeline_id", access=py_trees.common.Access.READ
+        )
         # Add the ability to set a planner_id
         self.move_to_blackboard.register_key(
             key="planner_id", access=py_trees.common.Access.READ
@@ -96,6 +100,24 @@ def __init__(
         self.move_to_blackboard.register_key(
             key="max_velocity_scaling_factor", access=py_trees.common.Access.READ
         )
+        # Add the ability to set acceleration scaling
+        self.move_to_blackboard.register_key(
+            key="max_acceleration_scaling_factor", access=py_trees.common.Access.READ
+        )
+        # Add the ability to set the cartesian jump threshold
+        self.move_to_blackboard.register_key(
+            key="cartesian_jump_threshold", access=py_trees.common.Access.READ
+        )
+        # Add the ability to set the cartesian max step
+        self.move_to_blackboard.register_key(
+            key="cartesian_max_step", access=py_trees.common.Access.READ
+        )
+        # Add the ability to set a cartesian fraction threshold (e.g., only
+        # accept plans that completed at least this fraction of the path)
+        self.move_to_blackboard.register_key(
+            key="cartesian_fraction_threshold", access=py_trees.common.Access.READ
+        )
+
         # Initialize the blackboard to read from the parent behavior tree
         self.tree_blackboard = self.attach_blackboard_client(
             name=name + " MoveTo", namespace=tree_name
@@ -144,6 +166,15 @@ def initialise(self) -> None:
         self.logger.info(f"{self.name} [MoveTo::initialise()]")
 
         with self.moveit2_lock:
+            # Set the planner_id
+            self.moveit2.planner_id = get_from_blackboard_with_default(
+                self.move_to_blackboard, "planner_id", "RRTstarkConfigDefault"
+            )
+            # Set the pipeline_id
+            self.moveit2.pipeline_id = get_from_blackboard_with_default(
+                self.move_to_blackboard, "pipeline_id", "ompl"
+            )
+
             # Set the planner_id
             self.moveit2.planner_id = get_from_blackboard_with_default(
                 self.move_to_blackboard, "planner_id", "RRTstarkConfigDefault"
@@ -159,6 +190,39 @@ def initialise(self) -> None:
                 self.move_to_blackboard, "allowed_planning_time", 0.5
             )
 
+            # Set the max acceleration
+            self.moveit2.max_acceleration = get_from_blackboard_with_default(
+                self.move_to_blackboard, "max_acceleration_scaling_factor", 0.1
+            )
+
+            # Set the allowed planning time
+            self.moveit2.allowed_planning_time = get_from_blackboard_with_default(
+                self.move_to_blackboard, "allowed_planning_time", 0.5
+            )
+
+        # Set the cartesian jump threshold
+        self.moveit2.cartesian_jump_threshold = get_from_blackboard_with_default(
+            self.move_to_blackboard, "cartesian_jump_threshold", 0.0
+        )
+
+        # Get whether we should use the cartesian interpolator
+        self.cartesian = get_from_blackboard_with_default(
+            self.move_to_blackboard, "cartesian", False
+        )
+
+        # Get the cartesian max step
+        self.cartesian_max_step = get_from_blackboard_with_default(
+            self.move_to_blackboard, "cartesian_max_step", 0.0025
+        )
+
+        # Get the cartesian fraction threshold
+        self.cartesian_fraction_threshold = get_from_blackboard_with_default(
+            self.move_to_blackboard, "cartesian_fraction_threshold", 0.0
+        )
+
+        # If the plan is cartesian, it should always avoid collisions
+        self.moveit2.cartesian_avoid_collisions = True
+
         # Reset local state variables
         self.prev_query_state = None
         self.planning_start_time = time.time()
@@ -173,14 +237,8 @@ def initialise(self) -> None:
         self.tree_blackboard.motion_initial_distance = 0.0
         self.tree_blackboard.motion_curr_distance = 0.0
 
-        # Get all parameters for motion, resorting to default values if unset.
-        self.joint_names = kinova.joint_names()
-        self.cartesian = get_from_blackboard_with_default(
-            self.move_to_blackboard, "cartesian", False
-        )
-
         # Set the joint names
-        self.distance_to_goal.set_joint_names(self.joint_names)
+        self.distance_to_goal.set_joint_names(kinova.joint_names())
 
     # pylint: disable=too-many-branches, too-many-return-statements
     # This is the heart of the MoveTo behavior, so the number of branches
@@ -210,7 +268,9 @@ def update(self) -> py_trees.common.Status:
                     return py_trees.common.Status.FAILURE
                 # Initiate an asynchronous planning call
                 with self.moveit2_lock:
-                    planning_future = self.moveit2.plan_async(cartesian=self.cartesian)
+                    planning_future = self.moveit2.plan_async(
+                        cartesian=self.cartesian, max_step=self.cartesian_max_step
+                    )
                 if planning_future is None:
                     self.logger.error(
                         f"{self.name} [MoveTo::update()] Failed to initiate planning!"
@@ -228,7 +288,9 @@ def update(self) -> py_trees.common.Status:
                 # Get the trajectory
                 with self.moveit2_lock:
                     traj = self.moveit2.get_trajectory(
-                        self.planning_future, cartesian=self.cartesian
+                        self.planning_future,
+                        cartesian=self.cartesian,
+                        cartesian_fraction_threshold=self.cartesian_fraction_threshold,
                     )
                 self.logger.info(f"Trajectory: {traj} | type {type(traj)}")
                 if traj is None:
@@ -237,6 +299,11 @@ def update(self) -> py_trees.common.Status:
                     )
                     return py_trees.common.Status.FAILURE
 
+                # MoveIt's default cartesian interpolator doesn't respect velocity
+                # scaling, so we need to manually add that.
+                if self.cartesian and self.moveit2.max_velocity > 0.0:
+                    MoveTo.scale_velocity(traj, self.moveit2.max_velocity)
+
                 # Set the trajectory's initial distance to goal
                 self.tree_blackboard.motion_initial_distance = (
                     self.distance_to_goal.set_trajectory(traj)
@@ -350,6 +417,40 @@ def shutdown(self) -> None:
         """
         self.logger.info(f"{self.name} [MoveTo::shutdown()]")
 
+    @staticmethod
+    def scale_velocity(traj: JointTrajectory, scale_factor: float) -> None:
+        """
+        Scale the velocity of the trajectory by the given factor. The resulting
+        trajectory should execute the same trajectory with the same continuity,
+        but just take 1/scale_factor as long to execute.
+
+        This function keeps positions the same and scales time, velocities, and
+        accelerations. It does not modify effort.
+
+        Parameters
+        ----------
+        traj: The trajectory to scale.
+        scale_factor: The factor to scale the velocity by, in [0, 1].
+        """
+        for point in traj.points:
+            # Scale time_from_start
+            nsec = point.time_from_start.sec * 10.0**9
+            nsec += point.time_from_start.nanosec
+            nsec /= scale_factor  # scale time
+            sec = int(math.floor(nsec / 10.0**9))
+            point.time_from_start.sec = sec
+            point.time_from_start.nanosec = int(nsec - sec * 10.0**9)
+
+            # Scale the velocities
+            # pylint: disable=consider-using-enumerate
+            # Necessary because we want to destructively modify the trajectory
+            for i in range(len(point.velocities)):
+                point.velocities[i] *= scale_factor
+
+            # Scale the accelerations
+            for i in range(len(point.accelerations)):
+                point.accelerations[i] *= scale_factor**2
+
 
 class DistanceToGoal:
     """

ada_feeding/ada_feeding/behaviors/toggle_collision_object.py

@@ -23,6 +23,9 @@ class ToggleCollisionObject(py_trees.behaviour.Behaviour):
     scene.
     """
 
+    # pylint: disable=too-many-instance-attributes
+    # One over is fine.
+
     def __init__(
         self,
         name: str,
@@ -60,6 +63,18 @@ def __init__(
             self.node,
         )
 
+    # pylint: disable=attribute-defined-outside-init
+    # For attributes that are only used during the execution of the tree
+    # and get reset before the next execution, it is reasonable to define
+    # them in `initialise`.
+    def initialise(self) -> None:
+        """
+        Reset the service_future.
+        """
+        self.logger.info(f"{self.name} [ToggleCollisionObject::initialise()]")
+
+        self.service_future = None
+
     def update(self) -> py_trees.common.Status:
         """
         (Dis)allow collisions between the robot and a collision
@@ -71,10 +86,24 @@ def update(self) -> py_trees.common.Status:
         """
         self.logger.info(f"{self.name} [ToggleCollisionObject::update()]")
         # (Dis)allow collisions between the robot and the collision object
-        with self.moveit2_lock:
-            succ = self.moveit2.allow_collisions(self.collision_object_id, self.allow)
+        if self.service_future is None:
+            with self.moveit2_lock:
+                service_future = self.moveit2.allow_collisions(
+                    self.collision_object_id, self.allow
+                )
+            if service_future is None:  # service not available
+                return py_trees.common.Status.FAILURE
+            self.service_future = service_future
+            return py_trees.common.Status.RUNNING
+
+        # Check if the service future is done
+        if self.service_future.done():
+            with self.moveit2_lock:
+                succ = self.moveit2.process_allow_collision_future(self.service_future)
+            # Return success/failure
+            if succ:
+                return py_trees.common.Status.SUCCESS
+            return py_trees.common.Status.FAILURE
 
-        # Return success
-        if succ:
-            return py_trees.common.Status.SUCCESS
-        return py_trees.common.Status.FAILURE
+        # Return running
+        return py_trees.common.Status.RUNNING