多脚ロボットの歩行制御と宇宙応用 — 足で歩く惑星探査ロボット

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Polygon
from matplotlib.collections import PatchCollection

# 4脚ロボットの足先位置（上から見た2D座標）
feet_positions = np.array([
    [0.3, 0.2],    # 左前脚
    [0.3, -0.2],   # 右前脚
    [-0.3, 0.2],   # 左後脚
    [-0.3, -0.2],  # 右後脚
])

# 支持多角形を計算（凸包）
from scipy.spatial import ConvexHull

def compute_support_polygon(feet):
    """接地している足先位置から支持多角形を計算"""
    if len(feet) < 3:
        return None
    hull = ConvexHull(feet)
    return feet[hull.vertices]

def compute_zmp(x_G, y_G, z_G, ddx_G, ddy_G, g=9.81):
    """簡略化ZMPの計算"""
    x_zmp = x_G - (ddx_G / g) * z_G
    y_zmp = y_G - (ddy_G / g) * z_G
    return x_zmp, y_zmp

def point_in_polygon(point, polygon):
    """点が凸多角形の内部にあるかを判定"""
    n = len(polygon)
    for i in range(n):
        edge = polygon[(i + 1) % n] - polygon[i]
        to_point = point - polygon[i]
        cross = edge[0] * to_point[1] - edge[1] * to_point[0]
        if cross < 0:
            return False
    return True

def stability_margin(zmp, polygon):
    """ZMPから支持多角形の最近辺までの距離"""
    n = len(polygon)
    min_dist = float('inf')
    for i in range(n):
        p1 = polygon[i]
        p2 = polygon[(i + 1) % n]
        edge = p2 - p1
        edge_len = np.linalg.norm(edge)
        edge_unit = edge / edge_len
        to_zmp = zmp - p1
        proj = np.dot(to_zmp, edge_unit)
        proj = np.clip(proj, 0, edge_len)
        closest = p1 + proj * edge_unit
        dist = np.linalg.norm(zmp - closest)
        min_dist = min(min_dist, dist)
    return min_dist

# --- シナリオ1: 全脚接地（静止） ---
z_G = 0.4  # 重心高さ
sp_full = compute_support_polygon(feet_positions)
zmp_static = compute_zmp(0.0, 0.0, z_G, 0.0, 0.0)

# --- シナリオ2: 全脚接地（加速中） ---
zmp_accel = compute_zmp(0.0, 0.0, z_G, 2.0, 1.0)

# --- シナリオ3: 右前脚を持ち上げ（遊脚） ---
feet_3leg = feet_positions[[0, 2, 3]]  # 左前・左後・右後
sp_3leg = compute_support_polygon(feet_3leg)
zmp_3leg = compute_zmp(-0.05, 0.02, z_G, 0.0, 0.0)

fig, axes = plt.subplots(1, 3, figsize=(15, 5))

for ax, title, sp, zmp, feet in [
    (axes[0], "4-leg support (static)", sp_full, zmp_static, feet_positions),
    (axes[1], "4-leg support (accelerating)", sp_full, zmp_accel, feet_positions),
    (axes[2], "3-leg support (RF leg swing)", sp_3leg, zmp_3leg, feet_3leg),
]:
    poly = Polygon(sp, closed=True, alpha=0.3, color='cyan', edgecolor='cyan', linewidth=2)
    ax.add_patch(poly)
    ax.scatter(feet[:, 0], feet[:, 1], s=100, c='white', edgecolors='cyan', zorder=5, label='Foot')
    ax.scatter(*zmp, s=150, c='red', marker='x', linewidths=3, zorder=6, label='ZMP')

    margin = stability_margin(np.array(zmp), sp)
    is_stable = point_in_polygon(np.array(zmp), sp)

    ax.set_title(f"{title}\nMargin={margin:.3f}m {'(Stable)' if is_stable else '(UNSTABLE)'}", fontsize=10)
    ax.set_xlim(-0.5, 0.5)
    ax.set_ylim(-0.4, 0.4)
    ax.set_aspect('equal')
    ax.grid(True, alpha=0.3)
    ax.set_xlabel('x [m]')
    ax.set_ylabel('y [m]')
    ax.legend(fontsize=8)

