Add more granularity to the stow service

[hello-amal]

Currently, the stow service stows the wrist, arm, and lift. However, when using the web app, it is common for the operator to want to only stow the wrist, while keeping the other joints stationary. Thus, we should generalize the stow wrist service to take in three boolean parameters -- exclude_wrist, exclude_arm, and exclude_lift -- which will determine which joint groups do(n't) get stowed.

(Note: currently the way the web app gets around this is by stowing the wrist by calling the trajectory server with hardcoded joint positions. However, that doesn't work well because the stow configuration per tool is slightly different, which stretch_ros2's service accounts for but the web app does not. This issue will allow us to move the web app to use the service, centralizing the stow functionality.)

[hello-binit]

Hi @hello-amal, the robot is able to stow without self collisions because it uses all of its joints. E.g. the Lift moves upward to prevent wrist collision with the base. Excluding joints or joints groups from the stowing procedure opens the possibility of self collisions.

We could implement an only_wrist mode where the stow service calculates if it can stow without self collision and return False if it cannot achieve it. IMO, supporting the general case of selective stowing complicates the implementation too much and introduces too many options in the operator UI for the user. What do you think?

Another more complicated option is to introduce a motion planner that is aware of the robot's geometry to plan a stow motion with the selected joints. Once again, this would return False if it cannot achieve it.

[hello-amal]

Yeah implementing a "stow wrist" service that either returns False or stows the wrist seems reasonable.

I was discussing the third option with @hello-fazil , and I do think that eventually we should have planning and execution actions. e.g., the client sends either a joint-space or a cartesian goal (e..g, target end-effector pose) to the action, the planner computes a collision-free trajectory to that goal if it exists, and then the driver executes it. At a minimum, this planner should account for self-collisions, but ideally once we have the infra for a voxel collision map, it should also account for object collisions. Having such a planner and collision scene would open up "proper" motions for things like click-to-pregrasp or automated grasping (as opposed to the current motions, which are one-joint at a time and have hard-coded conditionals to avoid self-collisions).

If we stay in the ROS2 world, MoveIt2 has good support for such a planner and executor (I saw we did integrate Stretch and MoveIt in ROS1). I've also made significant contributions to the open-source pymoveit2 repository, which provides a Python interface to interact with MoveIt2 in humble (MoveIt2 didn't release their Python bindings until iron).

But until we have the support for a proper collision scene and planner, I think implementing a "stow wrist" service makes sense :)