[ROS2] MoveTo Idiom(s) for checking starting configuration
amalnanavati opened this issue · comments
This issue was motivated by a few specific cases, although we should come up with a generic way to handle it:
- The web app will sometimes call MovetoMouth from the AbovePlate configuration (e.g., if the fork already has food and the user decides to skip acquisition). As it stands, MoveToMouth will fail because the above-plate pose doesn't meer the orientation path constraints. Therefore, MoveToMouth should first check if the starting state aligns with orientation path constraints and, if not, don't use them during the movement to the staging location.
- If a motion fails half-way, the step we encourage the user to try is to re-run the motion. That mostly works. However, consider the MoveFromMouth action, where the arm succesfully completes the cartesian motion to the staging pose, but then fails in the onwards motion. If the user retries, the cartesian path service does not check if it is already satisfied, so might compute a path that moves from the pose to the same pose, but across a joint-space jump that results in a large and dangerous swivel. Therefore, we should check if the starting pose satisfies the cartesian goal constraints and only plan if it doesn't.
- I have sometimes (but rarely) seen it where the arm already satisfies the configuration that is passed in as a goal to MoveToConfig, but then MoveIt fails (e.g., maybe it returns a plan with only one point or some other edge cases that makes it think it failed). In the scenario where the arm already satisfies a goal constraint, we should just not plan at all.
These are all about checking the start config of the robot arm relative to goal/path constraints and adjusting planning accordingly. Which is why I think there should be a unified, generic, elegant solution to it.
#60 should be done before this. That enables us to do things like compute FK before a MoveTo action and write it to blackboard, so that the decorators can check if the joint state and FK align with the constraint or not.
Just documenting that even with the current jump threshold of 2.0, I noticed 2 instances on the real robot running on t0b1 today where MoveFromMouth failed after the cartesian motion, and then when i re-did it it did a circular motion before doing the kinematic plan. That should hopefully be addressed by not doing a plan if the goal constraints are already satisfied.
Part of this issue is also that for the MoveFromMouth and MoneToMouth trees, it should be able to determine which half of the tree it should execute based on its current pose.
This is more important for MoveFromMouth, because if the user terminates it during the kinematic motion, and then resumes, it will try to start with the cartesian motion, which will fail.
With MoveToMouth, if users terminate in the cartesian motion and then resume, it will move back to the staging location, detect their face, and then move it. Which may be a bit annoying, but won't result in users getting stuck.
Addressed by #115