plt.tight_layout()
plt.savefig('zmp_support_polygon.png', dpi=150, bbox_inches='tight')
plt.show()

print(f"シナリオ1 (静止): ZMP=({zmp_static[0]:.3f}, {zmp_static[1]:.3f}), margin={stability_margin(np.array(zmp_static), sp_full):.3f} m")
print(f"シナリオ2 (加速): ZMP=({zmp_accel[0]:.3f}, {zmp_accel[1]:.3f}), margin={stability_margin(np.array(zmp_accel), sp_full):.3f} m")
print(f"シナリオ3 (3脚): ZMP=({zmp_3leg[0]:.3f}, {zmp_3leg[1]:.3f}), margin={stability_margin(np.array(zmp_3leg), sp_3leg):.3f} m")

import numpy as np
import matplotlib.pyplot as plt

def simulate_cpg(gait_phases, omega=2*np.pi, coupling=5.0, beta=0.6,
                 dt=0.001, T=5.0):
    """
    CPG結合振動子モデルのシミュレーション

    Parameters
    ----------
    gait_phases : dict
        各脚の目標位相差 {'LF': 0, 'RF': pi, ...}
    omega : float
        固有角振動数
    coupling : float
        結合強度
    beta : float
        デューティファクター
    dt : float
        時間刻み
    T : float
        シミュレーション時間
    """
    leg_names = list(gait_phases.keys())
    N = len(leg_names)
    target_phases = np.array([gait_phases[name] for name in leg_names])

    # 初期位相（少しランダムにずらす）
    phi = np.random.uniform(0, 2*np.pi, N)
    t_array = np.arange(0, T, dt)
    phi_history = np.zeros((len(t_array), N))

    for k, t in enumerate(t_array):
        phi_history[k] = phi.copy()
        dphi = np.zeros(N)
        for i in range(N):
            dphi[i] = omega
            for j in range(N):
                if i != j:
                    # 目標位相差: target_phases[j] - target_phases[i]
                    psi_ij = target_phases[j] - target_phases[i]
                    dphi[i] += coupling * np.sin(phi[j] - phi[i] - psi_ij)
        phi += dphi * dt
    return t_array, phi_history, leg_names, beta

# 歩容パターンの定義
gaits = {
    'Walk': {'LF': 0, 'RH': np.pi/2, 'RF': np.pi, 'LH': 3*np.pi/2},
    'Trot': {'LF': 0, 'RH': 0, 'RF': np.pi, 'LH': np.pi},
    'Gallop': {'LF': 0, 'RF': np.pi/8, 'LH': np.pi, 'RH': np.pi + np.pi/8},
    'Pronk': {'LF': 0, 'RF': 0, 'LH': 0, 'RH': 0},
}
betas = {'Walk': 0.75, 'Trot': 0.5, 'Gallop': 0.35, 'Pronk': 0.4}

fig, axes = plt.subplots(4, 1, figsize=(12, 10))

for ax, (gait_name, phases) in zip(axes, gaits.items()):
    beta = betas[gait_name]
    t, phi_hist, names, _ = simulate_cpg(phases, beta=beta, T=4.0)

    # 接地/遊脚のタイムチャート
    for i, name in enumerate(names):
        phase_mod = phi_hist[:, i] % (2 * np.pi)
        stance = phase_mod < 2 * np.pi * beta
        # 接地区間を塗りつぶし
        ax.fill_between(t, i - 0.4, i + 0.4, where=stance,
                        color='cyan', alpha=0.7)
        ax.fill_between(t, i - 0.4, i + 0.4, where=~stance,
                        color='gray', alpha=0.2)

    ax.set_yticks(range(len(names)))
    ax.set_yticklabels(names)
    ax.set_title(f'{gait_name} (β={beta})', fontsize=12, fontweight='bold')
    ax.set_xlim(1.5, 4.0)  # 収束後を表示
    ax.set_xlabel('Time [s]')
    ax.grid(True, axis='x', alpha=0.3)

plt.tight_layout()
plt.savefig('gait_patterns.png', dpi=150, bbox_inches='tight')
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from scipy.special import comb

