Spaces:
Sleeping
Sleeping
File size: 10,323 Bytes
dbc6675 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 | import re
import numpy as np
import pddl
# Special Values
VAL_PAD = -99.0 # Slot is unused (outside problem size)
VAL_DONTCARE = -10.0 # Goal value when variable is not specified
class FSFEncoder:
def __init__(self, domain_name, domain_pddl_path, max_objects):
"""
max_objects: The count of distinct objects in the largest problem. The actual vector size will be max_objects + 1 (for Global Slot 0).
"""
self.domain_name = domain_name
self.domain_pddl = pddl.parse_domain(domain_pddl_path)
# Vector Size = Max Objects + 1 (Index 0 is reserved for Global/Robot)
self.vector_size = max_objects + 1
print(
f" [FSF] Initialized with {self.vector_size} slots (Max Objects: {max_objects} + 1 Global)"
)
# Regex to correctly parse "(pred arg1 arg2)" groups
self.predicate_regex = re.compile(r"\(([\w-]+(?: [\w-]+)*)\)")
def _get_sorted_objects(self, problem_path):
"""
Parses problem to get all objects, including domain constants.
Returns sorted list of object names.
"""
problem = pddl.parse_problem(problem_path)
objs = set()
for o in problem.objects:
objs.add(o.name)
for o in self.domain_pddl.constants:
objs.add(o.name)
return sorted(list(objs))
def _get_object_indices(self, objects):
# Index 0 is Global. Objects start at 1.
return {o: i + 1 for i, o in enumerate(objects)}
def parse_state_atoms(self, state_lines):
"""
Parses list of strings using Regex to separate predicates.
Input: ['(on a b) (clear c)']
Output: [('on', 'a', 'b'), ('clear', 'c')]
"""
atoms = []
for line in state_lines:
# Find all matches of (pred arg1 arg2 ...)
matches = self.predicate_regex.findall(line)
for m in matches:
parts = m.split()
if parts:
atoms.append(tuple([p.lower() for p in parts]))
return atoms
def embed_trajectory(self, problem_path, trajectory_file, verbose=False):
"""
Reads a .traj file and returns [T, MAX_OBJECTS] matrix.
"""
objects = self._get_sorted_objects(problem_path)
obj_map = self._get_object_indices(objects)
if verbose:
print(f"\nDEBUG: {problem_path}")
print(f"Objects ({len(objects)}): {objects[:5]} ...")
print(f"Map: {list(obj_map.items())[:5]} ...")
with open(trajectory_file, "r") as f:
lines = f.readlines()
vectors = []
for i, line in enumerate(lines):
atoms = self.parse_state_atoms([line])
# Debug first and last step
is_debug_step = verbose and (i == 0 or i == len(lines) - 1)
vec = self._state_to_vector(atoms, objects, obj_map, debug=is_debug_step)
vectors.append(vec)
return np.array(vectors, dtype=np.float32)
def embed_goal(self, problem_path):
"""
Parses problem goal and returns [MAX_OBJECTS] vector.
Unspecified variables are set to VAL_DONTCARE.
"""
objects = self._get_sorted_objects(problem_path)
obj_map = self._get_object_indices(objects)
problem = pddl.parse_problem(problem_path)
# Extract goal atoms
goal_atoms = []
def visit(node):
if hasattr(node, "name") and hasattr(node, "terms"):
# Predicate
args = [t.name if hasattr(t, "name") else str(t) for t in node.terms]
goal_atoms.append(tuple([node.name] + args))
elif hasattr(node, "operands"):
for op in node.operands:
visit(op)
visit(problem.goal)
# Generate vector with DONTCARE as default
return self._state_to_vector(goal_atoms, objects, obj_map, is_goal=True)
def _state_to_vector(self, atoms, objects, obj_map, is_goal=False, debug=False):
"""
Core Logic: Maps atoms to vector based on domain rules.
"""
# 1. Initialize
# If it's a goal, default is DONTCARE.
# If it's a state, default is PAD (we fill valid slots below).
default_fill = VAL_DONTCARE if is_goal else VAL_PAD
# Create vector of size N+1
vec = np.full(self.vector_size, default_fill, dtype=np.float32)
# Initialize Valid Slots to 0.0 for states
if not is_goal:
# Global Slot (0) defaults to 0
vec[0] = 0.0
# Object Slots (1..N) default to 0.0 (e.g. Table/Unvisited/Free)
for i in range(len(objects)):
slot = i + 1
if slot < self.vector_size:
vec[slot] = 0.0
# Helper to get value (index) of an object
def get_val(name):
return float(obj_map.get(name, 0))
# Helper to get slot (index) of an object
def get_slot(name):
s = obj_map.get(name, -1)
if s >= self.vector_size:
# This should theoretically not happen if we scanned correctly
return -1
return s
if debug:
print(f"Processing Atoms: {atoms}")
# DOMAIN SPECIFIC LOGIC
if "blocks" in self.domain_name:
# Slot i = Block i
# Values: 0 (Table), -1 (Held), k (On block k)
# First pass: Set held
for pred in atoms:
name = pred[0]
args = pred[1:]
if name == "holding":
# (holding a) -> V[a] = -1
slot = get_slot(args[0])
if slot != -1:
vec[slot] = -1.0
if debug:
print(f" Set {args[0]} (slot {slot}) = -1.0 (Held)")
elif name == "on":
# (on a b) -> V[a] = Index(b)
slot = get_slot(args[0])
if slot != -1:
vec[slot] = get_val(args[1])
if debug:
print(
f" Set {args[0]} (slot {slot}) = {get_val(args[1])} (On {args[1]})"
)
elif name == "ontable":
# (ontable a) -> V[a] = 0
slot = get_slot(args[0])
if slot != -1:
vec[slot] = 0.0
elif "gripper" in self.domain_name:
# Slot 0: Robot Location (Room Index)
# Slot i (Ball): Room Index OR -1 * Gripper Index
# Slot i (Gripper): 0 (Free) or Ball Index (Holding)
for pred in atoms:
name = pred[0]
args = pred[1:]
if name == "at-robby":
vec[0] = get_val(args[0])
if debug:
print(
f" Set Global (slot 0) = {get_val(args[0])} (Robby at {args[0]})"
)
elif name == "at":
# (at ball room) -> V[ball] = Index(room)
slot = get_slot(args[0])
if slot != -1:
vec[slot] = get_val(args[1])
if debug:
print(
f" Set {args[0]} (slot {slot}) = {get_val(args[1])} (At {args[1]})"
)
elif name == "carry":
# (carry ball gripper)
ball, gripper = args
b_slot = get_slot(ball)
g_slot = get_slot(gripper)
if b_slot != -1:
vec[b_slot] = -1.0 * get_val(gripper)
if debug:
print(
f" Set {ball} (slot {b_slot}) = {-1.0 * get_val(gripper)} (Carried)"
)
if g_slot != -1:
vec[g_slot] = get_val(ball)
if debug:
print(
f" Set {gripper} (slot {g_slot}) = {get_val(ball)} (Holding)"
)
elif "logistics" in self.domain_name:
# Slot i (Pkg/Truck/Plane): Location Index
# Slot i (Pkg in vehicle): -1 * Vehicle Index
for pred in atoms:
name = pred[0]
args = pred[1:]
if name == "at":
# (at obj loc)
slot = get_slot(args[0])
if slot != -1:
vec[slot] = get_val(args[1])
if debug:
print(
f" Set {args[0]} (slot {slot}) = {get_val(args[1])} (At {args[1]})"
)
elif name == "in":
# (in pkg vehicle)
pkg, veh = args
slot = get_slot(pkg)
if slot != -1:
vec[slot] = -1.0 * get_val(veh)
if debug:
print(
f" Set {pkg} (slot {slot}) = {-1.0 * get_val(veh)} (In {veh})"
)
elif "visit" in self.domain_name: # visitall
# Slot 0: Robot Location (Cell Index)
# Slot i (Cell): 0 (Unvisited), 1 (Visited)
for pred in atoms:
name = pred[0]
args = pred[1:]
if name == "at-robot":
vec[0] = get_val(args[0])
if debug:
print(
f" Set Global (slot 0) = {get_val(args[0])} (Robot at {args[0]})"
)
elif name == "visited":
# (visited cell) -> V[cell] = 1
slot = get_slot(args[0])
if slot != -1:
vec[slot] = 1.0
if debug:
print(f" Set {args[0]} (slot {slot}) = 1.0 (Visited)")
return vec
|