XY-VLA / algorithms /phase1.py
LinJinwei's picture
Upload 52 files
d2f1e32 verified
Raw History Blame Contribute Delete
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