tables_demo_planning: Allow duplicate poses

[mintar]

When there are duplicate base poses in the config file, tables_demo_planning prints:

"Duplicate poses in 'cic_tables_demo.yaml' might let checks fail whether something is at a specific symbolic pose."

Could the code be generalized to allow duplicate poses? I.e., could the robot be allowed to be "at" multiple locations at once?

This would be nice for several reasons: First, there are legitimate reasons to have multiple identical poses in the config file; in our case, it's for example the base_pick_pose for the pick&place demo and the base_table_2_pose that should be identical. Second, the current workaround is to move the poses just a few centimeters apart in order to suppress the warning, but that means that the robot very quickly is "at" another pose with only a few centimeters difference in location.

[alexander-sung]

We indeed discussed this before but there are pros and cons as usual. Reporting multiple poses is no problem and can be handled easily by the robot_api. However, all future planning needs to be aware of this then, especially when using some of duplicated poses but not all. We shall implemented it this time and make sure not to introduce new issues for the users.

I think this was the reasoning we had for the original tables demo. I agree that the current situation is not good since it easily leads to unexpected errors or unpleasant workarounds.

[mintar]

Yes, I agree, this is not trivial to handle. For example, the "move" action has the post-condition that the robot is no longer at the original location and is now at the target location. This assumes a unique location for the robot, and I wouldn't even know how to handle this with overlapping locations: does the "move" action invalidate all previous locations? Or just some? No way to know without actually reasoning about the geometry of locations.

I don't have a solution to this yet. Maybe leave this issue open until we come up with a good solution?

[mintar]

This may be the wrong way to think about it. Perhaps "overlapping locations" isn't a feature we want to have; instead, we should tackle the issue of why we need synonyms in the first place? I.e., add another layer of indirection - change base_pick_pose from being a pose to being a pose name (in this case, base_table_2_pose)?

[alexander-sung]

I like the direction you're thinking. Yes, that's why I didn't simply introduce duplicate poses for the demo. Indeed, I would like to handle it properly as well. By the way, did you encounter the related warning? https://github.com/DFKI-NI/mobipick_labs/blob/noetic/tables_demo_planning/src/tables_demo_planning/demo_domain.py#L90

Surely, we can address the duplicate poses in our case but the general problem persists. If we are going to fully support poses being the same with different names, basically we would need to let this be known to planning explicitly. It would probably be cumbersome for most planning applications. Another idea would be to filter them out in the robot_api and give warnings about it. However, this still leaves the case unanswered when two poses are close but not identical and filtering one out would depend on a dynamic threshold.

[mintar]

By the way, did you encounter the related warning?

Yes, see the first post in this issue.

If we are going to fully support poses being the same with different names, basically we would need to let this be known to planning explicitly.

Regarding duplicate poses (completely identical): I think it's fine to model the domain using unique poses, and to distinguish between actual numeric poses (where it doesn't make sense to have duplicates) and "functional" poses/placeholders, which should refer to actual poses by name. If we properly document this, I don't see a problem.

Regarding poses very close to each other (but not identical): I suppose it's up to the user to decide if they really need to model their domain like this. If they really need to distinguish between poses that are so close to each other that it's below the localization/navigation accuracy threshold, they need to have some sort of external localization or whatnot. I believe that's a localization problem, not a planning problem.

If we're not thinking about poses, but areas, then sure, they can overlap; the robot can be in Apartment A, Room B, Pose C at the same time. But I think that's a battle for another day.

So regarding this issue, I'd be happy if we turned base_pick_pose = (x, y, z, ...) into base_pick_pose_name = "base_table_2_pose" instead.

[alexander-sung]

Yes, see the first post in this issue.

Okay, never mind then. 😅 When you posted the first message, I didn't even know where that output was coming from.

On duplicate poses, you actually described the remaining problem before. Typically, a move action moves away from only one symbolic pose, so could lead to inconsistencies. Of course, if the user uses duplicate poses with different names and doesn't handle it properly, it is prone to cause unwanted effects. On the other hand, however, we wouldn't handle it either, just don't use both pose names in the same planning problem. As suggested, I think we should try and test thoroughly. Anyway it's good to discuss potential pitfalls here.

About the base_pick_pose_name = "base_table_2_pose", I'm not sure what you mean. The symbolic pose is mapped to a geometry_msgs.msg.Pose for execution of planned move actions. Even if we first map to a string, then to a pose, I think the problem remains. Additionally, str would be very generic for a mutual mapping to a pose. It is possible in our example but not recommended. Your other points sound good to me.

