gaitlab 0.1.0__py3-none-any.whl

gaitlab/__init__.py

@@ -0,0 +1,13 @@
+"""gaitlab — 実在ロボット (MuJoCo Menagerie) を CPU で動かすサンドボックス.
+
+四足歩行ロボ (Unitree Go2 等) と産業用アーム (Franka / UR5e / KUKA iiwa / Kinova)
+を、同じ 1 つのエンジンで土俵 / ワークセルに読み込み・駆動・描画する。
+ロボット相撲 / 格闘ゲーム / 生産ライン のデモを CPU だけで動かせる。
+
+公開 API はサブパッケージ ``gaitlab.arena`` にある::
+
+    from gaitlab.arena import robots, build_arena, MatchEngine
+"""
+from __future__ import annotations
+
+__version__ = "0.1.0"

gaitlab/arena/__init__.py

@@ -0,0 +1,43 @@
+"""robot-sumo / robot-fight arena — 実在ロボ (MuJoCo Menagerie) を土俵で戦わせる.
+
+gaitlab の歩行進化資産 (CPG/MAP-Elites) とは別軸の「実機モデルを動かす」トラック。
+Menagerie の四足 (Go2 等) は torque(motor) actuator なので、position servo を
+持たない → PD 制御 (torque = kp*(q_des-q) - kd*qdot) で立たせ・動かす。
+
+構成:
+    robots      RobotSpec + REGISTRY (Menagerie ロボの home pose 内省)
+    pd_control  PDController (関節空間 PD → トルク)
+    build       build_arena (MjSpec attach で N 体 + 土俵を 1 model へ)
+    gaits       Hold / Walk(trot) / SumoDrive コントローラ
+    match       MatchEngine (ringout / 転倒 判定 + 勝敗)
+    render      試合を GIF 化
+"""
+from __future__ import annotations
+
+from gaitlab.arena.build import ArenaIndex, RobotIndex, build_arena
+from gaitlab.arena.gaits import (
+    ArmMotionController,
+    HoldController,
+    SteerableDriveController,
+    SumoDriveController,
+    WalkController,
+)
+from gaitlab.arena.match import MatchEngine, MatchResult
+from gaitlab.arena.pd_control import PDController
+from gaitlab.arena.robots import REGISTRY, RobotSpec
+
+__all__ = [
+    "REGISTRY",
+    "RobotSpec",
+    "PDController",
+    "ArenaIndex",
+    "RobotIndex",
+    "build_arena",
+    "HoldController",
+    "WalkController",
+    "SumoDriveController",
+    "SteerableDriveController",
+    "ArmMotionController",
+    "MatchEngine",
+    "MatchResult",
+]

gaitlab/arena/build.py

