Preconditions of move_arm_to_transport_pose not satisfied in uplexmo pick and place demo

[sebastianstock]

The SequentialPlanMonitor checks the preconditions when executing an action in the uplexmo pick and place demo. Currently, checking preconditions fails for the 4 action which moves the arm to the transport pose.

The unsatisfied precondition is get_robot_arm_at(interaction). The state of the action looks as follows:

 get_robot_arm_at(interaction) := false
  get_robot_arm_at(observe) := false
  get_robot_arm_at(home) := false
  get_robot_arm_at(tucked) := false
  get_robot_arm_at(transport) := false
  get_robot_arm_at(place) := false
  get_robot_arm_at(unknown) := true

The reason for this is that interaction pose is not known to Mobipick and therefore it assumes to be in an unknown pose.

[alexander-sung]

As discussed, this bug appears due to a workaround currently present in the demo. The robot actually (both in simulation and in reality) uses more poses for manipulation actions than specified in the config file. These are interim poses implicitly used when executing the pick and place macro actions. Since performing an action like "move arm to target pose" does not require to provide the starting pose, these unspecified poses are treated as "unknown" poses during execution. For planning, however, an "interaction" pose symbol was introduced to define actions for moving from source pose to target pose. Execution and planning thus differ, which now becomes apparent when validating the actions.

Two possible solutions:

1. Specify poses used by the robot as named poses and thus symbols for planning. If I remember correctly, only one more pose definition comes to mind for hovering the gripper over an object. Drawback: Pose configurations need to be kept consistent with moveit action definitions because they don't use the same sources. So far they do because a "move arm to pose" command is provided to moveit with the target pose.
2. Remove the interaction pose symbol and adjust actions for planning to move from "unknown" source poses. Drawback: Future extensions also using unknown poses might result in unexpected behavior.

[alexander-sung]

In a deeper discussion, @mintar pointed out that for solution 1 it is not possible to specify the target "interaction" pose after, for example, a pick-up action explicitly because these differ depending on the object grasped. On the other hand, solution 2 would not reflect reality well because not all actions are feasible from any "unknown" pose.

Proposed solution therefore would introduce a new fluent based on the moveit implementation for how the end effector is constraint after grasping an object, e.g., is_end_effector_upright(). The action to move to transport pose after picking up an object thus would not check for get_robot_arm_at() a specific pose but check for is_end_effector_upright() instead.

[alexander-sung]

Another thought on this. In an ideal world, reality can be represented precisely and planning would use this representation to plan accordingly. I understand this is preferred, and no artificial limitations should be necessary to "pre-solve" the problem.

Then reality hits us, and circumstances like cable entanglement are difficult to describe precisely. We definitely should get rid of the "interaction" symbol since it is not a correct representation. However, is using is_end_effector_upright() really correct to use as a precondition, @mintar, @sebastianstock? Moving into transport pose is possible from most but not all poses, including the handover pose, for example, which is not upright. It seems during talking about ideal solutions, we might have missed to address this issue properly.

[mintar]

In an ideal world, reality can be represented precisely [...] Then reality hits us, and circumstances like cable entanglement are difficult to describe precisely.

I think we are on the same page, but I would formulate it a bit differently: The symbolic representation used by planning will always be more abstract than the sub-symbolic representation used by motion planning. There will never be a one-to-one matching between the two, so that there is a valid motion between two poses in the task plan iff (= "if and only if") that valid motion also exists in the motion planner.

The interaction between planning and execution is an unsolved problem with many interesting research questions: How to communicate the reason for a failure to the task planner? How to re-plan so that we don't get stuck in an infinite "retry failing action" loop? Etc.

no artificial limitations should be necessary to "pre-solve" the problem

I agree that we shouldn't "pre-solve" the problem in the sense that we exclude valid actions from the planning domain just to reduce the search space for the planner.

However, is using is_end_effector_upright() really correct to use as a precondition, @mintar, @sebastianstock? Moving into transport pose is possible from most but not all poses, including the handover pose, for example, which is not upright.

Oh, you're right. I didn't remember that. I assumed that we can only move to the transport pose (with cartesian "upright" constraints) if the starting pose was already upright. If that assumption is wrong, then of course we also shouldn't model it this way.

I guess the best way forward is to first remove the "interaction" pose completely, then try the demo (and perhaps some other things, like a starting pose where the endeffector is not upright) and see whether we broke something. If yes, analyze the root cause of the failure (e.g., "end effector must be upright") or whatever and model that appropriately. By the way, the is_end_effector_upright (or whatever) also needs to be added to the effects of actions as well (depending if we're moving to a pose where it's upright or not).

If it's really true that we can do a move with cartesian constraints from an arbitrary starting pose, then we don't need to model the is_end_effector_upright fluent at all.

[alexander-sung]

I performed a simple fix by removing the interaction symbolic pose and using unknown pose instead. Please check if it is now working for you, @sebastianstock. Unfortunately, the action log now often mentions the unknown pose. Additionally, although the handover pose is fix it is not specified in https://github.com/DFKI-NI/mobipick/blob/noetic/mobipick_moveit_config/config/mobipick.srdf.xacro. Do you want to add it?

Most actions remain limited to specific starting poses for the robot arm. I didn't change that for now to avoid unwanted behavior but I'm open for suggestions. Removing the preconditions of specific starting poses from pick and place actions causes problems, and I remember trying this out before. It will require a bit of time to investigate in detail.

[mintar]

Additionally, although the handover pose is fix it is not specified in https://github.com/DFKI-NI/mobipick/blob/noetic/mobipick_moveit_config/config/mobipick.srdf.xacro. Do you want to add it?

Done: https://github.com/DFKI-NI/mobipick/commit/7ba6dbef676372aa49a84599a87525826b4069b3

[alexander-sung]

So, this issue is resolved, yes? @sebastianstock

[sebastianstock]

Yes, from my perspective it is fixed. I only left it open because I was not sure if you wanted to make other changes in this regard after those long discussions. :)

[alexander-sung]

Ah yes, I did in 264d8ff, just didn't add it here because the other aspects weren't causing the bug. Now it's linked. 🙂