[mintar]

What I mean is that we differentiate between real poses and pose names. The config file would look like this:

base_pick_pose_name:     "base_table_2_pose"
base_place_pose_name:    "base_table_3_pose"
base_handover_pose_name: "base_handover_pose"

base_handover_pose: [21.58, 14.80, 0.0, 0.979, 0.0, 0.0, -0.205]
base_home_pose:     [20.50, 15.20, 0.0, 0.0  , 0.0, 0.0,  1.0  ]
base_table_1_pose:  [19.18, 15.40, 0.0, 0.707, 0.0, 0.0, -0.707]
base_table_2_pose:  [19.44, 14.70, 0.0, 1.0  , 0.0, 0.0,  0.0  ]
base_table_3_pose:  [21.00, 14.70, 0.0, 1.0  , 0.0, 0.0,  0.0  ]
base_table_4_pose:  [21.80, 14.90, 0.0, 0.707, 0.0, 0.0,  0.707]   # unused

tables_demo_node only works on *_pose. No duplicate poses there.

pick_n_place_demo_node first looks up the real pose to use using *_pose_name. After that, everything works as in tables_demo_node - again, no duplicate poses.

[alexander-sung]

Now I see what you mean. This would be a solution to avoid duplicate poses for our examples but still leave the fundamental problem unchanged, of course. Basically your proposal would need an additional method to read these hardcoded names from a configuration instead - maybe also via the parameter server.

[mintar]

What's the fundamental problem that remains unsolved?

[alexander-sung]

Anyone can still write duplicate poses or poses too close to each other into this config file or another and encounter the situation that the current pose reported does not fit to poses specified for planning. I will thus leave the warning as it is, as you posted in your first message.

[mintar]

Yes, keeping the warning as it is is good, because it indicates that something was modelled incorrectly (with our new assumptions, all "real poses" should be unique). Are you also going to introduce the "pose names" indirection in order to tackle this issue?

[alexander-sung]

Yes, I'll handle it. Currently, we have some parallel development going on, so I'll resolve this issue after merging the other work.

[alexander-sung]

Maybe I misunderstood, but I don't think it is really good: 8c56b07

Also needs some comments in the code for explanation, I guess.

[alexander-sung]

In this first attempt, I used a dictionary for storing pose_aliases: https://github.com/DFKI-NI/mobipick_labs/commit/8c56b079002d2012a7a24084bdbfc4866aca6791#diff-9f34dffd55fcb8974464ab731bad6600504695eb71d82d2148a9f24463c970beR85

It doesn't look nice but has the advantage of updating it by random access while looping through rosparam.list_params(). To avoid the additional mapping of a dictionary, we could use variables directly instead, e.g. self.base_handover_pose_name = "base_handover_pose" and update them if a corresponding entry is on the rosparam server. However, this would need a try-except block or all parameters always being configured for the rosparam.get_param() calls - both also not nice, in my opinion.

[alexander-sung]

Second attempt moves pick and place poses to the pick_n_place demo entirely (maybe a good idea anyway) and loads them from the rosparam server. Still a bit cumbersome, I would say.

https://github.com/DFKI-NI/mobipick_labs/compare/add-aliases-for-duplicate-poses?expand=1

Edit: Link will be outdated with the next commit. Last commit so far was d0096bb.

[alexander-sung]

I'd like to suggest this third attempt to resolve the issue. It reverts most parts to noetic branch and instead chooses a subset of poses to avoid duplicates. You can see it in the SCENARIO_POSE_NAMES variable. The pick_n_place demo includes pick and place poses but no tables, the tables_demo in contrast includes the table poses but not pick and place. The config file remains untouched.

Additionally, I want to point out whatever method we decide on here needs to be reflected in the symbolic_fact_generation, as a to-be-done. So, if we introduce pose name remapping in the config file and on the rosparam server, this needs a code update for the symbolic_fact_generation accordingly. Therefore, I prefer to avoid introducing this rather unclear remapping by either not configuring duplicate poses in the first place (current noetic branch) or by choosing a subset without duplicates (current add-aliases-for-duplicate-poses branch).

Any opinions on this? @marcvinci, @sebastianstock, @mintar

[alexander-sung]

Current changes can be seen here: https://github.com/DFKI-NI/mobipick_labs/compare/add-aliases-for-duplicate-poses?expand=1

[alexander-sung]

Chosen solution 2 after discussions and rebased on noetic.