[ROS2] Improve Bite Transfer

[amalnanavati]

The MVP Bite Transfer in #42 and #56 results in some scary/dangerous motions, for a few reasons:

1. The movement from the staging position to the mouth is a kinematic motion that has few constraints, and can therefore be a large motion that gets out of the user's eyesight, gets very close to the user, etc.
2. We turn off the wheelchair collision object for the duration of the final movement to the mouth, which can result in trajectories where other joints of the robot get too close to or collide with the body.
3. The entire MoveFromMouth motion has the wheelchair collision constraint disabled (as opposed to just for the small part of the motion that gets out of collision with that mesh).

The MVP way to fix this is fundamentally moving back to the ROS1 bite transfer:

1. Make the motion from the staging location to the mouth be Cartesian.
2. For the MoveFromMouth actions, have it be composed of two motions, one back to the staging location (with the expanded wheelchair collision turned off), and the other to the above plate or resting position (with it turned on).
3. Potentially lower the speeds when moving towards and away from the mouth ([ROS2] Reduce velocity on actions #58 ).

As part of doing that, we should maybe move back to the center staging location for now, as opposed to the side one. We should also consider moving that staging location down, to account for people who are shorter.

Note that part of the issue is about collision constraints. We really only want to turn off collisions with an expanded mesh close to the head, but we end up turning off collisions with the entire expanded wheelchair collision object (meant to represent the user's body. That is problematic. Even if we do add a separate "wheelchair collision object" (meant to represent a user's body) and a separate "head collision object," the challenge is that people are different sizes and for some people their head will be in the wheelchair collision object. So maybe we have to get the Octomap working to deal with this?

[amalnanavati]

Re. cartesian motions, MoveIt2's default Cartesian planner is not great. It basically divides the cartesian path from teh start to the goal into n waypoint poses, and for each pose it makes a single IK call. If it succeeds, it appends it, if it fails, it stops. Then, it checks for if the joints jumped too much, and truncates the path at the point of the largest jump.

I'll try to develop a cartesian plan using OMPL's equality constraints and see if that is better.

[amalnanavati]

Updates:

• Adding position path constraints like this do work in terms of moving from the "in front of face" staging location 10 cm forward. Unfortunately, adding both the orientation path constraints (to prevent food from falling off of the fork) in addition to the position path constraints fails due to too many constraints. Documented here, and also a Github Issue that indicated "the projection method does not support a number of constraints larger than the ambient dimension" .
  • I dug into this limit on number of constraints a bit. If I add two orientation constraints or two position constraints, it seems to drop down into rejection sampling (which is unreasonably slow). If I have one orientation and one position constraint, it says "Space is over constrained!".
• I then went back to the cartesian planning from the "in front of mouth" staging location. From this position, and using a step size of 0.025, cartesian planning for a "simple" offset (e.g., 50cm in y, -10cm in z) seemed to work well.
  • Note that sometimes this resulted in weird plans. So we should consider adding a meta-planner that calls this multiple times and picks the "best" of the plans.
• For now, I will focus on reverting to the "in front of mouth" staging location and using cartesian planning, and going from there. I should be very careful about the cartesian planning parameters, such as whether to respect collision constraints and such. If it still doesn't work, I can also look into the PILZ planners that seems to be more suited for cartesian motions.