"""依頼235:止まった車いす(x 0.00・向き 0°)を、その場で向きを変えて x +0.42・向き 0° へ寄せる道があるかの計算(真上の2次元・長方形で近似)。 決まり(依頼235 の答え 1):向き ±30° まで・速さ 0.3 m/秒・向きの変わる速さ 25°/秒・後ろの車輪は車軸の向きにだけ進む(その場で回るのは車軸の中心で)。 前へ:足のせの前の端とハコブくんの前の端のあいだ 0.6 m 以上。後ろへ:1.0 m まで(車軸の中心の z)。 寸法は 233 と同じ:車いす 車軸の中心から 横 ±0.326・前 0.790(足のせの前の端)・後ろ 0.280。大城さん 根は車軸の後ろ 0.444・幅 ±0.2381・奥行 ±0.157。 手すりの内の面 ±0.84。ハコブくん x −0.4643・幅 ±0.330・前の端は z 0(止まった所。足のせの前の端は G=2.30 m 先)。 z は前(ハコブくんの方)を − に取る。道は「その場で回る」「まっすぐ前/後ろ」をつないだもの(hybrid A*)。費用=時間(速さの上限で)+切りかえ 1 回 0.8 秒""" import math, heapq, sys, json RAIL = 0.84 CW, CF, CB = 0.326, 0.790, 0.280 OB, OW, OD = 0.444, 0.2381, 0.157 G = float(sys.argv[1]) if len(sys.argv) > 1 else 2.30 MARGIN = float(sys.argv[2]) if len(sys.argv) > 2 else 0.05 # 手すりとの余り BACK_MAX = float(sys.argv[3]) if len(sys.argv) > 3 else 1.0 FWD_MAX = G - 0.6 # 足のせが前へ出てよい長さ(z) TH_MAX = 30.0 # 寺島さん(−x を向いて立つ・前 0.150・後ろ 0.140・幅 ±0.2381)。TZ は大城さんの根からの前後(+ は後ろ)。TX は根の x TX = float(sys.argv[5]) if len(sys.argv) > 5 else 9.0 TZ = float(sys.argv[6]) if len(sys.argv) > 6 else 0.0 PCLR = float(sys.argv[7]) if len(sys.argv) > 7 else 0.05 TF, TB, TW = 0.1499, 0.1399, 0.2381 def tera_box(): return (TX - TF, TX + TB, OB + TZ - TW, OB + TZ + TW) def poly_box_gap(pts, box): # 凸の四角(pts)と軸に沿った箱のすき間(近似:頂点と辺の点の距離・中に入れば −1) x0, x1, z0, z1 = box; best = 9.0 for i in range(4): a, b = pts[i], pts[(i + 1) % 4] for k in range(9): px = a[0] + (b[0] - a[0]) * k / 8; pz = a[1] + (b[1] - a[1]) * k / 8 dx = max(x0 - px, 0, px - x1); dz = max(z0 - pz, 0, pz - z1) best = min(best, -1.0 if dx == 0 and dz == 0 else math.hypot(dx, dz)) # 箱の角が四角の中 for (cx, cz) in ((x0, z0), (x0, z1), (x1, z0), (x1, z1)): inside = True for i in range(4): a, b = pts[i], pts[(i + 1) % 4] if (b[0] - a[0]) * (cz - a[1]) - (b[1] - a[1]) * (cx - a[0]) > 0: inside = False if inside: return -1.0 return best def corners(ax, az, th): s, c = math.sin(math.radians(th)), math.cos(math.radians(th)) # 前=−z、向き + は +x へ。点 (lx 横, lu 前) → 世界 def w(lx, lu): return (ax + lx * c + lu * s, az - lu * c + lx * s) ch = [w(-CW, CF), w(CW, CF), w(CW, -CB), w(-CW, -CB)] ob = [w(-OW, -OB + OD), w(OW, -OB + OD), w(OW, -OB - OD), w(-OW, -OB - OD)] return ch, ob def ok(ax, az, th): ch, ob = corners(ax, az, th) xs = [p[0] for p in ch + ob] if max(xs) > RAIL - MARGIN or min(xs) < -RAIL + MARGIN: return False fz = min(p[1] for p in ch) # 足のせの前の端(いちばん前の点) if 0.0 - fz > FWD_MAX + 1e-9: return False # z0 = 足のせの始めの前の端を 0 とする if az > BACK_MAX + 1e-9: return False if TX < 5: bx = tera_box() if min(poly_box_gap(ch, bx), poly_box_gap(ob, bx)) < PCLR: return False return True def rail_gap(ax, az, th): ch, ob = corners(ax, az, th); xs = [p[0] for p in ch + ob] return min(RAIL - max(xs), RAIL + min(xs)) DTH, DS = 1.0, 0.01 V, W = 0.3, 25.0 import os; SW = float(os.environ.get("SW", "0.8")) def key(ax, az, th): return (round(ax / 0.005), round(az / 0.01), round(th)) start = (0.0, 0.0, 0.0) goalx = 0.42 pq = [(0.0, 0.0, 0.0, 0.0, 0.0, "", None)] seen = {} parent = {} best = None n = 0 while pq: cost, ax, az, th, _, last, par = heapq.heappop(pq) k = key(ax, az, th) if k in seen: continue seen[k] = (ax, az, th, last, par) n += 1 if abs(ax - goalx) < 0.0026 and abs(th) < 0.5: best = k; break for mv in ("r+", "r-", "f", "b"): if mv == "r+": nax, naz, nth, dc = ax, az, th + DTH, DTH / W elif mv == "r-": nax, naz, nth, dc = ax, az, th - DTH, DTH / W else: sg = 1 if mv == "f" else -1 nax = ax + sg * DS * math.sin(math.radians(th)); naz = az - sg * DS * math.cos(math.radians(th)); nth = th; dc = DS / V if abs(nth) > TH_MAX + 1e-9 or not ok(nax, naz, nth): continue # 回る途中も確かめる(1° ごとなので端だけで足りる) kind = "r" if mv[0] == "r" else mv c2 = cost + dc + (SW if (last and kind != last) else 0.0) nk = key(nax, naz, nth) if nk in seen: continue heapq.heappush(pq, (c2, nax, naz, nth, 0.0, kind, k)) if best is None: print(f"G {G} MARGIN {MARGIN} BACK {BACK_MAX}: 道なし(調べた {n})"); sys.exit(1) path = [] k = best while k is not None: ax, az, th, last, par = seen[k]; path.append((ax, az, th, last)); k = par path.reverse() # 区切り segs = [] for p in path[1:]: if not segs or segs[-1][0] != p[3]: segs.append([p[3], p]) else: segs[-1].append(p) fwd = -min(p[1] for p in path); back = max(p[1] for p in path) fz_min = min(min(q[1] for q in corners(*p[:3])[0]) for p in path) rg = min(rail_gap(*p[:3]) for p in path) thmax = max(abs(p[2]) for p in path) T = 0; prev = None for s in segs: kind = s[0]; a = s[1]; b = s[-1] T += (len(s) - 1) * (DTH / W if kind == "r" else DS / V) desc = [] p0 = path[0] for s in segs: b = s[-1] desc.append(f"{ {'r':'回る','f':'前へ','b':'後ろへ'}[s[0]] }→(x {b[0]:+.3f}, z {b[1]:+.3f}, 向き {b[2]:+.0f}°)") res = {"G": G, "margin": MARGIN, "back_max": BACK_MAX, "turns": len(segs), "time_s": round(T + 0.8 * (len(segs) - 1), 2), "fwd_axle_m": round(fwd, 3), "back_axle_m": round(back, 3), "footrest_to_robot_min_m": round(G + fz_min, 3), "rail_gap_min_m": round(rg, 3), "heading_max_deg": thmax, "segs": desc, "path": [[round(p[0], 4), round(p[1], 4), p[2], p[3]] for p in path]} print(json.dumps({k: v for k, v in res.items() if k != "path"}, ensure_ascii=False)) if len(sys.argv) > 4: json.dump(res, open(sys.argv[4], "w"), ensure_ascii=False)