Allow users to customize the distance to mouth during bite transfer

[amalnanavati]

Describe this pull request. Link to relevant GitHub issues, if any.

Joint with ada_feeding#155.

Different users have different neck mobilities, and thus it is crucial for them to be able to customize how close the robot gets to their mouth during bite transfer. This PR accomplishes that through the following changes:

1. Re-implementing the settings menu, and removing all un-implemented settings.
2. Generalizing RobotMotion and DetectingFace to be able to be called from within the settings menu, without impacting non-settings global state.
3. Have the settings menu retrieve the current parameter from the robot (i.e., parameters are stored on the robot, not in web app cookies).
4. Allow the user to change the distance to mouth and try it out.
5. When the Done button is clicked, it executes motions to restore the robot arm to the configuration corresponding with the mealState from before the user entered the Settings menu.

In addition, this PR makes the following two changes:

1. Removes the combo MoveFromMouthToAbovePlate and MoveFromMouthToRestingPosition. Because these are two actions at once, if paused during the second action, it restarts the first, which can be unintuitive and dangerous.
   1. Instead, this PR replaces the above with first a call to the only MoveFromMouth (that goes to the staging pose), followed by a standard call to MoveAbovePlate / MoveToRestingPosition.
2. Adds a "move to staging configuration" button to the BiteDone page. This is necessary in the case where MoveToMouth succeeds but actually doesn't move close enough, so the user can go back to the staging configuration and retry.

Finally, the joint PR ada_feeding#155 addresses the issue that if MoveToMouth fails because it is too close to a singularity, MoveFromMouth will also fail, leaving the user stuck. Thus, that PR generalizes MoveFromMouth to have a fallback cartesian motion, so that it can always move back to the staging configuration.

Explain how this pull request was tested, including but not limited to the below checkmarks.

Sim

Setup:

• ros2 run ada_feeding dummy_ft_sensor.py
• ros2 launch feeding_web_app_ros2_test feeding_web_app_dummy_nodes_launch.xml run_motion:=false run_web_bridge:=true
• ros2 run ada_feeding_perception republisher --ros-args --params-file src/ada_feeding/ada_feeding_perception/config/republisher.yaml
• ros2 launch ada_feeding ada_feeding_launch.xml use_estop:=false run_web_bridge:=true
• ros2 launch ada_moveit demo.launch.py sim:=mock

Testing the MoveFromMouth change:

• Use the app as normal. On the BiteDone page, select each of the three buttons (one at a time), and verify they succeed as expected.
  • Move above plate
  • Move to resting position
  • Move to staging configuration
• Use the app as normal. On the BiteDone page, select each of the three buttons (one at a time). Then, pause and resume each action, and verify that it functions as expected.
  • Move above plate
    • Pause/resume during "move from mouth"
    • Pause/resume during "move above plate"
  • Move to resting position
    • Pause/resume during "move from mouth"
    • Pause/resume during "move to resting position"
  • Move to staging configuration
    • Pause/resume during "move from mouth"
• Use the app as normal. On the BiteDone page, select each of the top two buttons (one at a time) and let them succeed. Then go back to "Move To Mouth," pause it, and click "Back," thereby invoking "MoveFromMouth." Verify that it stops at the DetectingFace page, and doesn't move on to another motion.
  • Move above plate
  • Move to resting position
• Test the Responsiveness of the BiteDone page and modal in portrait and landscape.

Testing the Settings / Bite Transfer Menu:

• During standard usage of the app, while the robot is moving, click Settings. Verify that it doesn't go to Settings.
• During a screen where the robot is stationary, go to settings. Verify it goes to settings.
• Go to Bite Transfer settings. Change the settings with the arrow keys. Verify that the file ada_feeding/ada_feeding/configs/ada_feeding_action_servers_current.yaml gets updated accordingly.
• Same as above but change the settings with the text input and verify that it gets updated accordingly.
• Click "Move To Mouth". Verify that it stops the appropriate distance from your mouth.
• Click "Move From Mouth". Verify that it goes back.
• Change the distance. Repeat the above two and verify that it respects the new distance
• Click "Set to Default", verify that on the screen and in the yaml file it goes back to 2.5cm.
• Go back to "Home" without clicking "Done". Go up till the BiteDone screen. Then go to settings (verify it starts in the main settings screen). Go to Bite Transfer. Verify that it still moves to the Staging configuration.
• Go back to "Home" and go to an above plate meal state. Then, go into settings. click MoveToMouth. Refresh settings. Verify that it moves to the staging configuration.
• In the "BiteTransfer" menu, click "MoveToMouth". Before it finishes, pause it. Then close the modal. Then click each of the two buttons (one at a time) and verify that they don't start off in a paused state. Also verify that both run fully as expected.
  • Move To Mouth
  • Move From Mouth
