"""依頼233:寄せて通る形があるかの計算(真上の2次元・長方形で近似)。 車いす(根から x ±0.326・前後 −0.430〜+0.640。229 の Unity で測った範囲)、大城さん(根は車いすの根の後ろ 0.594・体 幅 ±0.238・奥行 ±0.157)、 ハコブくん(根から x ±0.330・前後 −0.380〜+0.382、寄った位置 x −0.4643 で止まる。229)。 車いすは 229 の寄せ方の決まり(前へ進みながら・向き 8° まで・横の速さ 0.12 m/秒まで・寄せ終わりで向き 0°)で、廊下の真ん中 x 0.00 から寄せる。 向きは道に沿って 0 → 最大 → 0(変わる所の長さ a)。最大 8°・a を変えて、寄せ終わりまでに前へ進む長さと、ハコブくんとのすき間の最小を出す""" import math, sys import numpy as np RX = -0.4643; RHX = 0.330; RF, RB = 0.382, 0.380 CX0, CX1, CF, CB = -0.326, 0.326, 0.640, 0.430 def rect_pts(cx, cz, yaw, x0, x1, f, b, n=12): # 前=−z。yaw は +x へ向く角度 s, c = math.sin(yaw), math.cos(yaw) pts = [] for i in range(n + 1): for (lx, lu) in ((x0 + (x1 - x0) * i / n, f), (x0 + (x1 - x0) * i / n, -b), (x0, -b + (f + b) * i / n), (x1, -b + (f + b) * i / n)): pts.append((cx + lx * c + lu * s, cz - lu * c + lx * s)) return pts def dist_robot(pts, rz): best = 9.0 for (x, z) in pts: dx = max(RX - RHX - x, 0, x - (RX + RHX)); dz = max(rz - RB - z, 0, z - (rz + RF)) inside = dx == 0 and dz == 0 d = -1.0 if inside else math.hypot(dx, dz) best = min(best, d) return best def run(DX, ang_deg, a, G, ds=0.002): rz = 0.0; zfront = rz + RF + G # 足のせの前の端 root_z0 = zfront + CF tmax = math.tan(math.radians(ang_deg)) # 向き θ(s):0→ang(長さ a)・平ら(長さ p)・ang→0(長さ a)。横の動き=∫tanθ ds = tmax の近似で解く # p を二分法で def lat(p): x = 0.0; s = 0.0; L = 2 * a + p while s < L: th = math.radians(ang_deg) * (min(1, s / a) if s < a else 1 if s < a + p else max(0, (L - s) / a)) x += math.tan(th) * ds; s += ds return x lo, hi = 0.0, 20.0 if lat(0.0) > DX: # 平らが無くても寄りすぎ → 角度を下げる代わりに a を縮めずにそのまま(報告用に記す) hi = 0.0 for _ in range(40): mid = 0.5 * (lo + hi) if lat(mid) < DX: lo = mid else: hi = mid p = hi; L = 2 * a + p x = 0.0; s = 0.0; gmin = 9.0; gmin_s = 0; x_at = [] while s <= L + 1e-9: th = math.radians(ang_deg) * (min(1, s / a) if s < a else 1 if s < a + p else max(0, (L - s) / a)) # 車軸は根の後ろ 0.15。根の位置(x, z) ax_z = root_z0 + 0.15 - s; ax_x = x rx_ = ax_x + 0.15 * math.sin(th); rz_ = ax_z - 0.15 * math.cos(th) pts = rect_pts(rx_, rz_, th, CX0, CX1, CF, CB) # 大城さん ox = rx_ - 0.594 * math.sin(th); oz = rz_ + 0.594 * math.cos(th) pts += rect_pts(ox, oz, th, -0.238, 0.238, 0.157, 0.157, 6) g = dist_robot(pts, rz) if g < gmin: gmin, gmin_s = g, s x += math.tan(th) * ds; s += ds return L, gmin, gmin_s out = [] P = lambda t: (print(t), out.append(t)) P("寄せる横の長さ ΔX と、向きの最大 8° で前へ進む長さの下限(向きが一瞬で変わるとしても)= ΔX / tan 8°") for DX, why in ((0.42, "229 と同じ x +0.42 へ"), (0.346, "寄った位置のハコブくんと 0.15 m あくのに要る最小(229:+0.42 で 0.224 m)"), (0.4805, "ふだんの位置 x −0.33 のハコブくんと 0.15 m あくのに要る最小(+0.42 で 0.0895 m)")): P(f" ΔX {DX:.4f}({why}):{DX / math.tan(math.radians(8)):.3f} m") P("止まった時の足のせの前の端とハコブくんの前の端のあいだ G(229 の決まりで止まると 2.30 m。できるだけ守る 2.0〜3.0 m)") P("向きの変わる所の長さ a・前へ進む長さ L・ハコブくん(x −0.4643 で止まったまま)と車いす/大城さんのすき間の最小(負は重なる)") for G in (2.0, 2.3, 3.0): for DX in (0.42, 0.346): for a in (0.3, 0.6, 1.0): L, g, gs = run(DX, 8.0, a, G) P(f" G {G:.1f}・ΔX {DX:.3f}・a {a:.1f}:前へ {L:.3f} m 進む(足のせの前の端がハコブくんの前の端を {L - G:+.3f} m 越える)・すき間の最小 {g:.3f} m(前へ {gs:.2f} m の所)") open(sys.argv[1] if len(sys.argv) > 1 else "/tmp/c233/geo/feas233.txt", "w").write("\n".join(out) + "\n") # ---------------------------------------------------------------- 依頼233 の置き方(寺島さんは +x 側)と、寄せる所のくわしい値 P("") P("■ 止まった所の置き方(車いす x 0.00・向き 0°・大城さん x 0.00、寺島さんは大城さんの +x 側で −x を向く。寸法は 231 の terashima231.txt)") RAIL = 0.84; NEED = 0.053 + 0.639 + 0.15 O_HW, O_HD = 0.2381, 0.157 T_F, T_B, T_HW = 0.1499, 0.1399, 0.2381 for g in (0.10, 0.15): xT = O_HW + g + T_F P(f" 寺島さんと大城さんのすき間 {g:.2f} m に置くと:寺島さんの根 x {xT:+.3f}・背中 x {xT + T_B:+.3f}(手すりと {RAIL - xT - T_B:.3f} m)") P(f" 車いすの所:−x 側 {(-0.326) + RAIL:.3f} m・+x 側 {RAIL - 0.326:.3f} m/大城さん・寺島さんの所:−x 側(大城さんと手すり){RAIL - O_HW:.3f} m・+x 側(寺島さんの背中と手すり)0.162〜0.212 m") P(f" ハコブくんが手すりの側を通るのに要る幅 {NEED:.3f} m(231 と同じ)→ どこも狭く、通れない") P(f" 寺島さんが後ろから追いついて横に並ぶ時(前を向いて歩く・幅 {2*T_HW:.3f} m):大城さんの +x の端 {O_HW:.3f} と手すり {RAIL:.2f} の間 {RAIL - O_HW:.3f} m → 真ん中を歩いて両側 {(RAIL - O_HW - 2*T_HW) / 2:.3f} m ずつ") P(f" 寺島さんの横(大城さんと同じ z)で、大城さんが寺島さんの顔を見るのに要る顔の向き:約 {math.degrees(math.atan2(O_HW + 0.15 + T_F, -0.09)) :.0f}°(228・229 の大城さんの顔の向きの最大 34°。大城さんの顔は Head の骨だけで回す作り)") P("") P("■ その場で寄せる形(依頼の形:車いすの前に空いている所の中で寄せ、寺島さんが下がって大城さんの寄る所を空ける)") for G in (2.3, 3.0): for a in (0.0, 0.6): D = G - 0.15 lat = math.tan(math.radians(8)) * max(0.0, D - a) P(f" 足のせとハコブくんのあいだ {G:.1f} m・前へ {D:.2f} m まで(ハコブくんの前の端と 0.15 m)・向きの変わる所 {a:.1f} m:寄せられる横の長さ {lat:.3f} m" f"(x +0.42 まで {0.42 - lat:.3f} m 足りない・寄った位置のハコブくんと 0.15 m あく x +0.346 まで {max(0.0, 0.346 - lat):.3f} m 足りない)") P("") P("■ 前へ進んでハコブくんの横まで行く形(229 の寄せ方の決まりで、当たらずに寄せられる形)で、寺島さんが動かない時の大城さんとのすき間") def tera_gap(DX, a, G, xT, zrel): rz = 0.0; zfront = rz + RF + G; root_z0 = zfront + CF; oz0 = root_z0 + 0.594 tz = oz0 + zrel tpts = rect_pts(xT, tz, -math.pi / 2, -T_HW, T_HW, T_F, T_B, 8) ds = 0.002; tmax = math.radians(8.0) def lat(p): x = 0.0; s = 0.0; L = 2 * a + p while s < L: th = tmax * (min(1, s / a) if s < a else 1 if s < a + p else max(0, (L - s) / a)); x += math.tan(th) * ds; s += ds return x lo, hi = 0.0, 20.0 for _ in range(40): mid = 0.5 * (lo + hi) if lat(mid) < DX: lo = mid else: hi = mid p = hi; L = 2 * a + p; x = 0.0; s = 0.0; best = 9.0 while s <= L: th = tmax * (min(1, s / a) if s < a else 1 if s < a + p else max(0, (L - s) / a)) ax_z = root_z0 + 0.15 - s; rx_ = x + 0.15 * math.sin(th); rz_ = ax_z - 0.15 * math.cos(th) ox = rx_ - 0.594 * math.sin(th); oz = rz_ + 0.594 * math.cos(th) opts = rect_pts(ox, oz, th, -O_HW, O_HW, O_HD, O_HD, 6) + rect_pts(rx_, rz_, th, CX0, CX1, CF, CB) for (px_, pz_) in opts: # 寺島さんの長方形(−x を向く)の中か・距離 lx = -(pz_ - tz); lu = -(px_ - xT) # 寺島さんの枠:横=z・前=−x dx = max(-T_HW - lx, 0, lx - T_HW); du = max(-T_B - lu, 0, lu - T_F) best = min(best, -1.0 if dx == 0 and du == 0 else math.hypot(dx, du)) x += math.tan(th) * ds; s += ds return best for zrel in (0.0, -0.2): g = tera_gap(0.42, 0.6, 2.3, O_HW + 0.15 + T_F, zrel) P(f" 寺島さん x {O_HW + 0.15 + T_F:+.3f}・大城さんより前へ {abs(zrel):.1f} m:下がらなくても大城さん・車いすとのすき間の最小 {g:.3f} m(大城さんは前へ離れていく)→ 下がる距離 0 m で当たらない") open(sys.argv[1] if len(sys.argv) > 1 else "/tmp/c233/geo/feas233.txt", "w").write("\n".join(out) + "\n")