Acquisition Recover on Failure

[egordon]

Description

This PR does 2 things:

(1) If Grasp/Extract fails OR it finishes and the Resting Pose / Above Plate movement would fail due to a F/T trip, we execute a Recovery Procedure.

The Recovery Procedure first sets the F/T threshold to max (50N) an then does a Selector to loop through recovery attempts.

Currently, this procedure only makes 1 recovery attempt: Servo up 3cm and re-check the F/T trip (attempt 3 times)

Tested in sim with manual insertions of Failure right before Extraction (and verification of 3x retry by setting the FT test to 0.01N instead of the default 4N)

(2) Calculate the TF of the food frame using the timestamp from the header.

Solves #126

Tested in sim by refreshing the webapp during food acquisition. Whenever the new goal wasn't rejected (which happened if the previous goal didn't finish terminating), the food frame did not move despite the robot camera frame having moved.

Before opening a pull request

• [x] Format your code using black formatter python3 -m black .
• [x] Run your code through pylint and address all warnings/errors. The only warnings that are acceptable to not address is TODOs that should be addressed in a future PR. From the top-level ada_feeding directory, run: pylint --recursive=y --rcfile=.pylintrc ..

Before Merging

• [x] Squash & Merge

[egordon]

@amalnanavati To be clear: this doesn't currently fix those errors in the sense that

(1) The max force (50N) is the threshold both in the recovery and the extraction, so if we experience >50N force we are stuck regardless (like, the only way out in that case is to remove force gating)

(2) Currently, it still uses Servo for recovery, so it will fail in the 2nd case as well in-collision.

This PR does contain the framework for adding a cartesian planner fallback, but at this point I think that is lower pri than re-adding online learning, so we can do that later.