• For each of the following states of the home screen, go into settings. For some of them click MoveToMouth, for others don't (see below). Then, click Done. Verify that it resets the robot to the configuration it was in before you went to settings.
  • U_PreMeal (in settings, click MoveToMouth)
  • U_BiteSelection
  • U_BiteAcquisitionCheck (in settings, click MoveToMouth)
  • U_BiteDone (should end at the staging configuration)
    • (in settings, click MoveToMouth)
    • (in settings, don't click MoveToMouth)
    • Back on the BiteDone Page, click "Move Above Plate" and verify it works
  • U_PostMeal
• Go into the bite transfer settings menu while the robot arm is either in the above plate or resting position configuration. Click MoveToMouth. Click Done. During each of the two subsequent stage of motion, pause the motion, then x out of the modal. Verify that it goes back to the main menu. Then, go back into BiteTransfer and verify that it works as expected.
  • [FAIL] Move From Mouth
    • NOTE: This FAILS because it tries to execute MoveToStaging at the beginning, but the robot arm is in collision with the in front of wheelchair collision object. I don't think there is anything to change on the app side because it is entering from the main settings menu so has no reason to suspect the arm is near the mouth (it shouldn't have been -- that is due to a failed ending procedure of the settings menu). Thus, the better solution is develo a MoveToStagingGeneric that checks the robot end effector pose, and based on whether it is closer to the user than the splash wall, decides whether to execute servo or the kinematic action. This is documented in issue Implement a Generic MoveToStaging, and perhaps a service to return the robot arm state #116 .
  • Move Above Plate / To Resting Position
• Go into the bite transfer settings menu while the robot arm is either in the above plate or resting position configuration. Move To Mouth. Click Done. During each of the two subsequent stage of motion, pause the motion and then resume it. Verify that it works as expected.
  • Move From Mouth
  • Move Above Plate / To Resting Position
• Test the Responsiveness of the BiteTransfer Settings page and modal in portrait and landscape.

Real

Setup:

• See wiki for standard robot setup. Verify that you're on ada_feeding#155

Testing the MoveFromMouth change (same as above, but instead of pausing we induce errors):

• Use the app as normal. On the BiteDone page, select each of the three buttons (one at a time). Then, induce an error and resume each action, and verify that it functions as expected.
  • Move above plate
    • Error/resume during "move from mouth"
    • Error/resume during "move above plate"
  • Move to resting position
    • Error/resume during "move from mouth"
    • Error/resume during "move to resting position"
  • Move to staging configuration
    • Error/resume during "move from mouth"
• Use the app as normal. On the BiteDone page, select each of the top two buttons (one at a time) and let them succeed. Then go back to "Move To Mouth," induce an error it, and click "Back," thereby invoking "MoveFromMouth." Verify that it stops at the DetectingFace page, and doesn't move on to another motion.
  • Move above plate
  • Move to resting position
• Throughout this entire test, the video feed on the app is never white.

Testing the Settings / Bite Transfer Menu:

• During standard usage of the app, while the robot is moving, click Settings. Verify that it doesn't go to Settings.
• During a screen where the robot is stationary, go to settings. Verify it goes to settings.
• Go to Bite Transfer settings. Change the settings with the arrow keys. Verify that the file ada_feeding/ada_feeding/configs/ada_feeding_action_servers_current.yaml gets updated accordingly.
• Same as above but change the settings with the text input and verify that it gets updated accordingly.
• Click "Move To Mouth". Verify that it stops the appropriate distance from your mouth.
• Click "Move From Mouth". Verify that it goes back.
• Change the distance. Repeat the above two and verify that it respects the new distance
• Click "Set to Default", verify that on the screen and in the yaml file it goes back to 2.5cm.
• Go back to "Home." Go up till the BiteDone screen. Then go to settings. Verify that it still moves to the Staging configuration.
• While it is Moving To Mouth, induce an error (e.g., put a collision in the way) and ensure the user doesn't get stuck and can continue.
• While it is Moving From Mouth, induce an error and ensure the user doesn't get stuck and can continue.
• In the "BiteTransfer" menu, click "MoveToMouth". Before it finishes, induce an error. Then close the modal. Then click each of the two buttons (one at a time) and verify that they don't start off in a paused state. Also verify that both run fully as expected.
  • Move To Mouth
  • Move From Mouth