@@ -0,0 +1,454 @@
+"""MjSpec attach で N 体のロボ + 土俵 (dohyo) を 1 つの MjModel に合成する.
+
+土俵 = 隆起シリンダー台 (top z=TOP_Z)。台から押し出されて落ちる = ringout を
+z で明確判定 (Menagerie survey で検証済みレシピ)。
+- 台: condim=3 / friction 高 (接触の condim/friction は両 geom の max → 足に摩擦)。
+- 見た目: clay の checker 材質 + 赤い境界リング + 俵 (視覚専用)。
+- Menagerie 四足は position(Spot/ANYmal/A1) と torque(Go2) が混在 → 制御は
+  match 側で actuator 種別に分岐。ここは合成と index 化のみ。
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+
+import mujoco
+import numpy as np
+
+from gaitlab.arena.robots import RobotSpec
+
+PLATFORM_HALF_H = 0.125  # 台の半高 → top = 2*half = 0.25 m の落差
+
+
+def _yaw_quat(yaw_deg: float) -> list[float]:
+    h = math.radians(yaw_deg) / 2.0
+    return [math.cos(h), 0.0, 0.0, math.sin(h)]
+
+
+@dataclass
+class RobotIndex:
+    """合成 model 内の 1 体を名前で解決したインデックス."""
+
+    prefix: str
+    spec_name: str
+    base_body: int
+    base_qadr: int
+    base_vadr: int
+    act_ids: np.ndarray
+    act_qadr: np.ndarray
+    act_vadr: np.ndarray
+    act_ctrlrange: np.ndarray  # (n, 2)
+    act_jntrange: np.ndarray  # (n, 2) 関節可動域 (q_des クランプ用; 無制限は ±inf)
+    act_leg: tuple[str, ...]  # 各アクチュエータの脚 (FL/FR/RL/RR/"")
+    act_role: tuple[str, ...]  # 各アクチュエータの役割 (abduction/thigh/calf/other)
+    home: np.ndarray
+    is_position: bool  # position(servo) 系なら ctrl=目標角、torque(motor) 系なら PD
+    stand_z: float  # 台上で立位したときの期待 base z (= top_z + home 立位高)
+    gait_sign: np.ndarray  # (n,) 各 thigh/calf の歩容符号 (足 Jacobian 由来。Go2=+1)
+    gait_dir: float  # 四足の大域前進方向 (+1/-1)。アームは未使用。
+    is_arm: bool  # 固定ベース (free joint 無し) のアームか
+    spawn: tuple[float, float, float, list[float]]
+
+    @property
+    def n_act(self) -> int:
+        return int(self.act_ids.shape[0])
+
+    def base_xy(self, data: mujoco.MjData) -> np.ndarray:
+        return np.asarray(data.xpos[self.base_body][:2], dtype=np.float64)
+
+    def base_height(self, data: mujoco.MjData) -> float:
+        return float(data.xpos[self.base_body][2])
+
+    def base_up(self, data: mujoco.MjData) -> float:
+        return float(data.xmat[self.base_body].reshape(3, 3)[2, 2])
+
+    def base_forward(self, data: mujoco.MjData) -> np.ndarray:
+        fwd = data.xmat[self.base_body].reshape(3, 3)[:, 0][:2]
+        n = np.linalg.norm(fwd)
+        return fwd / n if n > 1e-9 else np.array([1.0, 0.0])
+
+    def dist_from_center(self, data: mujoco.MjData) -> float:
+        return float(np.hypot(*self.base_xy(data)))
+
+    def read(self, data: mujoco.MjData) -> tuple[np.ndarray, np.ndarray]:
+        return (
+            np.asarray(data.qpos[self.act_qadr], dtype=np.float64),
+            np.asarray(data.qvel[self.act_vadr], dtype=np.float64),
+        )
+
+    def clamp_targets(self, q_des: np.ndarray) -> np.ndarray:
+        return np.clip(q_des, self.act_jntrange[:, 0], self.act_jntrange[:, 1])
+
+    def set_home(self, model: mujoco.MjModel, data: mujoco.MjData) -> None:
+        # 固定ベース (アーム) は base free joint が無い → base qpos は設定しない
+        # (位置は attach frame で確定済み)。関節のみ home へ。
+        if self.base_qadr >= 0:
+            x, y, z, quat = self.spawn
+            data.qpos[self.base_qadr : self.base_qadr + 3] = [x, y, z]
+            data.qpos[self.base_qadr + 3 : self.base_qadr + 7] = quat
+            data.qvel[self.base_vadr : self.base_vadr + 6] = 0.0
+        data.qpos[self.act_qadr] = self.home
+        data.qvel[self.act_vadr] = 0.0
+
+
+@dataclass
+class ArenaIndex:
+    model: mujoco.MjModel
+    robots: list[RobotIndex]
+    ring_radius: float
+    top_z: float
+    by_prefix: dict[str, RobotIndex] = field(default_factory=dict)
+
+    def __post_init__(self) -> None:
+        self.by_prefix = {r.prefix: r for r in self.robots}
+
+    def reset(self, data: mujoco.MjData) -> None:
+        mujoco.mj_resetData(self.model, data)
+        for r in self.robots:
+            r.set_home(self.model, data)
+        mujoco.mj_forward(self.model, data)
+
+
+def _classify_joint(base_name: str) -> tuple[str, str]:
+    """actuator 名 → (脚 FL/FR/RL/RR, 役割 abduction/thigh/calf/other).
+
+    命名: Go2/A1 = FL_hip/thigh/calf, ANYmal = LF_HAA/HFE/KFE,
+          Spot = fl_hx/hy/kn。未知は ("","other")。
+    """
+    n = base_name.lower()
+    # 脚 (前後×左右)。2 文字コードを正規化。
+    leg = ""
+    codes = {
+        "fl": "FL", "lf": "FL", "fr": "FR", "rf": "FR",
+        "rl": "RL", "lh": "RL", "hl": "RL", "rr": "RR", "rh": "RR", "hr": "RR",
+    }
+    for code, norm in codes.items():
+        if n.startswith(code + "_") or n.startswith(code):
+            leg = norm
+            break
+    # 役割
+    if any(k in n for k in ("calf", "knee", "kfe", "_kn")):
+        role = "calf"
+    elif any(k in n for k in ("thigh", "hfe", "_hy")):
+        role = "thigh"
+    elif any(k in n for k in ("hip", "haa", "_hx", "abduction", "abad")):
+        role = "abduction"
+    else:
+        role = "other"
+    return leg, role
+
+
+def _foot_geom(model: mujoco.MjModel, data: mujoco.MjData, calf_body: int) -> int | None:
+    """calf body に属す geom のうち最も低い (=足) を返す."""
+    gs = [g for g in range(model.ngeom) if int(model.geom_bodyid[g]) == calf_body]
+    if not gs:
+        return None
+    return min(gs, key=lambda g: float(data.geom_xpos[g][2]))
+
+
+def _compute_gait_signs(
+    model: mujoco.MjModel, base_qadr: int, base_z: float,
+    act_ids: np.ndarray, act_qadr: np.ndarray,
+    act_leg: tuple[str, ...], act_role: tuple[str, ...], home: np.ndarray,
+    delta: float = 0.05,
+) -> np.ndarray:
+    """各 thigh/calf の歩容符号を足 Jacobian(有限差分)から導出.
+
+    thigh_sign = -sign(d foot_x / d q_thigh), calf_sign = -sign(d foot_z / d q_calf)。
+    Go2 が全脚 +1 になる符号系。ロボの後脚が反転している場合 (ANYmal の X 字膝) は
+    その脚の符号が −1 になり、gait 側で乗じることで全脚の足先が同じ世界方向へ回る。
+    base は identity 向き (前方=+x) に固定して計測 (mj_forward は純 FK なので base は動かない)。
+    """
+    n = len(act_ids)
+    sign = np.ones(n, dtype=np.float64)
+    data = mujoco.MjData(model)
+    mujoco.mj_resetData(model, data)
+    # base を identity 姿勢へ、関節を home へ
+    data.qpos[base_qadr : base_qadr + 3] = [0.0, 0.0, base_z]
+    data.qpos[base_qadr + 3 : base_qadr + 7] = [1.0, 0.0, 0.0, 0.0]
+    data.qpos[act_qadr] = home
+    mujoco.mj_forward(model, data)
+
+    # 脚ごとに calf actuator の body から足 geom を得る
+    for i in range(n):
+        role = act_role[i]
+        if role not in ("thigh", "calf"):
+            continue
+        leg = act_leg[i]
+        calf_idx = [j for j in range(n) if act_leg[j] == leg and act_role[j] == "calf"]
+        if not calf_idx:
+            continue
+        calf_body = int(model.jnt_bodyid[model.actuator_trnid[act_ids[calf_idx[0]], 0]])
+        foot = _foot_geom(model, data, calf_body)
+        if foot is None:
+            continue
+        axis = 0 if role == "thigh" else 2  # thigh→foot_x, calf→foot_z
+        p0 = float(data.geom_xpos[foot][axis])
+        qa = int(act_qadr[i])
+        q_save = float(data.qpos[qa])
+        data.qpos[qa] = q_save + delta
+        mujoco.mj_forward(model, data)
+        p1 = float(data.geom_xpos[foot][axis])
+        data.qpos[qa] = q_save
+        mujoco.mj_forward(model, data)
+        d = p1 - p0
+        if abs(d) > 1e-9:
+            sign[i] = -float(np.sign(d))
+    return sign
+
+
+def _find_base_body(model: mujoco.MjModel, prefix: str) -> tuple[int, int]:
+    """(base body id, free joint id or -1) を返す.
+
+    四足 = prefix を持つ free joint の親 body。アーム(固定ベース) = free joint が
+    無いので、prefix を持ち親が world(0) の根 body を base とみなす (free jid=-1)。
+    """
+    for jid in range(model.njnt):
+        if int(model.jnt_type[jid]) != int(mujoco.mjtJoint.mjJNT_FREE):
+            continue
+        bid = int(model.jnt_bodyid[jid])
+        bname = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, bid) or ""
+        if bname.startswith(prefix):
+            return bid, jid
+    # 固定ベース: prefix を持ち親=world の根 body
+    for bid in range(1, model.nbody):
+        if int(model.body_parentid[bid]) != 0:
+            continue
+        bname = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, bid) or ""
+        if bname.startswith(prefix):
+            return bid, -1
+    raise ValueError(f"prefix '{prefix}' の base body が見つからない")
+
+
+def _index_robot(
+    model: mujoco.MjModel, prefix: str, spec: RobotSpec,
+    spawn: tuple[float, float, float, list[float]], top_z: float,
+) -> RobotIndex:
+    home_map = spec.home_targets()
+    base_body, base_free_jid = _find_base_body(model, prefix)
+    if base_free_jid >= 0:
+        base_qadr = int(model.jnt_qposadr[base_free_jid])
+        base_vadr = int(model.jnt_dofadr[base_free_jid])
+    else:
+        base_qadr = base_vadr = -1  # 固定ベース (アーム)
+
+    ids, qadr, vadr, ranges, jranges, home = [], [], [], [], [], []
+    legs, roles = [], []
+    n_position = 0
+    for aid in range(model.nu):
+        aname = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, aid) or ""
+        if not aname.startswith(prefix):
+            continue
+        base_name = aname[len(prefix):]
+        if base_name not in home_map:
+            raise KeyError(f"actuator '{aname}' の home が {spec.name} に無い")
+        jid = int(model.actuator_trnid[aid, 0])
+        ids.append(aid)
+        qadr.append(int(model.jnt_qposadr[jid]))
+        vadr.append(int(model.jnt_dofadr[jid]))
+        # ctrllimited でない actuator の ctrlrange は [0,0] のことがあり、
+        # そのまま clip すると制御が 0 に潰れる → 非制限は ±inf 扱い。
+        if int(model.actuator_ctrllimited[aid]):
+            ranges.append(model.actuator_ctrlrange[aid].tolist())
+        else:
+            ranges.append([-math.inf, math.inf])
+        if int(model.jnt_limited[jid]):
+            jranges.append(model.jnt_range[jid].tolist())
+        else:
+            jranges.append([-math.inf, math.inf])
+        home.append(home_map[base_name])
+        leg, role = _classify_joint(base_name)
+        legs.append(leg)
+        roles.append(role)
+        # position(servo): biastype=affine。torque(motor): biastype=none。
+        if int(model.actuator_biastype[aid]) == int(mujoco.mjtBias.mjBIAS_AFFINE):
+            n_position += 1
+
+    act_ids = np.asarray(ids, dtype=np.int32)
+    act_qadr = np.asarray(qadr, dtype=np.int32)
+    home_arr = np.asarray(home, dtype=np.float64)
+    # 歩容符号は四足のみ (アームは脚が無い → 全 1)
+    if spec.is_arm or base_qadr < 0:
+        gait_sign = np.ones(len(ids), dtype=np.float64)
+    else:
+        gait_sign = _compute_gait_signs(
+            model, base_qadr, spec.home_base_height(),
+            act_ids, act_qadr, tuple(legs), tuple(roles), home_arr,
+        )
+    return RobotIndex(
+        prefix=prefix, spec_name=spec.name, base_body=base_body,
+        base_qadr=base_qadr, base_vadr=base_vadr,
+        act_ids=act_ids, act_qadr=act_qadr,
+        act_vadr=np.asarray(vadr, dtype=np.int32),
+        act_ctrlrange=np.asarray(ranges, dtype=np.float64),
+        act_jntrange=np.asarray(jranges, dtype=np.float64),
+        act_leg=tuple(legs), act_role=tuple(roles),
+        home=home_arr,
+        # 過半が servo なら position 扱い (混在ロボは基本無い)
+        is_position=(n_position * 2 >= len(ids)),
+        stand_z=top_z + spec.home_base_height(),
+        gait_sign=gait_sign, gait_dir=spec.gait_dir, is_arm=spec.is_arm,
+        spawn=spawn,
+    )
+
+
+def _add_dohyo(spec: mujoco.MjSpec, ring_radius: float, top_z: float) -> None:
+    """隆起円形台 (dohyo) + 見た目 (checker clay / 赤リング / 俵) を追加."""
+    w = spec.worldbody
+    # 照明
+    for pos in ([0, 0, 3.2], [2.5, -2.5, 2.8]):
+        light = w.add_light()
+        light.pos = pos
+        light.dir = [-p for p in pos]
+    # 台下の地面 (落ちた先。ふつうの摩擦)
+    ground = w.add_geom()
+    ground.name = "ground"
+    ground.type = mujoco.mjtGeom.mjGEOM_PLANE
+    ground.size = [0.0, 0.0, 0.05]
+    ground.condim = 3
+    ground.friction = [1.0, 0.005, 0.0001]
+    ground.rgba = [0.24, 0.27, 0.32, 1.0]
+
+    # 台本体 (clay の checker 材質; 失敗時は素の rgba)
+    material_name = None
+    try:
+        tex = spec.add_texture()
+        tex.name = "dohyo_tex"
+        tex.type = mujoco.mjtTexture.mjTEXTURE_2D
+        tex.builtin = mujoco.mjtBuiltin.mjBUILTIN_CHECKER
+        tex.rgb1 = [0.86, 0.75, 0.55]
+        tex.rgb2 = [0.80, 0.67, 0.46]
+        tex.width = 256
+        tex.height = 256
+        mat = spec.add_material()
+        mat.name = "dohyo_mat"
+        mat.textures[mujoco.mjtTextureRole.mjTEXROLE_RGB] = "dohyo_tex"
+        mat.texrepeat = [4, 4]
+        material_name = "dohyo_mat"
+    except Exception:  # noqa: BLE001  材質 API 差異でも素の rgba で続行
+        material_name = None
+
+    dohyo = w.add_geom()
+    dohyo.name = "dohyo"
+    dohyo.type = mujoco.mjtGeom.mjGEOM_CYLINDER
+    dohyo.size = [ring_radius, top_z / 2.0, 0.0]
+    dohyo.pos = [0.0, 0.0, top_z / 2.0]
+    dohyo.condim = 3
+    dohyo.friction = [1.5, 0.02, 0.0002]
+    if material_name:
+        dohyo.material = material_name
+    else:
+        dohyo.rgba = [0.83, 0.71, 0.50, 1.0]
+
+    # 赤い境界リング (annulus トリック: 赤ディスク + clay 埋めディスク、視覚専用)
+    band = w.add_geom()
+    band.name = "ring_band"
+    band.type = mujoco.mjtGeom.mjGEOM_CYLINDER
+    band.size = [ring_radius, 0.002, 0.0]
+    band.pos = [0.0, 0.0, top_z + 0.002]
+    band.contype = 0
+    band.conaffinity = 0
+    band.rgba = [0.82, 0.14, 0.11, 1.0]
+
+    infill = w.add_geom()
+    infill.name = "ring_infill"
+    infill.type = mujoco.mjtGeom.mjGEOM_CYLINDER
+    infill.size = [ring_radius - 0.08, 0.002, 0.0]
+    infill.pos = [0.0, 0.0, top_z + 0.004]
+    infill.contype = 0
+    infill.conaffinity = 0
+    if material_name:
+        infill.material = material_name
+    else:
+        infill.rgba = [0.83, 0.71, 0.50, 1.0]
+
+    # 俵 (視覚専用の縁): 円周に接する短カプセル
+    n_bales = 24
+    for i in range(n_bales):
+        a = 2.0 * math.pi * i / n_bales
+        cx, cy = (ring_radius - 0.02) * math.cos(a), (ring_radius - 0.02) * math.sin(a)
+        tx, ty = -math.sin(a), math.cos(a)
+        hl = math.pi * ring_radius / n_bales * 0.95
+        bale = w.add_geom()
+        bale.name = f"tawara{i}"
+        bale.type = mujoco.mjtGeom.mjGEOM_CAPSULE
+        bale.size = [0.018, 0.0, 0.0]
+        bale.fromto = [cx - hl * tx, cy - hl * ty, top_z + 0.01,
+                       cx + hl * tx, cy + hl * ty, top_z + 0.01]
+        bale.contype = 0
+        bale.conaffinity = 0
+        bale.rgba = [0.78, 0.68, 0.40, 1.0]
+
+
+def _add_floor(spec: mujoco.MjSpec) -> None:
+    """土俵なしの平床 + 照明 (アーム/ワークセル用)."""
+    w = spec.worldbody
+    for pos in ([0, 0, 3.2], [2.5, -2.5, 2.8]):
+        light = w.add_light()
+        light.pos = pos
+        light.dir = [-p for p in pos]
+    floor = w.add_geom()
+    floor.name = "floor"
+    floor.type = mujoco.mjtGeom.mjGEOM_PLANE
+    floor.size = [0.0, 0.0, 0.05]
+    floor.condim = 3
+    floor.friction = [1.0, 0.005, 0.0001]
+    floor.rgba = [0.42, 0.44, 0.48, 1.0]
+
+
+def build_arena(
+    placements: list[tuple[RobotSpec, str, float, float, float]],
+    ring_radius: float = 1.4,
+    timestep: float = 0.002,
+    top_z: float = 2.0 * PLATFORM_HALF_H,
+    offwidth: int = 1280,
+    offheight: int = 800,
+    dohyo: bool = True,
+) -> tuple[mujoco.MjModel, ArenaIndex]:
+    """placements = [(spec, prefix, x, y, yaw_deg), ...] を配置し合成.
+
+    dohyo=True: 隆起土俵 (四足相撲)、四足は台上 (z=spawn_height+top_z) に出生。
+    dohyo=False: 平床 (アーム/ワークセル)、robot は z=spawn_height に固定配置。
+    アームは固定ベースなので dohyo に関わらず top_z を足さない。
+    returns (model, ArenaIndex)。
+    """
+    spec0 = mujoco.MjSpec()
+    spec0.option.timestep = timestep
+    spec0.option.impratio = 100.0
+    spec0.option.cone = mujoco.mjtCone.mjCONE_ELLIPTIC
+    # implicitfast: スティフな position サーボ(産業用アーム gain~4500)を安定積分
+    # (Euler だと発散して MuJoCo が毎ステップ自動リセット→時間が進まない)。四足も可。
+    spec0.option.integrator = mujoco.mjtIntegrator.mjINT_IMPLICITFAST
+    spec0.visual.headlight.diffuse = [0.5, 0.5, 0.5]
+    spec0.visual.headlight.ambient = [0.4, 0.4, 0.4]
+    try:
+        spec0.visual.global_.offwidth = offwidth
+        spec0.visual.global_.offheight = offheight
+    except Exception:  # noqa: BLE001
+        pass
+
+    if dohyo:
+        _add_dohyo(spec0, ring_radius, top_z)
+    else:
+        _add_floor(spec0)
+        top_z = 0.0
+
+    spawns: list[tuple[str, RobotSpec, tuple[float, float, float, list[float]]]] = []
+    for spec, prefix, x, y, yaw in placements:
+        child = mujoco.MjSpec.from_file(spec.robot_path)
+        quat = _yaw_quat(yaw)
+        # アーム(固定ベース)は台上に載せず床基準。四足は台上へ。
+        z = spec.spawn_height + (0.0 if spec.is_arm else top_z)
+        frame = spec0.worldbody.add_frame()
+        frame.pos = [x, y, z]
+        frame.quat = quat
+        spec0.attach(child, prefix=prefix, frame=frame)
+        spawns.append((prefix, spec, (x, y, z, quat)))
+
+    model = spec0.compile()
+    robots = [
+        _index_robot(model, prefix, spec, spawn, top_z) for prefix, spec, spawn in spawns
+    ]
+    return model, ArenaIndex(
+        model=model, robots=robots, ring_radius=ring_radius, top_z=top_z,
+    )

gaitlab/arena/gaits.py

@@ -0,0 +1,173 @@
+"""土俵用コントローラ: 各アクチュエータの目標角 q_des を返す (home からの合成).
+
+match 側で actuator 種別に応じて
+    position(servo): ctrl = clamp(q_des, ctrlrange)
+    torque(motor):   ctrl = PD(q, qdot, clamp(q_des, jntrange), ctrlrange)
+に変換する。ここは開ループの姿勢/歩容生成のみ。
+
+前進 gait の要 (実測): 対角トロットで calf を thigh より +pi/2 進める
+(足先が楕円を描き前方へ蹴る)。位相差 -pi/2 や pi は後退。freq2.5/At0.45/Ac0.20
+で Go2 は約 0.48 m/s 前進・転倒なし (kp60/kd1.5)。
+
+survey 知見: amp 上限 thigh<=0.45, calf<=0.28、amp は ramp_time で 0 から立上げ。
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+
+import numpy as np
+
+from gaitlab.arena.build import RobotIndex
+
+# 対角トロットの脚位相
+_LEG_PHASE = {"FL": 0.0, "RR": 0.0, "FR": math.pi, "RL": math.pi}
+# calf を thigh より進める位相差 (前進の要)
+_FWD_CALF_LEAD = math.pi / 2.0
+
+
+def _ramp(t: float, ramp_time: float) -> float:
+    if ramp_time <= 0:
+        return 1.0
+    return float(min(1.0, max(0.0, t / ramp_time)))
+
+
+@dataclass
+class HoldController:
+    """home 姿勢で立つだけ (整定/待機)."""
+
+    def targets(self, t: float, idx: RobotIndex) -> np.ndarray:  # noqa: ARG002
+        return idx.home.copy()
+
+
+@dataclass
+class WalkController:
+    """home 周りの対角トロットで前進する開ループ歩容 (calf を +pi/2 先行)."""
+
+    freq: float = 2.5
+    amp_thigh: float = 0.40
+    amp_calf: float = 0.20
+    direction: float = 1.0  # +1=前進, -1=後退
+    ramp_time: float = 0.5
+
+    def targets(self, t: float, idx: RobotIndex) -> np.ndarray:
+        q = idx.home.copy()
+        r = _ramp(t, self.ramp_time)
+        at = min(self.amp_thigh, 0.45) * r
+        ac = min(self.amp_calf, 0.28) * r
+        w = 2.0 * math.pi * self.freq
+        lead = _FWD_CALF_LEAD * self.direction * idx.gait_dir
+        for i, (leg, role) in enumerate(zip(idx.act_leg, idx.act_role)):
+            ph = _LEG_PHASE.get(leg, 0.0)
+            sg = float(idx.gait_sign[i])
+            if role == "thigh":
+                q[i] += sg * at * math.sin(w * t + ph)
+            elif role == "calf":
+                q[i] += sg * ac * math.sin(w * t + ph + lead)
+        return q
+
+
+@dataclass
+class SteerableDriveController:
+    """操作可能ドライブ (格闘ゲーム用): forward(前後) と turn(旋回) を毎フレーム更新.
+
+    forward=0 で立位、±で前後トロット、turn で左右脚の歩幅を非対称にして veer。
+    ゲームループが self.forward / self.turn を書き換える。
+    """
+
+    forward: float = 0.0  # -1..+1
+    turn: float = 0.0  # -1..+1
+    freq: float = 2.6
+    amp_thigh: float = 0.42
+    amp_calf: float = 0.22
+    crouch: float = 0.0  # >0 でしゃがむ (押し合いで低重心)
+    ramp_time: float = 0.4
+
+    def targets(self, t: float, idx: RobotIndex) -> np.ndarray:
+        q = idx.home.copy()
+        r = _ramp(t, self.ramp_time)
+        w = 2.0 * math.pi * self.freq
+        fwd = float(np.clip(self.forward, -1.0, 1.0))
+        turn = float(np.clip(self.turn, -1.0, 1.0))
+        base_at = min(self.amp_thigh, 0.45) * r
+        base_ac = min(self.amp_calf, 0.28) * r
+        for i, (leg, role) in enumerate(zip(idx.act_leg, idx.act_role)):
+            ph = _LEG_PHASE.get(leg, 0.0)
+            is_left = leg in ("FL", "RL")
+            # 脚ごとのドライブ = 前後成分 + 旋回差動 (左右で逆符号)。
+            # forward=0 でも turn があれば左右が逆に踏んでその場旋回する。
+            d_leg = float(np.clip(fwd + (turn if is_left else -turn), -1.0, 1.0))
+            mag = abs(d_leg)
+            lead = _FWD_CALF_LEAD * (1.0 if d_leg >= 0 else -1.0) * idx.gait_dir
+            sg = float(idx.gait_sign[i])
+            if role == "thigh":
+                q[i] += sg * base_at * mag * math.sin(w * t + ph)
+            elif role == "calf":
+                q[i] += sg * (base_ac * mag * math.sin(w * t + ph + lead) - r * self.crouch)
+        return q
+
+
+@dataclass
+class ArmMotionController:
+    """産業用アーム(固定ベース)の作業動作: 関節を段階位相の正弦波で動かす.
+
+    home 姿勢を中心に各関節へ振幅 amplitude・位相ずらし phase_stagger の正弦を足し、
+    「動いている作業ロボ」を作る。任意のアーム(Franka/UR5e/iiwa/Kinova…)に汎用。
+    match 側で position 制御なら ctrl=目標角、torque なら PD で追従。関節端は
+    RobotIndex.clamp_targets でクランプされる。生産ライン AI 化への土台(現状は
+    開ループ動作。学習/タスク制御は後段)。
+    """
+
+    period: float = 4.0  # 1 周期[s]
+    amplitude: float = 0.45  # 各関節の振幅[rad]
+    phase_stagger: float = 0.9  # 関節間の位相ずらし (波打つ動き)
+    phase0: float = 0.0  # 個体ごとの初期位相 (複数アームをずらす用)
+    ramp_time: float = 0.5
+
+    def targets(self, t: float, idx: RobotIndex) -> np.ndarray:
+        q = idx.home.copy()
+        r = _ramp(t, self.ramp_time)
+        w = 2.0 * math.pi / max(self.period, 1e-6)
+        for i in range(idx.n_act):
+            q[i] += r * self.amplitude * math.sin(w * t + self.phase0 + i * self.phase_stagger)
+        return q
+
+
+@dataclass
+class SumoDriveController:
+    """相撲: 低く構えた前進トロット + くさび姿勢 (前脚短く nose-down) で押し出す.
+
+    home からのオフセット表現で position/torque 両対応。aggression で押す強さを
+    調整 (Go2 同士の膠着を崩すのに左右で差をつける)。
+    """
+
+    aggression: float = 1.0  # 1.0=標準。>1 でより強く速く前へ。
+    crouch_front_calf: float = -0.25  # 前脚を短く (nose down + 低重心)
+    crouch_rear_thigh: float = 0.15  # 後脚で踏ん張る
+    wide: float = 0.08  # abduction 外向き (支持多角形拡大)
+    freq: float = 2.6
+    amp_thigh: float = 0.42
+    amp_calf: float = 0.22
+    ramp_time: float = 0.6
+
+    def targets(self, t: float, idx: RobotIndex) -> np.ndarray:
+        q = idx.home.copy()
+        r = _ramp(t, self.ramp_time)
+        w = 2.0 * math.pi * self.freq
+        at = min(self.amp_thigh * self.aggression, 0.45) * r
+        ac = min(self.amp_calf * self.aggression, 0.28) * r
+        lead = _FWD_CALF_LEAD * idx.gait_dir  # 前進 (ロボごとの大域方向)
+        for i, (leg, role) in enumerate(zip(idx.act_leg, idx.act_role)):
+            is_front = leg in ("FL", "FR")
+            is_left = leg in ("FL", "RL")
+            ph = _LEG_PHASE.get(leg, 0.0)
+            sg = float(idx.gait_sign[i])
+            if role == "abduction":
+                q[i] += r * self.wide * (1.0 if is_left else -1.0)
+            elif role == "thigh":
+                bias = 0.0 if is_front else self.crouch_rear_thigh
+                q[i] += sg * (r * bias + at * math.sin(w * t + ph))
+            elif role == "calf":
+                bias = self.crouch_front_calf if is_front else 0.0
+                q[i] += sg * (r * bias + ac * math.sin(w * t + ph + lead))
+        return q