Download algorithms/phase1.py from LinJinwei/XY-VLA: direct link, hf CLI and curl.
- Browser
- Download file 35.3 kB
-
https://huggingface.co/LinJinwei/XY-VLA/resolve/main/algorithms/phase1.py
- Command line
-
hf download hf://LinJinwei/XY-VLA/algorithms/phase1.py
-
curl -L -o phase1.py https://huggingface.co/LinJinwei/XY-VLA/resolve/main/algorithms/phase1.py
35.3 kB
| from __future__ import annotations | |
| from pathlib import Path | |
| from typing import Any, Callable | |
| import numpy as np | |
| from .instruction_parse import ( | |
| extract_ordered_mission_billboard_ids, | |
| extract_ordered_traversal_billboard_ids, | |
| ) | |
| from .portal_geometry import ( | |
| compute_leg_corridor_xy, | |
| expand_corridor_xy, | |
| is_portal_object, | |
| portal_opening_center_world, | |
| ) | |
| from .phase2 import run_navigation_phase2_topdown_xvla_xy_plan | |
| from .phase_recording import ( | |
| PHASE2_DIR, | |
| STEREO45_PNG, | |
| TOPDOWN_PNG, | |
| draw_portal_number_labels_overlay_bgr, | |
| resolve_experiment_base_folder, | |
| resolve_phase_folder, | |
| save_bgr_views, | |
| ) | |
| from .phase3 import ( | |
| PHASE3_RGBA_IDLE, | |
| Phase3FeedbackZone, | |
| Phase3FeedbackZones, | |
| collect_phase3_feedback_entries, | |
| entry_key, | |
| ) | |
| def _placed_obj_nav_kind(c: dict) -> str: | |
| sh = str(c.get("shape", "cube")).lower() | |
| if sh in ("rect_frame", "square_frame", "frame"): | |
| return "gate" | |
| if sh == "sphere": | |
| return "sphere" | |
| if sh == "ramp": | |
| return "ramp" | |
| return "cube" | |
| def _placed_object_aabb_lo_hi_world(c: dict) -> tuple[np.ndarray, np.ndarray]: | |
| pos = np.asarray(c["pos"], dtype=np.float64).reshape(3) | |
| bh = c.get("bounds_half") | |
| if bh is not None: | |
| h = np.asarray(bh, dtype=np.float64).reshape(3) | |
| return pos - h, pos + h | |
| half = float(c.get("half", 0.025)) | |
| h = np.array([half, half, half], dtype=np.float64) | |
| return pos - h, pos + h | |
| def _phase1_include_placed_obj(c: dict) -> bool: | |
| return _placed_obj_nav_kind(c) in ("gate", "cube", "sphere", "ramp") | |
| def _placed_obj_display_name(c: dict) -> str: | |
| col = str(c.get("color_name", c.get("color", "?"))) | |
| return f"{col} {_placed_obj_nav_kind(c)}" | |
| def _placed_object_pyb_aabb_pair(c: dict) -> tuple[list[float], list[float]]: | |
| lo, hi = _placed_object_aabb_lo_hi_world(c) | |
| return lo.tolist(), hi.tolist() | |
| def _draw_phase1_detection_overlay_bgr( | |
| img_bgr: np.ndarray, | |
| *, | |
| wproj: Callable[[float, float, float], tuple[int, int] | None], | |
| x_min: float, | |
| y_min: float, | |
| x_max: float, | |
| y_max: float, | |
| z_ground: float, | |
| start_xy: np.ndarray, | |
| start_wz: float, | |
| target_xyz: np.ndarray, | |
| obstacle_centers: list[tuple[float, float, float]], | |
| target_label: str = "TARGET", | |
| ) -> None: | |
| import cv2 | |
| rect_w = [ | |
| wproj(x_min, y_min, z_ground), | |
| wproj(x_max, y_min, z_ground), | |
| wproj(x_max, y_max, z_ground), | |
| wproj(x_min, y_max, z_ground), | |
| ] | |
| if all(pt is not None for pt in rect_w): | |
| poly = np.array([[int(p[0]), int(p[1])] for p in rect_w], dtype=np.int32).reshape((-1, 1, 2)) | |
| cv2.polylines( | |
| img_bgr, | |
| [poly], | |
| isClosed=True, | |
| color=(144, 238, 144), | |
| thickness=3, | |
| lineType=cv2.LINE_AA, | |
| ) | |
| sxy = np.asarray(start_xy, dtype=np.float64).reshape(-1) | |
| txyz = np.asarray(target_xyz, dtype=np.float64).reshape(-1) | |
| sp = wproj(float(sxy[0]), float(sxy[1]), float(start_wz)) | |
| tp = wproj(float(txyz[0]), float(txyz[1]), float(txyz[2])) | |
| if sp is not None: | |
| cv2.circle(img_bgr, sp, 8, (0, 255, 0), -1) | |
| cv2.putText( | |
| img_bgr, | |
| "START", | |
| (sp[0] - 30, sp[1] - 10), | |
| cv2.FONT_HERSHEY_SIMPLEX, | |
| 0.5, | |
| (0, 255, 0), | |
| 1, | |
| cv2.LINE_AA, | |
| ) | |
| if tp is not None: | |
| cv2.circle(img_bgr, tp, 8, (255, 0, 255), -1) | |
| cv2.putText( | |
| img_bgr, | |
| target_label, | |
| (tp[0] - 30, tp[1] + 15), | |
| cv2.FONT_HERSHEY_SIMPLEX, | |
| 0.5, | |
| (255, 0, 255), | |
| 1, | |
| cv2.LINE_AA, | |
| ) | |
| for i, (px, py, pz) in enumerate(obstacle_centers): | |
| opc = wproj(float(px), float(py), float(pz)) | |
| if opc is None: | |
| continue | |
| cv2.circle(img_bgr, opc, 6, (0, 165, 255), -1) | |
| cv2.putText( | |
| img_bgr, | |
| f"OBJ{i+1}", | |
| (opc[0] - 20, opc[1]), | |
| cv2.FONT_HERSHEY_SIMPLEX, | |
| 0.4, | |
| (0, 165, 255), | |
| 1, | |
| cv2.LINE_AA, | |
| ) | |
| def setup_phase1_feedback_spheres( | |
| p: Any, | |
| intersected_objects: list[dict[str, Any]], | |
| target_obj_info: dict[str, Any] | None, | |
| *, | |
| registry: Phase3FeedbackZones | None = None, | |
| feedback_margin_m: float | None = None, | |
| ) -> Phase3FeedbackZones: | |
| from .phase3 import ( | |
| DEFAULT_FEEDBACK_MARGIN_M, | |
| PHASE3_RGBA_ALPHA, | |
| apply_feedback_sphere_visual, | |
| create_feedback_sphere_visual_shape, | |
| ) | |
| manager = registry if registry is not None else Phase3FeedbackZones() | |
| existing = manager.zone_keys() | |
| entries = collect_phase3_feedback_entries( | |
| intersected_objects, | |
| target_obj_info, | |
| feedback_margin_m=float( | |
| DEFAULT_FEEDBACK_MARGIN_M if feedback_margin_m is None else feedback_margin_m | |
| ), | |
| ) | |
| new_entries = [ | |
| e | |
| for e in entries | |
| if entry_key(str(e.get("name", "?")), np.asarray(e["center"])) not in existing | |
| ] | |
| if not new_entries: | |
| if not entries: | |
| print("[Phase 1] No valid objects in detection rectangle; skipping feedback spheres.") | |
| return manager | |
| print( | |
| f"\n[Phase 1] Creating feedback spheres for {len(new_entries)} detected object(s) " | |
| f"(light blue {PHASE3_RGBA_ALPHA * 100:.1f}% opacity)" | |
| ) | |
| for entry in new_entries: | |
| center = np.asarray(entry["center"], dtype=np.float64).reshape(3) | |
| radius = float(entry["radius"]) | |
| name = str(entry.get("name", "object")) | |
| is_target = bool(entry.get("is_target", False)) | |
| vis = create_feedback_sphere_visual_shape(p, radius, PHASE3_RGBA_IDLE) | |
| body_uid = p.createMultiBody( | |
| baseMass=0, | |
| baseVisualShapeIndex=vis, | |
| basePosition=center.tolist(), | |
| ) | |
| tag = " (target)" if is_target else "" | |
| print( | |
| f" - {name}{tag}: center={center.round(3).tolist()} " | |
| f"radius={radius:.3f}m" | |
| ) | |
| zone = Phase3FeedbackZone( | |
| name=name, | |
| center=center, | |
| radius=radius, | |
| is_target=is_target, | |
| body_uid=int(body_uid), | |
| entered=False, | |
| aabb=entry.get("aabb"), | |
| ) | |
| apply_feedback_sphere_visual(p, zone) | |
| manager.zones.append(zone) | |
| print(f"[Phase 1] Feedback spheres created: {manager.num_zones} total.") | |
| return manager | |
| def compute_phase1_corridor_and_obstacles( | |
| *, | |
| drone_pos: np.ndarray, | |
| g_world: np.ndarray, | |
| placed_cubes: list[dict], | |
| mission_cmd: str | None, | |
| cur_instruction: str, | |
| first_rect_portal_for_instruction_color_fn: Callable[..., Any | None], | |
| target_portal_ref: dict | None = None, | |
| corridor_margin_m: float = 0.0, | |
| corridor_bandwidth_m: float = 0.12, | |
| feedback_radius_m: float = 0.0, | |
| ) -> dict[str, Any]: | |
| start_xy = np.asarray(drone_pos, dtype=np.float64).reshape(-1)[:2] | |
| target_xy = np.asarray(g_world, dtype=np.float64).reshape(-1)[:2] | |
| portal_ref_early = target_portal_ref | |
| if portal_ref_early is None: | |
| portal_ref_early = first_rect_portal_for_instruction_color_fn( | |
| str(mission_cmd or cur_instruction or ""), | |
| placed_cubes, | |
| prefer_near_xyz=np.asarray(drone_pos, dtype=np.float64), | |
| ) | |
| fb_r = float(max(0.0, feedback_radius_m)) | |
| if portal_ref_early is not None and fb_r <= 0.0: | |
| bh = portal_ref_early.get("bounds_half") | |
| if bh is not None: | |
| fb_r = float(np.linalg.norm(np.asarray(bh, dtype=np.float64).reshape(3))) * 0.55 + 0.08 | |
| x_min, y_min, x_max, y_max = compute_leg_corridor_xy( | |
| np.asarray(drone_pos, dtype=np.float64).reshape(3), | |
| np.asarray(g_world, dtype=np.float64).reshape(3), | |
| margin_m=float(corridor_margin_m), | |
| bandwidth_m=float(corridor_bandwidth_m), | |
| portal_ref=portal_ref_early, | |
| feedback_radius_m=fb_r, | |
| ) | |
| path2 = target_xy.astype(np.float64) - start_xy.astype(np.float64) | |
| path_len2 = float(np.dot(path2, path2)) | |
| def _phase1_xy_depth_key(cx: float, cy: float) -> tuple[float, float]: | |
| pxy = np.array([cx, cy], dtype=np.float64) - start_xy.astype(np.float64) | |
| if path_len2 < 1e-14: | |
| return (0.0, float(np.linalg.norm(pxy))) | |
| t = float(np.dot(pxy, path2) / path_len2) | |
| proj = path2 * t | |
| perp = float(np.linalg.norm(pxy - proj)) | |
| return (t, perp) | |
| portal_ref = portal_ref_early | |
| if portal_ref is None: | |
| portal_ref = first_rect_portal_for_instruction_color_fn( | |
| str(mission_cmd or cur_instruction or ""), | |
| placed_cubes, | |
| prefer_near_xyz=np.asarray(drone_pos, dtype=np.float64), | |
| ) | |
| target_obj_info = None | |
| target_z = float(np.asarray(g_world, dtype=np.float64).reshape(-1)[2]) | |
| cand: list[tuple[float, dict, tuple]] = [] | |
| for obj in placed_cubes: | |
| if not _phase1_include_placed_obj(obj): | |
| continue | |
| aabb = _placed_object_pyb_aabb_pair(obj) | |
| mn = np.asarray(aabb[0], dtype=np.float64) | |
| mx = np.asarray(aabb[1], dtype=np.float64) | |
| if not (mn[0] <= g_world[0] <= mx[0] and mn[1] <= g_world[1] <= mx[1]): | |
| continue | |
| cvec = (mn + mx) * 0.5 | |
| d3 = float(np.linalg.norm(cvec - np.asarray(g_world, dtype=np.float64))) | |
| cand.append((d3, obj, aabb)) | |
| if portal_ref is not None: | |
| aabb = _placed_object_pyb_aabb_pair(portal_ref) | |
| mn = np.asarray(aabb[0], dtype=np.float64) | |
| mx = np.asarray(aabb[1], dtype=np.float64) | |
| oc = portal_opening_center_world(portal_ref) | |
| target_obj_info = { | |
| "name": _placed_obj_display_name(portal_ref), | |
| "ref": portal_ref, | |
| "center": [float(oc[0]), float(oc[1]), float(oc[2])], | |
| "aabb": aabb, | |
| "kind": "gate", | |
| } | |
| elif cand: | |
| cand.sort(key=lambda x: x[0]) | |
| _d3, obj, aabb = cand[0] | |
| mn = np.asarray(aabb[0], dtype=np.float64) | |
| mx = np.asarray(aabb[1], dtype=np.float64) | |
| target_obj_info = { | |
| "name": _placed_obj_display_name(obj), | |
| "ref": obj, | |
| "center": [ | |
| float((mn[0] + mx[0]) * 0.5), | |
| float((mn[1] + mx[1]) * 0.5), | |
| float((mn[2] + mx[2]) * 0.5), | |
| ], | |
| "aabb": aabb, | |
| } | |
| target_ref_p1: dict | None = target_obj_info["ref"] if target_obj_info else None | |
| intersected_objects = [] | |
| for obj in placed_cubes: | |
| if not _phase1_include_placed_obj(obj): | |
| continue | |
| if target_ref_p1 is not None and obj is target_ref_p1: | |
| continue | |
| aabb = _placed_object_pyb_aabb_pair(obj) | |
| obj_x_min, obj_y_min = aabb[0][0], aabb[0][1] | |
| obj_x_max, obj_y_max = aabb[1][0], aabb[1][1] | |
| overlap_x = (x_min <= obj_x_max) and (x_max >= obj_x_min) | |
| overlap_y = (y_min <= obj_y_max) and (y_max >= obj_y_min) | |
| if overlap_x and overlap_y: | |
| cx = (obj_x_min + obj_x_max) / 2.0 | |
| cy = (obj_y_min + obj_y_max) / 2.0 | |
| cz = (aabb[0][2] + aabb[1][2]) / 2.0 | |
| t_key, perp_key = _phase1_xy_depth_key(cx, cy) | |
| entry: dict[str, Any] = { | |
| "name": _placed_obj_display_name(obj), | |
| "dist": float(np.linalg.norm(np.array([cx, cy]) - start_xy)), | |
| "path_t": t_key, | |
| "path_perp": perp_key, | |
| "aabb": aabb, | |
| "center": [cx, cy, cz], | |
| "ref": obj, | |
| } | |
| if is_portal_object(obj): | |
| entry["kind"] = "gate" | |
| intersected_objects.append(entry) | |
| intersected_objects.sort(key=lambda x: (x["path_t"], x["path_perp"])) | |
| return { | |
| "start_xy": start_xy, | |
| "corridor_xy": (float(x_min), float(y_min), float(x_max), float(y_max)), | |
| "intersected_objects": intersected_objects, | |
| "target_obj_info": target_obj_info, | |
| "target_z": target_z, | |
| } | |
| def run_single_leg_phase1_and_phase2( | |
| *, | |
| p: Any, | |
| placed_cubes: list[dict], | |
| mission_cmd: str | None, | |
| cur_instruction: str, | |
| g_world: np.ndarray, | |
| drone_pos: np.ndarray, | |
| drone_pos_local: np.ndarray, | |
| drone_R: np.ndarray, | |
| virtual_base_world: np.ndarray, | |
| workspace_lo: np.ndarray, | |
| workspace_hi: np.ndarray, | |
| treat_pos_as: str, | |
| delta_pos_scale: float, | |
| gripper_state: float, | |
| server_url: str, | |
| scene_catalog_str: str, | |
| xvla_scene_semantic_context: bool, | |
| xvla_path_planning_instruction_suffix: str, | |
| workspace_camera_width: int, | |
| workspace_camera_height: int, | |
| navigation_phase2_xvla_steps: int, | |
| xvla_act_request_timeout_s: float, | |
| navigation_phase2_sync_root_config: bool, | |
| navigation_phase2_sync_qs: bool, | |
| qs_policy_path_for_sync: Any | None, | |
| config_json_path: Any, | |
| navigation_phase2_extra_instruction: str, | |
| navigation_phase2_geom_astar: bool, | |
| navigation_phase2_astar_cell_m: float, | |
| navigation_phase2_astar_obstacle_pad_m: float, | |
| navigation_phase1_corridor_margin_m: float = 0.18, | |
| navigation_phase1_corridor_bandwidth_m: float = 0.12, | |
| navigation_collision_pad_m: float | None = None, | |
| navigation_phase2_optional_topdown_xvla: bool, | |
| navigation_phase2_z_clearance_enabled: bool, | |
| navigation_phase2_z_clearance_margin_m: float, | |
| navigation_phase2_z_workspace_margin_m: float, | |
| recording_scene_fov: float, | |
| recording_scene_margin: float, | |
| recording_camera_distance_scale: float, | |
| recording_topview_distance_scale: float, | |
| recording_stereo45_distance_scale: float = 1.5, | |
| rec_folder: Any | None, | |
| recording_folder: Any | None, | |
| root_dir: Any, | |
| world_recording_view_proj_fn: Callable[..., tuple[list[float], list[float]]], | |
| render_world_recording_rgb_fn: Callable[..., np.ndarray], | |
| world_xyz_to_recording_image_pixel_fn: Callable[ | |
| [np.ndarray, list[float], list[float], int, int], tuple[int, int] | None | |
| ], | |
| first_rect_portal_for_instruction_color_fn: Callable[..., Any | None], | |
| build_proprio_fn: Callable[[np.ndarray, np.ndarray, float], np.ndarray], | |
| query_xvla_fn: Callable[..., Any], | |
| compose_instruction_fn: Callable[..., str], | |
| read_config_fn: Callable[[Any], dict[str, Any]], | |
| load_qs_policies_fn: Callable[[Any], list[dict[str, str]]], | |
| leg_subfolder: str | None = None, | |
| target_portal_ref: dict | None = None, | |
| phase2_sidecar_name: str = "navigation_phase2_xy.json", | |
| phase2_png_filename: str = TOPDOWN_PNG, | |
| phase2_stereo_png_filename: str = STEREO45_PNG, | |
| leg_index: int | None = None, | |
| billboard_id: int | None = None, | |
| sync_root_config: bool | None = None, | |
| sync_qs: bool | None = None, | |
| pause_message: bool = False, | |
| create_feedback_spheres: bool = True, | |
| feedback_zones_registry: Phase3FeedbackZones | None = None, | |
| ) -> dict[str, Any] | None: | |
| leg_tag = "" | |
| if leg_index is not None: | |
| leg_tag = f" [leg {leg_index}]" | |
| if billboard_id is not None: | |
| leg_tag += f" billboard_id={billboard_id}" | |
| print("\n" + "=" * 60) | |
| print(f"[Phase 1]{leg_tag} top-down detection rectangle") | |
| p1 = compute_phase1_corridor_and_obstacles( | |
| drone_pos=drone_pos, | |
| g_world=g_world, | |
| placed_cubes=placed_cubes, | |
| mission_cmd=mission_cmd, | |
| cur_instruction=cur_instruction, | |
| first_rect_portal_for_instruction_color_fn=first_rect_portal_for_instruction_color_fn, | |
| target_portal_ref=target_portal_ref, | |
| corridor_margin_m=float(navigation_phase1_corridor_margin_m), | |
| corridor_bandwidth_m=float(navigation_phase1_corridor_bandwidth_m), | |
| feedback_radius_m=0.0, | |
| ) | |
| start_xy = p1["start_xy"] | |
| x_min, y_min, x_max, y_max = p1["corridor_xy"] | |
| intersected_objects = p1["intersected_objects"] | |
| target_obj_info = p1["target_obj_info"] | |
| target_z = p1["target_z"] | |
| print(f"[Phase 1] start XY: {start_xy.round(3).tolist()}") | |
| print( | |
| f"[Phase 1] start XYZ (drone): " | |
| f"{np.asarray(drone_pos, dtype=np.float64).reshape(-1)[:3].round(3).tolist()}" | |
| ) | |
| print(f"[Phase 1] goal XY: {np.asarray(g_world).reshape(-1)[:2].round(3).tolist()}") | |
| print(f"[Phase 1] detection rect [X] {x_min:.3f} to {x_max:.3f}, [Y] {y_min:.3f} to {y_max:.3f}") | |
| print("\n[Phase 1] goal point geometry:") | |
| print(f" goal XYZ: {np.asarray(g_world).round(3).tolist()}") | |
| print(f" goal Z height: {target_z:.3f}m") | |
| if target_obj_info is not None: | |
| print(f" -> goal object: {target_obj_info['name']}") | |
| print(f" object center XYZ: {np.array(target_obj_info['center']).round(3).tolist()}") | |
| print( | |
| f"\n[Phase 1] {len(intersected_objects)} obstacle(s) in detection rectangle " | |
| f"(encounter order start->goal; goal object excluded):" | |
| ) | |
| for i, obj in enumerate(intersected_objects): | |
| print( | |
| f" {i+1}. {obj['name']} | path_t={obj['path_t']:.3f} " | |
| f"perp={obj['path_perp']:.3f}m | dist_from_start={obj['dist']:.3f}m" | |
| ) | |
| try: | |
| import cv2 | |
| tw, th = 800, 800 | |
| view_m, proj_m = world_recording_view_proj_fn( | |
| p, | |
| workspace_lo + virtual_base_world, | |
| workspace_hi + virtual_base_world, | |
| view_kind="top", | |
| width=tw, | |
| height=th, | |
| fov=float(recording_scene_fov), | |
| margin=float(recording_scene_margin), | |
| distance_scale=float(recording_camera_distance_scale), | |
| top_view_distance_scale=float(recording_topview_distance_scale), | |
| ) | |
| topdown_img = render_world_recording_rgb_fn(p, view_m, proj_m, width=tw, height=th) | |
| experiment_base = resolve_experiment_base_folder( | |
| rec_folder=rec_folder, | |
| recording_folder=recording_folder, | |
| root_dir=root_dir, | |
| ) | |
| phase1_folder = resolve_phase_folder( | |
| experiment_base, "phase1", leg_subfolder=leg_subfolder | |
| ) | |
| phase2_folder = resolve_phase_folder( | |
| experiment_base, PHASE2_DIR, leg_subfolder=leg_subfolder | |
| ) | |
| phase3_folder = resolve_phase_folder(experiment_base, "phase3", mkdir=False) | |
| z_ground = float( | |
| np.asarray(workspace_lo, dtype=np.float64)[2] | |
| + np.asarray(virtual_base_world, dtype=np.float64)[2] | |
| ) | |
| def wproj(wx: float, wy: float, wz: float) -> tuple[int, int] | None: | |
| return world_xyz_to_recording_image_pixel_fn( | |
| np.array([wx, wy, wz], dtype=np.float64), | |
| view_m, | |
| proj_m, | |
| width=int(tw), | |
| height=int(th), | |
| ) | |
| img_marked = cv2.cvtColor(topdown_img, cv2.COLOR_RGB2BGR) | |
| start_wz = float(np.asarray(drone_pos, dtype=np.float64).reshape(-1)[2]) | |
| gv = np.asarray(g_world, dtype=np.float64).reshape(-1) | |
| obs_centers_g = [ | |
| (float(o["center"][0]), float(o["center"][1]), float(o["center"][2])) | |
| for o in intersected_objects | |
| ] | |
| tgt_lbl = "TARGET" | |
| if billboard_id is not None: | |
| tgt_lbl = f"EXIT id={billboard_id}" | |
| _draw_phase1_detection_overlay_bgr( | |
| img_marked, | |
| wproj=wproj, | |
| x_min=float(x_min), | |
| y_min=float(y_min), | |
| x_max=float(x_max), | |
| y_max=float(y_max), | |
| z_ground=z_ground, | |
| start_xy=start_xy, | |
| start_wz=start_wz, | |
| target_xyz=gv, | |
| obstacle_centers=obs_centers_g, | |
| target_label=tgt_lbl, | |
| ) | |
| draw_portal_number_labels_overlay_bgr(img_marked, wproj, placed_cubes) | |
| view_stereo_g, proj_stereo_g = world_recording_view_proj_fn( | |
| p, | |
| workspace_lo + virtual_base_world, | |
| workspace_hi + virtual_base_world, | |
| view_kind="45deg", | |
| width=tw, | |
| height=th, | |
| fov=float(recording_scene_fov), | |
| margin=float(recording_scene_margin), | |
| distance_scale=float(recording_camera_distance_scale), | |
| top_view_distance_scale=float(recording_topview_distance_scale), | |
| stereo45_view_distance_scale=float(recording_stereo45_distance_scale), | |
| ) | |
| stereo_rgb_g = render_world_recording_rgb_fn( | |
| p, view_stereo_g, proj_stereo_g, width=tw, height=th | |
| ) | |
| img_stereo_marked_g = cv2.cvtColor(stereo_rgb_g, cv2.COLOR_RGB2BGR) | |
| def wproj_stereo_g(wx: float, wy: float, wz: float) -> tuple[int, int] | None: | |
| return world_xyz_to_recording_image_pixel_fn( | |
| np.array([wx, wy, wz], dtype=np.float64), | |
| view_stereo_g, | |
| proj_stereo_g, | |
| width=int(tw), | |
| height=int(th), | |
| ) | |
| _draw_phase1_detection_overlay_bgr( | |
| img_stereo_marked_g, | |
| wproj=wproj_stereo_g, | |
| x_min=float(x_min), | |
| y_min=float(y_min), | |
| x_max=float(x_max), | |
| y_max=float(y_max), | |
| z_ground=z_ground, | |
| start_xy=start_xy, | |
| start_wz=start_wz, | |
| target_xyz=gv, | |
| obstacle_centers=obs_centers_g, | |
| target_label=tgt_lbl, | |
| ) | |
| det_name = TOPDOWN_PNG | |
| phase1_saved = save_bgr_views(phase1_folder, img_marked, img_stereo_marked_g, topdown_name=det_name) | |
| print(f"\n[Phase 1] phase1 recordings saved (light-green rect = detection corridor):") | |
| print(f" top-down: {phase1_saved.get('topdown')}") | |
| if "stereo45deg" in phase1_saved: | |
| print(f" stereo 45deg: {phase1_saved['stereo45deg']}") | |
| phase3_entries: list[dict[str, Any]] = [] | |
| phase3_zones: Phase3FeedbackZones | None = feedback_zones_registry | |
| try: | |
| phase3_entries = collect_phase3_feedback_entries( | |
| intersected_objects, | |
| target_obj_info, | |
| ) | |
| if create_feedback_spheres and phase3_entries: | |
| phase3_zones = setup_phase1_feedback_spheres( | |
| p, | |
| intersected_objects, | |
| target_obj_info, | |
| registry=feedback_zones_registry, | |
| ) | |
| except Exception as pe1fb: | |
| print(f"[Phase 1] WARN: feedback sphere setup failed - {pe1fb}") | |
| obs_lines_parts_gcam: list[str] = [] | |
| for idx, obj in enumerate(intersected_objects): | |
| aabb_o = obj["aabb"] | |
| lo_c = np.asarray(aabb_o[0], dtype=np.float64) | |
| hi_c = np.asarray(aabb_o[1], dtype=np.float64) | |
| nm = str(obj.get("name", f"obj_{idx + 1}")) | |
| obs_lines_parts_gcam.append( | |
| f" - {nm}: XY low [{float(lo_c[0]):.4f},{float(lo_c[1]):.4f}] " | |
| f"high [{float(hi_c[0]):.4f},{float(hi_c[1]):.4f}]" | |
| ) | |
| obs_lines_txt_gcam = ( | |
| "\n".join(obs_lines_parts_gcam) | |
| if obs_lines_parts_gcam | |
| else " (none — corridor contains only the goal footprint)" | |
| ) | |
| do_sync_cfg = ( | |
| bool(sync_root_config) | |
| if sync_root_config is not None | |
| else bool(navigation_phase2_sync_root_config) | |
| ) | |
| do_sync_qs = ( | |
| bool(sync_qs) if sync_qs is not None else bool(navigation_phase2_sync_qs) | |
| ) | |
| print("\n" + "=" * 60) | |
| print(f"[Phase 2]{leg_tag} corridor + A* coarse path...") | |
| p2 = run_navigation_phase2_topdown_xvla_xy_plan( | |
| server_url=server_url, | |
| topdown_rgb=topdown_img, | |
| img_marked_bgr=img_marked, | |
| view_m=view_m, | |
| proj_m=proj_m, | |
| render_width=tw, | |
| render_height=th, | |
| phase2_folder=phase2_folder, | |
| png_filename=str(phase2_png_filename), | |
| stereo_img_marked_bgr=img_stereo_marked_g, | |
| stereo_view_m=view_stereo_g, | |
| stereo_proj_m=proj_stereo_g, | |
| stereo_png_filename=str(phase2_stereo_png_filename), | |
| drone_pos_world=np.asarray(drone_pos, dtype=np.float32), | |
| drone_pos_local=np.asarray(drone_pos_local, dtype=np.float32), | |
| drone_R=drone_R, | |
| virtual_base_world=virtual_base_world, | |
| workspace_lo=workspace_lo, | |
| workspace_hi=workspace_hi, | |
| treat_pos_as=treat_pos_as, | |
| delta_pos_scale=delta_pos_scale, | |
| gripper_state=gripper_state, | |
| mission_cmd=mission_cmd, | |
| language_instruction=cur_instruction, | |
| obs_lines_text=obs_lines_txt_gcam, | |
| corridor_xy=(float(x_min), float(y_min), float(x_max), float(y_max)), | |
| goal_xyz_world=np.asarray(g_world, dtype=np.float64), | |
| scene_catalog_str=scene_catalog_str, | |
| xvla_scene_semantic_context=bool(xvla_scene_semantic_context), | |
| xvla_path_planning_instruction_suffix=xvla_path_planning_instruction_suffix, | |
| workspace_camera_width=int(workspace_camera_width), | |
| workspace_camera_height=int(workspace_camera_height), | |
| phase2_steps=int(navigation_phase2_xvla_steps), | |
| xvla_act_request_timeout_s=float(xvla_act_request_timeout_s), | |
| sync_root_config=do_sync_cfg, | |
| sync_qs=do_sync_qs, | |
| qs_policy_path=qs_policy_path_for_sync, | |
| config_json_path=config_json_path, | |
| phase2_extra_instruction=str(navigation_phase2_extra_instruction), | |
| phase1_obstacle_entries=list(intersected_objects), | |
| world_xyz_to_recording_image_pixel_fn=world_xyz_to_recording_image_pixel_fn, | |
| build_proprio_fn=build_proprio_fn, | |
| query_xvla_fn=query_xvla_fn, | |
| compose_instruction_fn=compose_instruction_fn, | |
| read_config_fn=read_config_fn, | |
| load_qs_policies_fn=load_qs_policies_fn, | |
| navigation_phase2_geom_astar=bool(navigation_phase2_geom_astar), | |
| navigation_phase2_astar_cell_m=float(navigation_phase2_astar_cell_m), | |
| navigation_phase2_astar_obstacle_pad_m=float(navigation_phase2_astar_obstacle_pad_m), | |
| navigation_phase2_optional_topdown_xvla=bool(navigation_phase2_optional_topdown_xvla), | |
| navigation_phase2_z_clearance_enabled=bool(navigation_phase2_z_clearance_enabled), | |
| navigation_phase2_z_clearance_margin_m=float(navigation_phase2_z_clearance_margin_m), | |
| navigation_phase2_z_workspace_margin_m=float(navigation_phase2_z_workspace_margin_m), | |
| navigation_collision_pad_m=navigation_collision_pad_m, | |
| phase2_sidecar_name=str(phase2_sidecar_name), | |
| leg_index=leg_index, | |
| billboard_id=billboard_id, | |
| pause_message=pause_message, | |
| placed_cubes=placed_cubes, | |
| ) | |
| return { | |
| "trajectory": p2.get("trajectory"), | |
| "phase2_snapshot": p2.get("snapshot"), | |
| "planner": p2.get("planner"), | |
| "experiment_base": experiment_base, | |
| "phase1_folder": phase1_folder, | |
| "phase2_folder": phase2_folder, | |
| "phase3_folder": phase3_folder, | |
| "img_marked_bgr": img_marked, | |
| "img_stereo_bgr": img_stereo_marked_g, | |
| "wproj": wproj, | |
| "wproj_stereo": wproj_stereo_g, | |
| "intersected_objects": intersected_objects, | |
| "target_obj_info": target_obj_info, | |
| "phase3_entries": phase3_entries, | |
| "phase3_zones": phase3_zones, | |
| } | |
| except Exception as e: | |
| print(f"[Phase 1+2] WARN: leg execution failed - {e}") | |
| return None | |
| def run_navigation_phase1_and_phase2_topdown( | |
| *, | |
| p: Any, | |
| placed_cubes: list[dict], | |
| mission_cmd: str | None, | |
| cur_instruction: str, | |
| g_world: np.ndarray, | |
| drone_pos: np.ndarray, | |
| drone_pos_local: np.ndarray, | |
| drone_R: np.ndarray, | |
| virtual_base_world: np.ndarray, | |
| workspace_lo: np.ndarray, | |
| workspace_hi: np.ndarray, | |
| treat_pos_as: str, | |
| delta_pos_scale: float, | |
| gripper_state: float, | |
| server_url: str, | |
| scene_catalog_str: str, | |
| xvla_scene_semantic_context: bool, | |
| xvla_path_planning_instruction_suffix: str, | |
| workspace_camera_width: int, | |
| workspace_camera_height: int, | |
| navigation_phase2_xvla_steps: int, | |
| xvla_act_request_timeout_s: float, | |
| navigation_phase2_sync_root_config: bool, | |
| navigation_phase2_sync_qs: bool, | |
| qs_policy_path_for_sync: Any | None, | |
| config_json_path: Any, | |
| navigation_phase2_extra_instruction: str, | |
| navigation_phase2_geom_astar: bool, | |
| navigation_phase2_astar_cell_m: float, | |
| navigation_phase2_astar_obstacle_pad_m: float, | |
| navigation_phase1_corridor_margin_m: float = 0.18, | |
| navigation_phase1_corridor_bandwidth_m: float = 0.12, | |
| navigation_collision_pad_m: float | None = None, | |
| navigation_phase2_optional_topdown_xvla: bool, | |
| navigation_phase2_z_clearance_enabled: bool, | |
| navigation_phase2_z_clearance_margin_m: float, | |
| navigation_phase2_z_workspace_margin_m: float, | |
| recording_scene_fov: float, | |
| recording_scene_margin: float, | |
| recording_camera_distance_scale: float, | |
| recording_topview_distance_scale: float, | |
| recording_stereo45_distance_scale: float = 1.5, | |
| rec_folder: Any | None, | |
| recording_folder: Any | None, | |
| root_dir: Any, | |
| world_recording_view_proj_fn: Callable[..., tuple[list[float], list[float]]], | |
| render_world_recording_rgb_fn: Callable[..., np.ndarray], | |
| world_xyz_to_recording_image_pixel_fn: Callable[ | |
| [np.ndarray, list[float], list[float], int, int], tuple[int, int] | None | |
| ], | |
| first_rect_portal_for_instruction_color_fn: Callable[..., Any | None], | |
| build_proprio_fn: Callable[[np.ndarray, np.ndarray, float], np.ndarray], | |
| query_xvla_fn: Callable[..., Any], | |
| compose_instruction_fn: Callable[..., str], | |
| read_config_fn: Callable[[Any], dict[str, Any]], | |
| load_qs_policies_fn: Callable[[Any], list[dict[str, str]]], | |
| portal_leg_goal_fn: Callable[[dict, np.ndarray], np.ndarray] | None = None, | |
| create_feedback_spheres: bool = True, | |
| feedback_zones_registry: Phase3FeedbackZones | None = None, | |
| ) -> dict[str, Any] | None: | |
| instruction = str(mission_cmd or cur_instruction or "") | |
| mission_ids = extract_ordered_mission_billboard_ids(instruction) | |
| billboard_ids = extract_ordered_traversal_billboard_ids(instruction) | |
| action_cache = None | |
| if feedback_zones_registry is not None and feedback_zones_registry.action_cache: | |
| action_cache = dict(feedback_zones_registry.action_cache) | |
| common_kw = dict( | |
| p=p, | |
| placed_cubes=placed_cubes, | |
| mission_cmd=mission_cmd, | |
| cur_instruction=cur_instruction, | |
| g_world=g_world, | |
| drone_pos=drone_pos, | |
| drone_pos_local=drone_pos_local, | |
| drone_R=drone_R, | |
| virtual_base_world=virtual_base_world, | |
| workspace_lo=workspace_lo, | |
| workspace_hi=workspace_hi, | |
| treat_pos_as=treat_pos_as, | |
| delta_pos_scale=delta_pos_scale, | |
| gripper_state=gripper_state, | |
| server_url=server_url, | |
| scene_catalog_str=scene_catalog_str, | |
| xvla_scene_semantic_context=xvla_scene_semantic_context, | |
| xvla_path_planning_instruction_suffix=xvla_path_planning_instruction_suffix, | |
| workspace_camera_width=workspace_camera_width, | |
| workspace_camera_height=workspace_camera_height, | |
| navigation_phase2_xvla_steps=navigation_phase2_xvla_steps, | |
| xvla_act_request_timeout_s=xvla_act_request_timeout_s, | |
| navigation_phase2_sync_root_config=navigation_phase2_sync_root_config, | |
| navigation_phase2_sync_qs=navigation_phase2_sync_qs, | |
| qs_policy_path_for_sync=qs_policy_path_for_sync, | |
| config_json_path=config_json_path, | |
| navigation_phase2_extra_instruction=navigation_phase2_extra_instruction, | |
| navigation_phase2_geom_astar=navigation_phase2_geom_astar, | |
| navigation_phase2_astar_cell_m=navigation_phase2_astar_cell_m, | |
| navigation_phase1_corridor_margin_m=navigation_phase1_corridor_margin_m, | |
| navigation_phase1_corridor_bandwidth_m=navigation_phase1_corridor_bandwidth_m, | |
| navigation_collision_pad_m=navigation_collision_pad_m, | |
| navigation_phase2_astar_obstacle_pad_m=navigation_phase2_astar_obstacle_pad_m, | |
| navigation_phase2_optional_topdown_xvla=navigation_phase2_optional_topdown_xvla, | |
| navigation_phase2_z_clearance_enabled=navigation_phase2_z_clearance_enabled, | |
| navigation_phase2_z_clearance_margin_m=navigation_phase2_z_clearance_margin_m, | |
| navigation_phase2_z_workspace_margin_m=navigation_phase2_z_workspace_margin_m, | |
| recording_scene_fov=recording_scene_fov, | |
| recording_scene_margin=recording_scene_margin, | |
| recording_camera_distance_scale=recording_camera_distance_scale, | |
| recording_topview_distance_scale=recording_topview_distance_scale, | |
| recording_stereo45_distance_scale=recording_stereo45_distance_scale, | |
| rec_folder=rec_folder, | |
| recording_folder=recording_folder, | |
| root_dir=root_dir, | |
| world_recording_view_proj_fn=world_recording_view_proj_fn, | |
| render_world_recording_rgb_fn=render_world_recording_rgb_fn, | |
| world_xyz_to_recording_image_pixel_fn=world_xyz_to_recording_image_pixel_fn, | |
| first_rect_portal_for_instruction_color_fn=first_rect_portal_for_instruction_color_fn, | |
| build_proprio_fn=build_proprio_fn, | |
| query_xvla_fn=query_xvla_fn, | |
| compose_instruction_fn=compose_instruction_fn, | |
| read_config_fn=read_config_fn, | |
| load_qs_policies_fn=load_qs_policies_fn, | |
| ) | |
| if len(mission_ids) >= 2: | |
| from .multi_leg import run_multi_action_navigation_phase12 | |
| multi_action = run_multi_action_navigation_phase12( | |
| mission_cmd=instruction, | |
| phase12_kwargs=common_kw, | |
| initial_drone_pos=np.asarray(drone_pos, dtype=np.float64), | |
| create_feedback_spheres=create_feedback_spheres, | |
| feedback_zones_registry=feedback_zones_registry, | |
| action_cache=action_cache, | |
| ) | |
| if multi_action is not None: | |
| print("[Phase 1+2] Multi-action planning complete.") | |
| return multi_action | |
| if len(billboard_ids) >= 2 and portal_leg_goal_fn is not None: | |
| from .multi_leg import run_multi_leg_navigation_phase12 | |
| multi_result = run_multi_leg_navigation_phase12( | |
| billboard_ids=billboard_ids, | |
| portal_leg_goal_fn=portal_leg_goal_fn, | |
| phase12_kwargs=common_kw, | |
| initial_drone_pos=np.asarray(drone_pos, dtype=np.float64), | |
| create_feedback_spheres=create_feedback_spheres, | |
| feedback_zones_registry=feedback_zones_registry, | |
| ) | |
| if multi_result is None: | |
| return None | |
| print("[Phase 1+2] Multi-leg planning complete.") | |
| return multi_result | |
| single = run_single_leg_phase1_and_phase2( | |
| **common_kw, | |
| pause_message=False, | |
| create_feedback_spheres=create_feedback_spheres, | |
| feedback_zones_registry=feedback_zones_registry, | |
| ) | |
| if single is None: | |
| return None | |
| print("[Phase 1+2] Single-leg planning complete.") | |
| return single | |