Improve Bite Transfer

[amalnanavati]

Description

In service of #64 , this PR improves bite transfer in the following ways:

1. Moves the staging location to in-front of the face, since that location works better for cartesian motions to approahc the mouth.
2. Makes the approach to the mouth cartesian. Adds a jump threshold (to prevent joint jumps that result in non-cartesian motions by the post-processor), and required fraction of the path that must be completed to consider it a success.
3. Creates MoveFromMouth actions, that first move to the staging location, and then move to a specified end configuration (this PR replaces Added MoveFromMouth Actions #56 ).
4. Tunes the range parameter for RRTStar to result in plans that don't make unnecessarily large, overshooting motions. See ada_ros2#17.
5. Other small improvements:
   1. Moves some of the behaviors used in both MoveToMouth and MoveFromMouth into an idiom.
   2. Implements proper cleanup as a fallback of the tree.

Testing procedure

1. Pull the latest code in the following packages:
   1. ada_feeding: this branch amaln/improve_bite_transfer
   2. pymoveit2: branch amaln/cartesian_avoid_collision.
   3. ada_ros2: branch amaln/rrt_range
   4. forque_sensor_hardware: branch main
   5. py_trees_ros: branch amaln/service_client
2. Follow the[ instructions in the README]
   (https://github.com/personalrobotics/ada_feeding/tree/amaln/improve_bite_transfer/ada_feeding#usage) for what code to run before you can run the action servers.

• Move to the resting position and ensure it works as expected: ros2 action send_goal /MoveToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback
• Move to mouth and ensure it works as expected: ros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveTo "{}" --feedback
• Move from mouth to resting position and ensure it works as expected: ros2 action send_goal /MoveFromMouthToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback
• Move back to the mouth, and then move from mouth to above plate, and ensure it works as expected: ros2 action send_goal /MoveFromMouthToAbovePlate ada_feeding_msgs/action/MoveTo "{}" --feedback
• Terminate the perception nodes. Move to the resting position, and then move to the mouth. Ensure the motion to the mouth fails immediately (as opposed to e.g., looping forever).

Further Improve Plan Quality

Although this PR is enough to bring bite transfer up to MVP, the following are directions to investigate to further improve plan question.

• Further tune RRT*'s parameters and/or switch to AnytimePathShortening as documented in ada_ros2#17.
• Try different IK solvers (e.g., TRAC-IK, or others documented here), to see if they give better solutions, where better is defined as faster, better looking, or provides us more control (e.g., to tune the parameters and produce more human-like IK).

Before opening a pull request

• Format your code using black formatter python3 -m black .
• 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

• Squash & Merge

[amalnanavati]

Have tested in sim. Yet to test on real.

[amalnanavati]

Tested on t0b1. Necessary improvements:

1. The cartesian plan service doesn't take into account velocity scaling. That must be manually added (e.g., by the MoveTo behavior).
2. I'm not happy with the fact that the motion to the staging configuration sometimes takes an unnecessarily large trajectory. Maybe we should reduce the workspace size? Or Plan multiple times and pick the shortest (Related to [ROS2] Make Planning or Execution Optional for MoveTo #61 )? I can try adding position path constraints, but my hunch is that will over-constrain OMPL's planner (as documented here).
3. Sometimes the cartesian motion to the mouth goes mostly to the mouth, but then does a huge swivel before stopping (although it ends in a similar position). To address this, we should do the following:
   1. See if planning multiple times addresses this. If so, we can compute n plans and then take the shortest. This would require [ROS2] Make Planning or Execution Optional for MoveTo #61 so MoveTo can only plan without executing.
   2. If not, see what knob addresses this (step size? Increasing the goal tolerance? the jump thresholds in the cartesian planning message? etc.). Ensure that knob reliably addresses the issue.
   3. As an ultimate safety measure, we should try to detect large swivels of the arm and truncate the trajectory at that point. This does require forward kinematics along the trajectory, but it may be necessary.

The Point 1 should be addressed in this PR. Point 2 and 3 should be addressed in this PR if the solution is something other than planning multiple times. If the solution is planning multiple times, it should wait on #61 .

[amalnanavati]

Updates re. each of the issues above:

1. Has gotten addressed with the recent pushes.
2. I tried multiple approaches to address this. For documentation's sake, I'll list those that worked and those that didn't:
   1. PILZ LIN Planner: After some finnagling with the parameters we pass to the MoveTo behavior, I was able to make headway on PILZ. But then I started running into it being unable to compute the IK. Before I could look deeply into addressing that, I realized that PILZ also gets linearly interpolated poses and computes one IK solution, so it seems very similar to the cartesian interpolator. The benefit of PILZ is unclear.
   2. OMPL Objectives: No improvements with changing objectives. The Minimax / MaximizeMin objectives could not find a plan.
   3. Workspace bounds: These only seem to impact mobile bases, not manipulators.
   4. Adding more path constraints: OMPL constrained planner currently only supports a single position/orientation path constraint, and unfortunately the alternative, rejection sampling, is too slow.
   5. Modifying RRTStar Parameters: I started with the range parameter, which seemed most relevant. This works. The default (if set to 0.0) is ~7.0. I found better trajectories with 3.5. 1.75 has even better trajectories, but sometimes fails within 5 or 10 secs of planning time. I think the sweet spot will be somewhere between 1.75-3.5, but it requires more trails to find.
   6. Future Work: In addition to finalizing the range parameter tuning, other knobs worth playing with include: CHOMP post-processer (or STOMP, although I couldn't find it in humble), and OMPL's AnytimePathShortening.
3. I have seen this again (rarely, but twice). I have verified that it is an issue with the plan, not the controller. The anecdotal behavior is that the fork moves to the final pose, and then there is a weird motion to adjust the configuration (even though the end pose is still the same). I need to look more into the CartesianInterpolator to understand why this is. I have saved one of the "bad" trajectories, for introspection.
4. Additionally, I saw a new issue where the Cartesian plan only solves 2%, but still returns and gets executed. The MoveTo behavior should instead take in a threshold fraction completed it uses to determine success, and return failure otherwise.

[amalnanavati]

Updates re. each issue:

1. Scaling cartesian plans' velocity has already been addressed.
2. The issue of large motions from the resting position to the staging location has been addressed by setting the range parameter to 2.5 in ada_ros2#17.
3. The issue of a large swivel at the end of a cartesian motion:
   1. Problem:
      1. We did not pass a jump_threshold with the cartesian motion request, so any joint jumps between consecutive IK solutions was allowed.
      2. As a result, the last (?) IK was sometimes very different from the rest.
      3. The time optimal trajectory generator (called from here), then, smoothly interpolated the joints between this joint-space jump.
      4. That resulted in a path that is nearly cartesian, but has a huge non-cartesian swivel at the end.
   2. Solution:
      1. Set a jump_threshold of 4.0. I determined this by looking at the sample trajectory I saved with a large swivel. For the configurations without the swivel, the jump threshold was maximally 2.5, but for the configuration with the swivel, the jump threshold was ~13. To be safe (and remove smaller swivels as well), I set the jump_threshold to 4.0.
      2. Incidentally, the default max_step that was being used, 0.25cm set in pymoveit2, was resulting in trajectories with ~100 points, even though only ~17 were unique and the rest were getting removed by the post-processor. So I also increased that to 1cm.
4. I added the ability to reject plans that have not completed at least a required fraction, which I set to 0.85 for MoveToMouth and MoveFromMouth.

Rarely, the motion from the staging configuration to the resting configuration would get unnecessarily close to the user's head. I only saw it once over ~20 trajectories. This should be addressed in #65 .

[amalnanavati]

At this stage, I think the improved Bite Transfer is ready to test in-person.

[amalnanavati]

Issues from in-person testing:

1. The staging->resting plan sometimes runs into the weebo monitor. The easiest solution to this would be adding a collision wall there. As long as we're doing it, we should also add a collision wall at the left side of the wheelchair, so the robot doesn't overshoot it. We can also add a top collision wall if we want. (Effectively, we are creating a workspace. It is unfortunate that MoveIt's workspace only applies to mobile bases, not manipulators.)
   1. Incidentally, this also made me realize that the our table STL is about twice as wide as our actual table. Of course it's not crucial for it to exactly match, but just FYI.
2. The cartesian trajectories are now very jerky because of how I did velocity scaling. I should find another way to do it.

[amalnanavati]

Updates on the in-person testing issues:

1. I addressed this my adding walls to the workspace, in Added workspace walls to the planning scene #82 .
2. I addressed this by fixing cartesian velocity scaling to not just update the time from start, but also update the velocity and acceleration.

Ready for in-person testing again.

[amalnanavati]

Tested it and it works!

[amalnanavati]

This is now thoroughly tested and proven, with and without the web app, and ready to be merged in. (#92 and #94 addressed all final bugs).

The few lingering issues necessary to bring bite transfer up to MVP are #96 , #83 , and #54 .