def swing_trajectory_ellipse(tau, stride, height):
    """半楕円遊脚軌道"""
    x = -stride/2 + stride * tau
    z = height * np.sin(np.pi * tau)
    return x, z

def bezier_curve(tau, control_points):
    """n次ベジェ曲線"""
    n = len(control_points) - 1
    point = np.zeros(2)
    for k in range(n + 1):
        coeff = comb(n, k) * (1 - tau)**(n - k) * tau**k
        point += coeff * control_points[k]
    return point[0], point[1]

def swing_trajectory_bezier(tau, stride, height):
    """5次ベジェ曲線遊脚軌道"""
    control_points = np.array([
        [-stride/2, 0.0],       # P0: 離地点
        [-stride/2, height],    # P1: 急速に持ち上げ
        [-stride/4, height*1.2],# P2: 最高点付近
        [stride/4, height*1.2], # P3: 最高点付近
        [stride/2, height*0.3], # P4: 降下開始
        [stride/2, 0.0],        # P5: 着地点
    ])
    return bezier_curve(tau, control_points)

# 軌道の比較プロット
tau_vals = np.linspace(0, 1, 200)
stride = 0.2  # ストライド長 [m]
height = 0.08  # 持ち上げ高さ [m]

x_ell = np.zeros(len(tau_vals))
z_ell = np.zeros(len(tau_vals))
x_bez = np.zeros(len(tau_vals))
z_bez = np.zeros(len(tau_vals))

for i, tau in enumerate(tau_vals):
    x_ell[i], z_ell[i] = swing_trajectory_ellipse(tau, stride, height)
    x_bez[i], z_bez[i] = swing_trajectory_bezier(tau, stride, height)

fig, axes = plt.subplots(1, 2, figsize=(14, 5))

# 軌道の形状比較
axes[0].plot(x_ell, z_ell, 'c-', linewidth=2, label='Semi-ellipse')
axes[0].plot(x_bez, z_bez, 'm--', linewidth=2, label='Bézier (5th order)')
axes[0].fill_between(np.linspace(-0.15, 0.15, 50), -0.01, 0, color='gray', alpha=0.3)
axes[0].set_xlabel('x [m] (Forward)')
axes[0].set_ylabel('z [m] (Height)')
axes[0].set_title('Swing Foot Trajectory Comparison')
axes[0].legend()
axes[0].grid(True, alpha=0.3)
axes[0].set_aspect('equal')
axes[0].set_ylim(-0.02, 0.12)

# 高さの時間変化
axes[1].plot(tau_vals, z_ell, 'c-', linewidth=2, label='Semi-ellipse')
axes[1].plot(tau_vals, z_bez, 'm--', linewidth=2, label='Bézier (5th order)')
axes[1].set_xlabel('Normalized phase τ')
axes[1].set_ylabel('z [m] (Height)')
axes[1].set_title('Foot Height vs Phase')
axes[1].legend()
axes[1].grid(True, alpha=0.3)

plt.tight_layout()
plt.savefig('swing_trajectory.png', dpi=150, bbox_inches='tight')
plt.show()

import numpy as np
import matplotlib.pyplot as plt

# 天体パラメータ
bodies = {
    'Earth': {'g': 9.81, 'color': 'deepskyblue'},
    'Mars': {'g': 3.72, 'color': 'orangered'},
    'Moon': {'g': 1.62, 'color': 'silver'},
    'Titan': {'g': 1.35, 'color': 'gold'},
    'Europa': {'g': 1.31, 'color': 'lightyellow'},
}

leg_length = 0.5  # 脚長 [m]
z_G = 0.4         # 重心高さ [m]
robot_mass = 50   # ロボット質量 [kg]
mu = 0.6          # 摩擦係数
n_contact = 3     # 接地脚数

fig, axes = plt.subplots(2, 2, figsize=(14, 10))

# (1) フルード数と速度の関係
ax = axes[0, 0]
v_range = np.linspace(0, 3, 200)
for name, params in bodies.items():
    Fr = v_range**2 / (params['g'] * leg_length)
    ax.plot(v_range, Fr, color=params['color'], linewidth=2, label=name)
