1"""Glue-and-weld dynamics for the n-shaped-bridge environment.
2
3Mechanism inferred from real probes on level 1 (see ./journal.md section 2):
4
5* The bottle is a glue dispenser with a downward nozzle at (bx, by, bz - nozzle_drop).
6 Glue is deposited on the upward-facing surface underneath the nozzle only; bringing the
7 bottle alongside a vertical face does nothing (measured at 0.033 m and 0.012 m standoff).
8* Which named face receives the glue depends on where over the block the nozzle sits.
9 Over the middle of the up-facing area -> `glue_top`; over the +x/-x end region ->
10 `glue_end_b` / `glue_end_a`. For a block standing on end (pitch = -pi/2) the up-facing
11 area IS an end face, so that end is the one that gets glued.
12* Glue is a level that ramps up while the nozzle is held in place (observed 0 -> 0.2 -> 0.4
13 -> 1.0 over the ~4-action dwell that MoveTo performs at its target).
14* Two blocks whose opposing end faces come into contact, with glue on them, fuse into one
15 rigid body and BOTH glue levels are consumed (observed 1.0 -> 0.0 at the weld instant).
16 Observed welds formed on real contact, not merely on being placed 1.3 mm apart.
17
18Glue levels and welds live in model memory (observation-driven); the welds are realised in
19the engine through `restore_model_attachments`, which is what makes a welded assembly move
20and collision-check as one held body.
21"""
22
23import numpy as np # noqa: F401 (also pre-injected)
24
25from predicators.code_sim_learning.fit_space import ParamSpec
26
27BLOCK_TYPE = "block"
28BOTTLE_TYPE = "bottle"
29FACES = ("top", "end_a", "end_b")
30_HALF = np.array([0.05, 0.025, 0.025])
31
32
33def _rot(roll, pitch, yaw):
34 """World-from-body rotation matrix for the recorded euler triple."""
35 cr, sr = np.cos(roll), np.sin(roll)
36 cp, sp = np.cos(pitch), np.sin(pitch)
37 cy, sy = np.cos(yaw), np.sin(yaw)
38 rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]])
39 ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]])
40 rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
41 return rz @ ry @ rx
42
43
44def _block_frame(feat):
45 """Return (centre, rotation, half-extents) for one observed block."""
46 centre = np.array([feat["x"], feat["y"], feat["z"]])
47 rot = _rot(feat["roll"], feat["pitch"], feat["yaw"])
48 half = np.array([feat.get("half_x", 0.05),
49 feat.get("half_y", 0.025),
50 feat.get("half_z", 0.025)])
51 return centre, rot, half
52
53
54def _up_axis(rot):
55 """Which local axis points most nearly straight up, and its sign."""
56 col_z = rot[2, :] # world-z component of each local axis
57 axis = int(np.argmax(np.abs(col_z)))
58 return axis, float(np.sign(col_z[axis]) or 1.0)
59
60
61def _dab_face(feat, nozzle, params):
62 """Face name the nozzle would glue, or None.
63
64 The nozzle must sit above the block's up-facing surface, within `dab_height` of it and
65 inside its lateral extent (plus `dab_radius` of slop). The face is the top face unless
66 the hit point lies in the outer `1 - end_frac` fraction of the long (local x) axis, in
67 which case it is the corresponding end face. When the up-facing surface is itself an
68 end face (a block standing on end), that end face is the one glued.
69 """
70 centre, rot, half = _block_frame(feat)
71 axis, sign = _up_axis(rot)
72 local = rot.T @ (nozzle - centre)
73 height = sign * local[axis] - half[axis]
74 if height < -1e-4 or height > params["dab_height"]:
75 return None
76 # lateral containment in the other two axes
77 for other in range(3):
78 if other == axis:
79 continue
80 if abs(local[other]) > half[other] + params["dab_radius"]:
81 return None
82 if axis == 0:
83 return "end_b" if sign > 0 else "end_a"
84 # up-facing area is a long face: pick top vs end from the position along local x
85 if local[0] > params["end_frac"] * half[0]:
86 return "end_b"
87 if local[0] < -params["end_frac"] * half[0]:
88 return "end_a"
89 return "top"
90
91
92def _end_point(feat, face):
93 """World position of the centre of an end face, and its outward normal."""
94 centre, rot, half = _block_frame(feat)
95 sign = 1.0 if face == "end_b" else -1.0
96 normal = sign * rot[:, 0]
97 return centre + normal * half[0], normal
98
99
100def _observed(observation, type_name):
101 out = {}
102 for obj in observation:
103 if obj.type.name != type_name:
104 continue
105 out[obj.name] = {
106 f: float(observation.get(obj, f))
107 for f in obj.type.feature_names
108 }
109 return out
110
111
112class BridgeGlueWeld(BaseSimulator): # noqa: F821 (injected by the loader)
113
114 AGENT_PARAM_SPECS = [
115 # how fast a held nozzle deposits glue (level per action)
116 # directly observed on level 1: the reading stepped 0 -> 0.2 -> 0.4 -> 1.0, so the
117 # per-action deposit is ~0.2; the bounds keep the pose-residual fit from running away
118 # to a degenerate value that the pose data cannot actually see.
119 ParamSpec("glue_rate", 0.2, lo=0.05, hi=0.6),
120 # lateral slop around the up-facing area that still catches the dab
121 ParamSpec("dab_radius", 0.015, lo=0.002, hi=0.05),
122 # how far above the surface the nozzle may sit and still deposit
123 ParamSpec("dab_height", 0.02, lo=0.002, hi=0.06),
124 # fraction of the half-length beyond which a dab counts as an end, not the top
125 ParamSpec("end_frac", 0.55, lo=0.2, hi=0.95),
126 # largest face-to-face gap that still cures into a weld
127 ParamSpec("weld_gap", 0.006, lo=0.001, hi=0.025),
128 # largest lateral (off-axis) offset of two mating end faces that still cures
129 ParamSpec("weld_lateral", 0.02, lo=0.005, hi=0.045),
130 # glue level required before a contact cures
131 ParamSpec("cure_level", 0.15, lo=0.05, hi=0.9),
132 # nozzle offset below the bottle origin
133 ParamSpec("nozzle_drop", 0.03, lo=0.01, hi=0.05),
134 ]
135
136 # Scored quantities: the block poses, which is what the welds actually change.
137 RESIDUAL_FEATURES = {BLOCK_TYPE: ["x", "y", "z", "roll", "pitch", "yaw"]}
138
139 MODEL_STATE_INIT = {"glue": {}, "welds": [], "seen": False}
140
141 # ---------------------------------------------------------------- memory
142
143 # Generous slack used only to decide whether a REMEMBERED weld is still plausible.
144 # It must be looser than the curing tolerances so observation noise on a genuinely
145 # welded pair can never prune it.
146 STALE_GAP = 0.035
147 STALE_LATERAL = 0.045
148
149 @classmethod
150 def _pair_adjacency(cls, fa, fb):
151 """Return (face_a, face_b) if the two blocks sit end-to-end, else None."""
152 best = None
153 for face_a in ("end_a", "end_b"):
154 pa, na = _end_point(fa, face_a)
155 for face_b in ("end_a", "end_b"):
156 pb, nb = _end_point(fb, face_b)
157 if float(na @ nb) > -0.85:
158 continue
159 delta = pb - pa
160 gap = abs(float(delta @ na))
161 lateral = float(np.linalg.norm(delta - (delta @ na) * na))
162 if gap <= cls.STALE_GAP and lateral <= cls.STALE_LATERAL:
163 if best is None or gap < best[2]:
164 best = (face_a, face_b, gap)
165 return None if best is None else (best[0], best[1])
166
167 @classmethod
168 def update_model_state(cls, observation, model_state, params, action):
169 blocks = _observed(observation, BLOCK_TYPE)
170 bottles = _observed(observation, BOTTLE_TYPE)
171 glue = model_state.setdefault("glue", {})
172 welds = model_state.setdefault("welds", [])
173 for name in blocks:
174 glue.setdefault(name, {f: 0.0 for f in FACES})
175
176 # 0. consistency: a weld is rigid, so its two blocks must still be end-adjacent in
177 # the current observation. Any remembered pair that is not is stale (e.g. memory
178 # carried across episodes, or a joint that never really took) and is dropped.
179 kept = []
180 for pair in welds:
181 a, b = pair[0], pair[1]
182 if a not in blocks or b not in blocks:
183 continue
184 if cls._pair_adjacency(blocks[a], blocks[b]) is not None:
185 kept.append(pair)
186 welds[:] = kept
187
188 # 1. trust the environment's own glue readings when they are present: they are
189 # observable features, so tracking a real episode needs no integration at all.
190 for name, feat in blocks.items():
191 for face in FACES:
192 key = "glue_" + face
193 if key in feat:
194 glue[name][face] = max(glue[name][face], float(feat[key]))
195
196 # 2. dispense from a held bottle onto the surface under the nozzle.
197 for feat in bottles.values():
198 if feat.get("is_held", 0.0) < 0.5:
199 continue
200 nozzle = np.array([feat["x"], feat["y"],
201 feat["z"] - params["nozzle_drop"]])
202 for name, bfeat in blocks.items():
203 if bfeat.get("is_held", 0.0) > 0.5:
204 continue
205 face = _dab_face(bfeat, nozzle, params)
206 if face is not None:
207 glue[name][face] = min(1.0, glue[name][face]
208 + params["glue_rate"])
209
210 # 3. cure: opposing end faces in contact with glue on both fuse, consuming the glue.
211 names = sorted(blocks)
212 existing = {tuple(sorted(p)) for p in welds}
213 for i, a in enumerate(names):
214 for b in names[i + 1:]:
215 if tuple(sorted((a, b))) in existing:
216 continue
217 for fa in ("end_a", "end_b"):
218 pa, na = _end_point(blocks[a], fa)
219 for fb in ("end_a", "end_b"):
220 pb, nb = _end_point(blocks[b], fb)
221 if float(na @ nb) > -0.9: # faces must oppose
222 continue
223 delta = pb - pa
224 gap = abs(float(delta @ na))
225 lateral = float(np.linalg.norm(delta - (delta @ na) * na))
226 if gap > params["weld_gap"]:
227 continue
228 if lateral > params["weld_lateral"]:
229 continue
230 if (glue[a][fa] < params["cure_level"]
231 or glue[b][fb] < params["cure_level"]):
232 continue
233 welds.append([a, b])
234 existing.add(tuple(sorted((a, b))))
235 glue[a][fa] = 0.0
236 glue[b][fb] = 0.0
237 break
238 else:
239 continue
240 break
241 model_state["seen"] = True
242
243 # ------------------------------------------------------------- dynamics
244
245 def _weld_pairs(self):
246 """Remembered welds, gated on the pair still being end-adjacent right now.
247
248 The gate matters because remembered welds can be stale (memory reconstructed over a
249 different episode). A weld is rigid, so a remembered pair that is nowhere near
250 end-to-end in the live scene cannot be real and must not be realised in the engine.
251 """
252 state = self.model_state or {}
253 pairs = [(p[0], p[1]) for p in state.get("welds", [])]
254 if not pairs:
255 return []
256 try:
257 blocks = _observed(self.get_observation(), BLOCK_TYPE)
258 except Exception: # pragma: no cover - keep raw memory if unreadable
259 return pairs
260 out = []
261 for a, b in pairs:
262 if a in blocks and b in blocks:
263 if self._pair_adjacency(blocks[a], blocks[b]) is not None:
264 out.append((a, b))
265 return out
266
267 def _publish_links(self, force=False):
268 """Register the inferred rigid links, but ONLY when the set changes.
269
270 Re-registering an unchanged link every step re-creates the engine constraint at the
271 latest relative transform, which pumps energy into the assembly (observed: a welded
272 beam jittering off the table). Publishing on change only keeps the joint rigid and
273 the rollout stable. The empty set is published too, so a pruned weld disappears.
274 """
275 pairs = self._weld_pairs()
276 key = tuple(sorted(tuple(sorted(p)) for p in pairs))
277 if force or key != getattr(self, "_published_links", None):
278 self.restore_model_attachments(pairs)
279 self._published_links = key
280
281 def _domain_specific_step(self):
282 self._publish_links()
283
284 def restore_model_state(self):
285 self._published_links = None
286 self._publish_links(force=True)
287
288
289RESIDUAL_ENV = BridgeGlueWeld
290RESIDUAL_FEATURES = BridgeGlueWeld.RESIDUAL_FEATURES