Add support for more flexible goal specification

[threefruits]

The current process_goal_specification function can only process simple goal specifications.

https://github.com/sea-bass/pyrobosim/blob/ada1d009830f5ba07494a3a25c2775034b7e383a/pyrobosim/pyrobosim/planning/pddlstream/utils.py#L128C1-L143C1

This pull request updates the process_goal_specification function to process more flexible goal specifications, including constructs such as Exists and Imply.

c9e8fd8#diff-03e61f646fe833861aaa6e076241f719d3854c411ea0ba8f531552096bab6098R135-R148

[sea-bass]

This is a really cool addition!

Two comments:

1. (Un)fortunately, PDDLStream lets you create derived predicates so that you do not need to directly specify stuff like this. See for example:

   pyrobosim/pyrobosim/pyrobosim/data/pddlstream/domains/02_derived/domain.pddl

   Lines 95 to 162 in ada1d00

   	(:derived (Has ?loc ?entity)
   	(or
   	; CASE 1: Location and entity specified as instances
   	(and (or (Obj ?entity) (Robot ?entity))
   	(Location ?loc)
   	(or (At ?entity ?loc)
   	(exists (?s)
   	(and (Room ?loc) (Location ?s)
   	(At ?entity ?s) (AtRoom ?s ?loc))
   	)
   	)
   	)
   	; CASE 2: Location is a type, entity is instance
   	(exists (?l) (and (or (Obj ?entity) (Robot ?entity))
   	(Location ?l)
   	(or (and (Type ?loc) (Is ?l ?loc))
   	(and (Room ?loc) (AtRoom ?l ?loc)))
   	(At ?entity ?l))
   	)
   	; CASE 3: Location is instance, entity is a type
   	(exists (?o) (and (Type ?entity) (Obj ?o)
   	(Location ?loc)
   	(Is ?o ?entity)
   	(or (At ?o ?loc)
   	(exists (?s)
   	(and (Room ?loc) (Location ?s)
   	(At ?o ?s) (AtRoom ?s ?loc)))
   	)
   	)
   	)
   	; CASE 4: Location and object specified as types
   	(exists (?o ?l) (and (Type ?entity) (Obj ?o)
   	(Type ?loc) (Location ?l)
   	(or (and (Type ?loc) (Is ?l ?loc))
   	(and (Room ?loc) (AtRoom ?l ?loc)))
   	(Is ?o ?entity)
   	(At ?o ?l)
   	)
   	)
   	; CASE 5: Robot holding an object instance
   	(and (Robot ?loc) (Obj ?entity)
   	(Holding ?loc ?entity)
   	)
   	(exists (?r)
   	(and (Robot ?r) (Location ?loc) (Obj ?entity)
   	(Holding ?r ?entity)
   	(or (At ?r ?loc)
   	(and (Room ?loc) (exists (?s)
   	(and (Location ?s) (At ?r ?s) (AtRoom ?s ?loc))))
   	)
   	)
   	)
   	; CASE 6: Robot holding an object type
   	(exists (?o)
   	(and (Robot ?loc) (Obj ?o) (Holding ?loc ?o)
   	(Type ?entity) (Is ?o ?entity))
   	)
   	(exists (?r ?o)
   	(and (Robot ?r) (Location ?loc) (Obj ?o) (Holding ?r ?o)
   	(Type ?entity) (Is ?o ?entity)
   	(or (At ?r ?loc)
   	(and (Room ?loc) (exists (?s)
   	(and (Location ?s) (At ?r ?s) (AtRoom ?s ?loc))))
   	)
   	)
   	)
   	)
   	)

However, I think it's nice to have the option to do this as well. I assume that based on how PDDLStream works, this functionality would also hold for things like Imply as well?

2. You will need to support this in the ROS interface as well. See src/pyrobosim/pyrobosim_ros/examples/demo_pddl_goal_publisher.py.

The tricky thing about this is that the ROS messages GoalSpecification and GoalPredicate don't currently have a mechanism to specify such complex expressions ... hence the derived predicates.

So I might say, unless the ROS interface is also figured out, it might be better to encourage users to create derived predicates instead?

[sea-bass]

I would probably prefer you just used the literal "?o" directly in the expression below

... and actually, it should be ?l or ?loc since the Has derived predicate accepts Location type and not Object.

[sea-bass]

Need to add description of what the input and output arguments are. See other docstrings.

[sea-bass]

Suggested change

	return entity if entity else elem
	return entity or elem

[sea-bass]

Does this mean that the replace_goal_literal_tuple function can be removed with this change?

Seems like your new function replaces it completely, as this was the only place in the code base it was being used, so you can remove it.

I'd also suggest renaming it from process_element() to something more descriptive, like populate_entities() - - this might better indicate what it is the function does.

[sea-bass]

Might be nice to add a comment here.