ax.axhline(y=0.5, color='red', linestyle='--', alpha=0.7, label='Walk/Run transition')
ax.set_xlabel('Walking speed v [m/s]')
ax.set_ylabel('Froude number Fr')
ax.set_title('Froude Number vs Walking Speed')
ax.legend(fontsize=9)
ax.grid(True, alpha=0.3)
ax.set_ylim(0, 3)

# (2) 最大歩行速度の比較
ax = axes[0, 1]
names = list(bodies.keys())
g_vals = [bodies[n]['g'] for n in names]
v_max = [np.sqrt(0.5 * g * leg_length) for g in g_vals]
colors = [bodies[n]['color'] for n in names]
bars = ax.bar(names, v_max, color=colors, edgecolor='white', linewidth=1.5)
for bar, v in zip(bars, v_max):
    ax.text(bar.get_x() + bar.get_width()/2., bar.get_height() + 0.02,
            f'{v:.2f}', ha='center', fontsize=10)
ax.set_ylabel('Max walk speed [m/s]')
ax.set_title(f'Maximum Walking Speed (l={leg_length}m)')
ax.grid(True, axis='y', alpha=0.3)

# (3) ZMPずれの比較
ax = axes[1, 0]
accel_range = np.linspace(0, 3, 200)
for name, params in bodies.items():
    zmp_shift = (accel_range / params['g']) * z_G
    ax.plot(accel_range, zmp_shift * 100, color=params['color'], linewidth=2, label=name)
ax.axhline(y=15, color='red', linestyle='--', alpha=0.5, label='Typical margin (15cm)')
ax.set_xlabel('Horizontal acceleration [m/s²]')
ax.set_ylabel('ZMP shift [cm]')
ax.set_title('ZMP Shift vs Horizontal Acceleration')
ax.legend(fontsize=9)
ax.grid(True, alpha=0.3)

# (4) 最大牽引力の比較
ax = axes[1, 1]
f_max = [mu * robot_mass * g / n_contact for g in g_vals]
bars = ax.bar(names, f_max, color=colors, edgecolor='white', linewidth=1.5)
for bar, f in zip(bars, f_max):
    ax.text(bar.get_x() + bar.get_width()/2., bar.get_height() + 1,
            f'{f:.1f}N', ha='center', fontsize=10)
ax.set_ylabel('Max traction force per leg [N]')
ax.set_title(f'Maximum Traction Force (M={robot_mass}kg, μ={mu})')
ax.grid(True, axis='y', alpha=0.3)

plt.tight_layout()
plt.savefig('low_gravity_walking.png', dpi=150, bbox_inches='tight')
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Polygon as MplPolygon
from scipy.spatial import ConvexHull