• For each of the following states of the home screen, go into settings. For some of them click MoveToMouth, for others don't (see below). Then, click Done. Verify that it resets the robot to the configuration it was in before you went to settings.
  • U_PreMeal (in settings, click MoveToMouth)
  • U_BiteSelection
  • U_BiteAcquisitionCheck (in settings, click MoveToMouth)
  • U_BiteDone
    • (in settings, click MoveToMouth)
    • (in settings, don't click MoveToMouth)
  • U_PostMeal
• Go into the bite transfer settings menu while the robot arm is either in the above plate or resting position configuration. Click Done. During each of the two subsequent stage of motion, induce an error the motion, then x out of the modal. Verify that it goes back to the main menu. Then, go back into BiteTransfer and verify that it works as expected.
  • Move To Mouth
  • Move From Mouth
• Go into the bite transfer settings menu while the robot arm is either in the above plate or resting position configuration. Click Done. During each of the two subsequent stage of motion, induce an error the motion and then resume/retry it. Verify that it works as expected.
  • Move From Mouth
  • Move Above Plate / To Resting Position

Stress Testing MoveToMouth:

• While on the BiteTransfer Settings menu, try going to/from your mouth with your head in the below positions. Verify that it works as expected.
  • Head in a "normal" position for you
  • Head lower than "normal" for you
  • Head higher than "normal" for you
  • Head to the left of "normal" for you
  • Head to the right of "normal" for you

---

Before creating a pull request

• Format React code with npm run format
• [N/A] Format Python code by running python3 -m black . in the top-level of this repository
• Thoroughly test your code's functionality, including unintended uses.
• Fully test the responsiveness of the feature as documented in the Responsiveness Testing Guidelines. If you deviate from those guidelines, document above why you deviated and what you did instead.
• Consider the user flow between states that this feature introduces, consider different situations that might occur for the user, and ensure that there is no way for the user to get stuck in a loop.

Before merging a pull request

• Squash all your commits into one (or Squash and Merge)

[amalnanavati]

Remaining TODOs (in addition to in-person testing above):

• Although ada_feeding's amaln/move_from_mouth_fallback branch implements a cartesian fallback followed by a kinematic one, the kinematic one can actually result in dangerous trajectories near the mouth and a non-ideal end configuration (since it is a pose goal, not a configuration goal). We should remove that. Consider having the kinematic one move to the stagign configuration, but even that seems slightly dangerous. If it gets to a singularity, maybe the user will have to teleop it... ?
  • Addressed by removing kinematic fallback and removing the tilt, which increases the success of servo and cartesian.
• Say the user refreshes the Settings menu after they have moves to their mouth. It will call the standard MoveToStagingConfiguration, which will fail. Fix this! (one option is adding a fallback to MoveToStaging to remove the in front of face wall, but that brings up the above issue of kinematic motions near the face).
  • Addressed by storing whether the settings menu is at the mouth or not.
• There is sometimes the below error. Fix it!
  
  • Addressed by moving the setPaused(false) our of the robotMotionProps useMemo and inside the useEffect that runs at the beginning and the setLocalMealStateWrapper useCallback. This works because the latter two are not involved in rendering, so React is never in the scenario where it is re-rendering BiteTransfer and then has a state change that simultaneously requires re-rendering RobotMotion
  • NOTE: If it was not addressed, the next thing to do is not set setPaused in BiteTransfer at all, and find a different way to ensure that a previously paused action does not result in the next one being paused.

[amalnanavati]

Tested in-person. While the above tests passed, there were lingering errors:

• Sometimes Settings/BiteTransfer started in a paused state.
  • I believe I addressed this but should test it further.
• Lingering visual issues with the text and footer (see screenshots)
  
  
  
  • Addressed by changing the view the Pause button was nested in, and by slightly increasing the height of the Modal. (the core issue is that the font size is based in vh/vw, not based on the Modal height/width it is within. That can be a later TODO.)
• When BiteTransfer ends, the app doesn't restore the robot to its old configuration.
  • Addressed by having Done execute up to two actions that move the robot arm back to the configuration corresponding to the non-moving state the app was in before foing into settings.
• The MoveFromMouthToAbovePlate is confusing when it faces an error (both back and resume have the same icon), plus if you retry during the MoveAbovePlate part of it, it does a really awful attempt to Servo to the staging configuration.
  • Addressed by removing MoveFromMouthToAbovePlate (see above)

[amalnanavati]

Addressed all the above issues (see resolutions below the issues) and thoroughly tested in sim (see new test cases above). Once all the test cases for the real robot have also been run, it will be ready to merge in.