class QuadrupedCPGWalker:
    """4脚ロボットのCPG歩行シミュレーション"""

    def __init__(self, g=9.81, body_mass=50.0, body_length=0.6,
                 body_width=0.4, leg_length=0.5, stride=0.15,
                 swing_height=0.06):
        # 環境パラメータ
        self.g = g
        # ロボットパラメータ
        self.body_mass = body_mass
        self.body_length = body_length
        self.body_width = body_width
        self.leg_length = leg_length
        self.stride = stride
        self.swing_height = swing_height
        # 股関節位置（ボディ座標系）
        self.hip_offsets = np.array([
            [ body_length/2,  body_width/2],  # LF
            [ body_length/2, -body_width/2],  # RF
            [-body_length/2,  body_width/2],  # LH
            [-body_length/2, -body_width/2],  # RH
        ])
        # CPGパラメータ（トロット歩容）
        self.target_phases = np.array([0, np.pi, np.pi, 0])  # LF, RF, LH, RH
        self.omega = 2 * np.pi * 1.0  # 1 Hz
        self.coupling = 8.0
        self.beta = 0.6  # デューティファクター

    def simulate(self, T=6.0, dt=0.002, walk_speed=0.3):
        """歩行シミュレーションを実行"""
        steps = int(T / dt)
        # CPG位相
        phi = np.array([0.0, np.pi + 0.1, np.pi - 0.1, 0.2])
        # 記録用配列
        t_hist = np.zeros(steps)
        phi_hist = np.zeros((steps, 4))
        body_x_hist = np.zeros(steps)
        foot_pos_hist = np.zeros((steps, 4, 2))  # 各脚のxy位置
        zmp_hist = np.zeros((steps, 2))
        margin_hist = np.zeros(steps)
        stance_hist = np.zeros((steps, 4), dtype=bool)

        body_x = 0.0
        body_vx = walk_speed
        prev_vx = walk_speed

        for k in range(steps):
            t = k * dt
            t_hist[k] = t
            phi_hist[k] = phi.copy()
            body_x_hist[k] = body_x

            # CPGの更新
            dphi = np.zeros(4)
            for i in range(4):
                dphi[i] = self.omega
                for j in range(4):
                    if i != j:
                        psi_ij = self.target_phases[j] - self.target_phases[i]
                        dphi[i] += self.coupling * np.sin(phi[j] - phi[i] - psi_ij)
            phi += dphi * dt

            # 各脚の状態と位置
            foot_positions = np.zeros((4, 2))
            stance = np.zeros(4, dtype=bool)

            for i in range(4):
                phase_mod = phi[i] % (2 * np.pi)
                is_stance = phase_mod < 2 * np.pi * self.beta
                stance[i] = is_stance

                hip_x = body_x + self.hip_offsets[i, 0]
                hip_y = self.hip_offsets[i, 1]

                if is_stance:
                    # 立脚: 足先は地面に固定（ボディ座標系では後退）
                    stance_progress = phase_mod / (2 * np.pi * self.beta)
                    foot_x = hip_x + self.stride/2 - self.stride * stance_progress
                    foot_positions[i] = [foot_x, hip_y]
                else:
                    # 遊脚: 半楕円軌道
                    swing_progress = (phase_mod - 2*np.pi*self.beta) / (2*np.pi*(1-self.beta))
                    foot_x = hip_x - self.stride/2 + self.stride * swing_progress
                    foot_positions[i] = [foot_x, hip_y]

            foot_pos_hist[k] = foot_positions
            stance_hist[k] = stance

            # ZMP計算（簡略版）
            # ボディの加速度を推定（速度の数値微分）
            body_ax = 0.0  # 等速歩行を仮定
            body_ay = 0.0
            z_G = self.leg_length

            zmp_x = body_x - (body_ax / self.g) * z_G
            zmp_y = 0.0 - (body_ay / self.g) * z_G
            zmp_hist[k] = [zmp_x, zmp_y]

            # 安定余裕の計算
            stance_feet = foot_positions[stance]
            if len(stance_feet) >= 3:
                try:
                    hull = ConvexHull(stance_feet)
                    polygon = stance_feet[hull.vertices]
                    margin = self._stability_margin(np.array([zmp_x, zmp_y]), polygon)
                except:
                    margin = 0.0
            elif len(stance_feet) == 2:
                # 2脚支持: ZMPから線分への距離
                margin = self._point_to_segment_dist(
                    np.array([zmp_x, zmp_y]), stance_feet[0], stance_feet[1])
                margin = -margin  # 線分上にはZMPは乗らないので負
            else:
                margin = -1.0
            margin_hist[k] = margin

            # ボディ位置の更新
            body_x += body_vx * dt

        return {
            't': t_hist, 'phi': phi_hist, 'body_x': body_x_hist,
            'foot_pos': foot_pos_hist, 'zmp': zmp_hist,
            'margin': margin_hist, 'stance': stance_hist,
        }

    def _stability_margin(self, zmp, polygon):
        """ZMPから支持多角形の最近辺までの距離"""
        n = len(polygon)
        min_dist = float('inf')
        for i in range(n):
            p1 = polygon[i]
            p2 = polygon[(i + 1) % n]
            dist = self._point_to_segment_dist(zmp, p1, p2)
            min_dist = min(min_dist, dist)
        # 内部判定
        inside = True
        for i in range(n):
            edge = polygon[(i+1)%n] - polygon[i]
            to_p = zmp - polygon[i]
            cross = edge[0]*to_p[1] - edge[1]*to_p[0]
            if cross < 0:
                inside = False
                break
        return min_dist if inside else -min_dist

    def _point_to_segment_dist(self, p, a, b):
        """点pから線分abへの距離"""
        ab = b - a
        ap = p - a
        t = np.dot(ap, ab) / (np.dot(ab, ab) + 1e-10)
        t = np.clip(t, 0, 1)
        closest = a + t * ab
        return np.linalg.norm(p - closest)

import numpy as np
import matplotlib.pyplot as plt

# 3つの重力環境でシミュレーション
environments = {
    'Earth (g=9.81)': 9.81,
    'Mars (g=3.72)': 3.72,
    'Moon (g=1.62)': 1.62,
}
colors = {'Earth (g=9.81)': 'deepskyblue', 'Mars (g=3.72)': 'orangered', 'Moon (g=1.62)': 'silver'}

results = {}
for name, g in environments.items():
    walker = QuadrupedCPGWalker(g=g, stride=0.15, swing_height=0.06)
    results[name] = walker.simulate(T=5.0, dt=0.002, walk_speed=0.3)

fig, axes = plt.subplots(3, 1, figsize=(14, 12))

# (1) 安定余裕の時間変化
ax = axes[0]
for name, res in results.items():
    # 移動平均でスムージング
    window = 50
    margin_smooth = np.convolve(res['margin'], np.ones(window)/window, mode='valid')
    t_smooth = res['t'][:len(margin_smooth)]
    ax.plot(t_smooth, margin_smooth * 100, color=colors[name], linewidth=1.5,
            label=name, alpha=0.8)
ax.axhline(y=0, color='red', linestyle='--', alpha=0.5, label='Stability boundary')
ax.set_xlabel('Time [s]')
ax.set_ylabel('Stability margin [cm]')
ax.set_title('Stability Margin Comparison (Trot Gait, v=0.3 m/s)')
ax.legend()
ax.grid(True, alpha=0.3)
ax.set_xlim(1.5, 5.0)

# (2) 歩容タイムチャート（地球）
ax = axes[1]
res_earth = results['Earth (g=9.81)']
leg_names = ['LF', 'RF', 'LH', 'RH']
for i, name in enumerate(leg_names):
    stance = res_earth['stance'][:, i]
    ax.fill_between(res_earth['t'], i - 0.35, i + 0.35,
                    where=stance, color='cyan', alpha=0.7)
    ax.fill_between(res_earth['t'], i - 0.35, i + 0.35,
                    where=~stance, color='gray', alpha=0.2)
ax.set_yticks(range(4))
ax.set_yticklabels(leg_names)
ax.set_xlabel('Time [s]')
ax.set_title('Gait Timing Chart (Earth, Trot)')
ax.set_xlim(1.5, 4.0)
ax.grid(True, axis='x', alpha=0.3)

# (3) ZMP軌跡の比較（上面図）
ax = axes[2]
for name, res in results.items():
    # 定常状態のZMP
    start_idx = int(1.5 / 0.002)
    zmp = res['zmp'][start_idx:]
    body_x = res['body_x'][start_idx:]
    # ZMPのボディ座標系での位置
    zmp_body_x = zmp[:, 0] - body_x
    ax.scatter(zmp_body_x[::10], zmp[::10, 1] * 100, s=3, alpha=0.5,
              color=colors[name], label=name)

# 股関節位置を表示
hip_x = [0.3, 0.3, -0.3, -0.3]
hip_y = [20, -20, 20, -20]
ax.scatter(hip_x, hip_y, s=100, c='white', edgecolors='cyan', zorder=5, marker='s')
for hx, hy, ln in zip(hip_x, hip_y, leg_names):
    ax.annotate(ln, (hx, hy), textcoords="offset points", xytext=(10, 5), fontsize=9)

ax.set_xlabel('x (body frame) [m]')
ax.set_ylabel('y [cm]')
ax.set_title('ZMP Position in Body Frame')
ax.legend()
ax.grid(True, alpha=0.3)
ax.set_aspect('equal')

plt.tight_layout()
plt.savefig('cpg_walking_simulation.png', dpi=150, bbox_inches='tight')
plt.show()

# 統計量の出力
print("=== 安定余裕の統計（定常状態, t > 1.5s） ===")
for name, res in results.items():
    start_idx = int(1.5 / 0.002)
    margin = res['margin'][start_idx:]
    print(f"{name}: mean={np.mean(margin)*100:.1f} cm, "
          f"min={np.min(margin)*100:.1f} cm, "
          f"std={np.std(margin)*100:.1f} cm")

import numpy as np
import matplotlib.pyplot as plt

# パラメータ
leg_lengths = np.linspace(0.2, 1.0, 50)  # 脚長 [m]
g_earth = 9.81
g_mars = 3.72
g_moon = 1.62

# 各重力環境での歩容遷移速度
v_walk_max_earth = np.sqrt(0.5 * g_earth * leg_lengths)
v_walk_max_mars = np.sqrt(0.5 * g_mars * leg_lengths)
v_walk_max_moon = np.sqrt(0.5 * g_moon * leg_lengths)

fig, axes = plt.subplots(1, 2, figsize=(14, 6))

# (1) 脚長 vs 最大歩行速度
ax = axes[0]
ax.plot(leg_lengths, v_walk_max_earth, 'deepskyblue', linewidth=2, label='Earth')
ax.plot(leg_lengths, v_walk_max_mars, 'orangered', linewidth=2, label='Mars')
ax.plot(leg_lengths, v_walk_max_moon, 'silver', linewidth=2, label='Moon')
ax.fill_between(leg_lengths, 0, v_walk_max_moon, alpha=0.1, color='silver')
ax.fill_between(leg_lengths, v_walk_max_moon, v_walk_max_mars, alpha=0.1, color='orangered')
ax.fill_between(leg_lengths, v_walk_max_mars, v_walk_max_earth, alpha=0.1, color='deepskyblue')

# 代表的ロボットの脚長を注釈
robots = {'SpaceBok': 0.35, 'ANYmal': 0.5, 'ATHLETE': 0.8}
for rname, rl in robots.items():
    ax.axvline(x=rl, color='white', linestyle=':', alpha=0.5)
    ax.annotate(rname, (rl, ax.get_ylim()[1]*0.9 if ax.get_ylim()[1] > 0 else 2.0),
                fontsize=9, ha='center', color='white',
                bbox=dict(boxstyle='round,pad=0.3', facecolor='gray', alpha=0.7))

ax.set_xlabel('Leg length [m]')
ax.set_ylabel('Max walking speed (Fr=0.5) [m/s]')
ax.set_title('Walk-to-Run Transition Speed vs Leg Length')
ax.legend(fontsize=11)
ax.grid(True, alpha=0.3)

# (2) 各ロボット×各天体の実用歩行速度域
ax = axes[1]
robot_data = {
    'SpaceBok\n(l=0.35m)': 0.35,
    'ANYmal\n(l=0.50m)': 0.50,
    'ATHLETE\n(l=0.80m)': 0.80,
}
envs = [('Earth', g_earth, 'deepskyblue'),
        ('Mars', g_mars, 'orangered'),
        ('Moon', g_moon, 'silver')]

x_pos = np.arange(len(robot_data))
width = 0.25

for idx, (env_name, g_val, color) in enumerate(envs):
    v_maxs = [np.sqrt(0.5 * g_val * ll) for ll in robot_data.values()]
    bars = ax.bar(x_pos + idx*width, v_maxs, width, color=color,
                  edgecolor='white', linewidth=1, label=env_name)
    for bar, v in zip(bars, v_maxs):
        ax.text(bar.get_x() + bar.get_width()/2., bar.get_height() + 0.02,
                f'{v:.2f}', ha='center', fontsize=8)

ax.set_xticks(x_pos + width)
ax.set_xticklabels(robot_data.keys(), fontsize=9)
ax.set_ylabel('Max walking speed [m/s]')
ax.set_title('Walking Speed Envelope by Robot × Environment')
ax.legend()
ax.grid(True, axis='y', alpha=0.3)

plt.tight_layout()
plt.savefig('gait_transition_analysis.png', dpi=150, bbox_inches='tight')
plt.show()

# 数値サマリ
print("=== 歩容遷移速度 (Fr=0.5) ===")
for rname, rl in robots.items():
    print(f"\n{rname} (leg={rl}m):")
    for env_name, g_val, _ in envs:
        v = np.sqrt(0.5 * g_val * rl)
        print(f"  {env_name}: {v:.2f} m/s ({v*3.6:.1f} km/h)")