#!/usr/bin/env python3
"""RS06 concept kinematics and point-load torque mapping; not a robot simulation.

Coordinates X forward, Y left, Z up. Lengths internal to this script are metres.
q = (outward HAA, HFE about common +Y, relative knee about common +Y), radians.
The KFE stator is fixed to the HAA carrier. Equal, open-belt pulleys give
motor_coordinates = (qA, qH, qH+qK), before hardware encoder signs/zeros.
"""
from __future__ import annotations

import hashlib
import json
import math
from pathlib import Path

HERE = Path(__file__).resolve().parent
LEGS = {"FL": (1, 1), "FR": (1, -1), "RL": (-1, 1), "RR": (-1, -1)}
G = 9.81


def dot(a, b):
    return sum(x * y for x, y in zip(a, b))


def cross(a, b):
    return [a[1] * b[2] - a[2] * b[1],
            a[2] * b[0] - a[0] * b[2],
            a[0] * b[1] - a[1] * b[0]]


def transpose_times(jacobian, force):
    return [sum(jacobian[row][col] * force[row] for row in range(3)) for col in range(3)]


def forward(cfg, leg, q, haa_z):
    fore, side = LEGS[leg]
    a, h, k = q
    l1, l2 = cfg["upper_length_mm"] / 1000, cfg["lower_length_mm"] / 1000
    d = (cfg["sagittal_y_mm"] - cfg["haa_y_mm"]) / 1000
    x = -l1 * math.sin(h) - l2 * math.sin(h + k)
    z = -l1 * math.cos(h) - l2 * math.cos(h + k)
    return [fore * cfg["hip_x_mm"] / 1000 + x,
            side * (cfg["haa_y_mm"] / 1000 + d * math.cos(a) - z * math.sin(a)),
            haa_z + d * math.sin(a) + z * math.cos(a)]


def forward_by_rotations(cfg, leg, q, haa_z):
    """Independent rotation-composition implementation used only for checks."""
    fore, side = LEGS[leg]
    a, h, k = q
    def ry(angle, vector):
        x, y, z = vector
        return [math.cos(angle) * x + math.sin(angle) * z, y,
                -math.sin(angle) * x + math.cos(angle) * z]
    def rx(angle, vector):
        x, y, z = vector
        return [x, math.cos(angle) * y - math.sin(angle) * z,
                math.sin(angle) * y + math.cos(angle) * z]
    upper = ry(h, [0, 0, -cfg["upper_length_mm"] / 1000])
    lower = ry(h + k, [0, 0, -cfg["lower_length_mm"] / 1000])
    offset = [0, side * (cfg["sagittal_y_mm"] - cfg["haa_y_mm"]) / 1000, 0]
    vector = rx(side * a, [sum(values) for values in zip(upper, lower, offset)])
    base = [fore * cfg["hip_x_mm"] / 1000, side * cfg["haa_y_mm"] / 1000, haa_z]
    return [x + y for x, y in zip(base, vector)]


def jacobian(cfg, leg, q):
    """Analytic d(foot position)/d(outward abduction, hip, relative knee)."""
    _, side = LEGS[leg]
    a, h, k = q
    l1, l2 = cfg["upper_length_mm"] / 1000, cfg["lower_length_mm"] / 1000
    d = (cfg["sagittal_y_mm"] - cfg["haa_y_mm"]) / 1000
    x = -l1 * math.sin(h) - l2 * math.sin(h + k)
    z = -l1 * math.cos(h) - l2 * math.cos(h + k)
    dx_dk, dz_dk = -l2 * math.cos(h + k), l2 * math.sin(h + k)
    return [[0, z, dx_dk],
            [side * (-d * math.sin(a) - z * math.cos(a)),
             side * math.sin(a) * x, -side * math.sin(a) * dz_dk],
            [d * math.cos(a) - z * math.sin(a),
             -math.cos(a) * x, math.cos(a) * dz_dk]]


def finite_difference_jacobian(cfg, leg, q, haa_z, epsilon=1e-6):
    columns = []
    for axis in range(3):
        plus, minus = list(q), list(q)
        plus[axis] += epsilon
        minus[axis] -= epsilon
        fp, fm = forward(cfg, leg, plus, haa_z), forward(cfg, leg, minus, haa_z)
        columns.append([(x - y) / (2 * epsilon) for x, y in zip(fp, fm)])
    return [list(row) for row in zip(*columns)]


def motor_torques(joint_torques):
    a, h, k = joint_torques
    return [a, h - k, k]


def torques_by_moment_arms(cfg, leg, q, haa_z, force):
    """Independent world-space axis projection of (foot-origin) cross force."""
    fore, side = LEGS[leg]
    a, h, _ = q
    foot = forward_by_rotations(cfg, leg, q, haa_z)
    hip = [fore * cfg["hip_x_mm"] / 1000, side * cfg["haa_y_mm"] / 1000, haa_z]
    l1 = cfg["upper_length_mm"] / 1000
    upper = [-l1 * math.sin(h), side * l1 * math.cos(h) * math.sin(a),
             -l1 * math.cos(h) * math.cos(a)]
    knee = [v + w for v, w in zip(hip, upper)]
    haa_axis = [side, 0, 0]
    pitch_axis = [0, math.cos(a), side * math.sin(a)]
    # Origins can move along their own axes without changing axial moments.
    # These equivalent origins omit the irrelevant axial offsets of the motor faces.
    return [dot(axis, cross([v - w for v, w in zip(foot, origin)], force))
            for axis, origin in [(haa_axis, hip), (pitch_axis, hip), (pitch_axis, knee)]]


def validate_parameter_consistency(parameters):
    """Prevent a stale kinematics block from describing different CAD inputs."""
    cfg = parameters["kinematics"]
    equivalents = {
        "upper_length_mm": parameters["leg"]["upper_length_mm"],
        "lower_length_mm": parameters["leg"]["lower_length_mm"],
        "foot_radius_mm": parameters["leg"]["foot_radius_mm"],
        "haa_y_mm": parameters["body"]["haa_y_mm"],
        "sagittal_y_mm": parameters["leg"]["lower_center_y_mm"],
        "hip_x_mm": parameters["body"]["pitch_axis_x_mm"],
        "abduction_deg": parameters["pose"]["haa_outward_deg"],
        "hip_deg": parameters["pose"]["hip_deg"],
        "knee_deg": parameters["pose"]["knee_relative_deg"],
    }
    mismatches = {name: [cfg[name], expected] for name, expected in equivalents.items()
                  if not math.isclose(cfg[name], expected, abs_tol=1e-12)}
    if mismatches:
        raise ValueError("Kinematic inputs disagree with layout parameters: " + str(mismatches))


def grounded_haa_height(cfg, q):
    return cfg["foot_radius_mm"] / 1000 - forward(cfg, "FL", q, 0)[2]


def hull(points):
    points = sorted(set(tuple(p) for p in points))
    if len(points) < 3:
        return points
    def cross(o, a, b):
        return (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0])
    lower, upper = [], []
    for point in points:
        while len(lower) >= 2 and cross(lower[-2], lower[-1], point) <= 0:
            lower.pop()
        lower.append(point)
    for point in reversed(points):
        while len(upper) >= 2 and cross(upper[-2], upper[-1], point) <= 0:
            upper.pop()
        upper.append(point)
    return lower[:-1] + upper[:-1]


def make_pose(cfg, angles, haa_z, description):
    feet = {leg: forward(cfg, leg, q, haa_z) for leg, q in angles.items()}
    contact_legs = [leg for leg, foot in feet.items()
                    if abs(foot[2] - cfg["foot_radius_mm"] / 1000) < 1e-8]
    contact_xy = [[1000 * feet[leg][0], 1000 * feet[leg][1]] for leg in contact_legs]
    xs, ys = [v[0] * 1000 for v in feet.values()], [v[1] * 1000 for v in feet.values()]
    return {"description": description, "haa_axis_height_mm": haa_z * 1000,
            "joint_angles_deg": {leg: list(map(math.degrees, q)) for leg, q in angles.items()},
            "foot_centers_mm": {leg: [v * 1000 for v in foot] for leg, foot in feet.items()},
            "foot_lift_above_ground_mm": {leg: foot[2] * 1000 - cfg["foot_radius_mm"] for leg, foot in feet.items()},
            "geometric_contact_legs": contact_legs, "contact_polygon_xy_mm": hull(contact_xy),
            "foot_center_bounds_xy_mm": {"x_min": min(xs), "x_max": max(xs), "y_min": min(ys), "y_max": max(ys),
                                          "length": max(xs) - min(xs), "width": max(ys) - min(ys)},
            "jacobians_m_per_rad": {leg: jacobian(cfg, leg, q) for leg, q in angles.items()},
            "collision_checked": False, "stability_or_gait_validated": False}


def validate(cfg, poses):
    checks = []
    force, qdot = [17.0, -9.0, 23.0], [0.13, -0.21, 0.17]
    samples = [(leg, list(map(math.radians, pose["joint_angles_deg"][leg])), pose["haa_axis_height_mm"] / 1000)
               for pose in poses.values() for leg in LEGS]
    for leg in LEGS:
        for degrees in [(0, 0, -60), (-10, 20, -110), (20, 70, -140), (15, -25, 45)]:
            samples.append((leg, list(map(math.radians, degrees)), 0.3))
    for leg, q, haa_z in samples:
        j = jacobian(cfg, leg, q)
        fd = finite_difference_jacobian(cfg, leg, q, haa_z)
        tau_joint = transpose_times(j, force)
        tau_moment = torques_by_moment_arms(cfg, leg, q, haa_z, force)
        tau_motor = motor_torques(tau_joint)
        motor_rates = [qdot[0], qdot[1], qdot[1] + qdot[2]]
        eps = 1e-6
        plus = forward(cfg, leg, [a + eps * b for a, b in zip(q, qdot)], haa_z)
        minus = forward(cfg, leg, [a - eps * b for a, b in zip(q, qdot)], haa_z)
        velocity = [(a - b) / (2 * eps) for a, b in zip(plus, minus)]
        p_joint, p_motor, p_fd = dot(tau_joint, qdot), dot(tau_motor, motor_rates), dot(force, velocity)
        checks.append({"leg": leg, "angles_rad": q,
                       "fk_rotation_error_m": max(abs(a-b) for a, b in zip(forward(cfg, leg, q, haa_z), forward_by_rotations(cfg, leg, q, haa_z))),
                       "jacobian_error_m_per_rad": max(abs(a-b) for x, y in zip(j, fd) for a, b in zip(x, y)),
                       "world_moment_torque_error_Nm": max(abs(a-b) for a, b in zip(tau_joint, tau_moment)),
                       "virtual_work_fd_error_W": abs(p_joint - p_fd),
                       "joint_motor_power_error_W": abs(p_joint - p_motor)})
    limits = {"fk_rotation_error_m": 1e-12, "jacobian_error_m_per_rad": 1e-8,
              "world_moment_torque_error_Nm": 1e-12,
              "virtual_work_fd_error_W": 1e-7, "joint_motor_power_error_W": 1e-12}
    maximum = {key: max(row[key] for row in checks) for key in limits}
    passed = all(maximum[key] < limit for key, limit in limits.items())
    if not passed:
        raise ValueError("Kinematic numerical crosscheck failed: " + str(maximum))
    return {"passed": passed, "samples": len(checks), "maximum_errors": maximum,
            "tolerances": limits, "finite_difference_step_rad": 1e-6,
            "method": "Analytic Jacobian versus central FK differences; FK versus rotation composition; J^T F versus world-space axis-projected moment arms; J^T F power versus directional finite difference and coupled motor power."}


def main():
    parameter_file = HERE / "parameters.json"
    parameters = json.loads(parameter_file.read_text())
    validate_parameter_consistency(parameters)
    cfg = parameters["kinematics"]
    stand_q = list(map(math.radians, [cfg["abduction_deg"], cfg["hip_deg"], cfg["knee_deg"]]))
    stand_z = grounded_haa_height(cfg, stand_q)
    stand_angles = {leg: stand_q[:] for leg in LEGS}
    crouch_q = list(map(math.radians, [cfg["abduction_deg"], 60, -120]))
    raised_angles = {leg: q[:] for leg, q in stand_angles.items()}
    raised_angles["FL"] = list(map(math.radians, [cfg["abduction_deg"], 20, -115]))
    poses = {
        "stand": make_pose(cfg, stand_angles, stand_z, "Nominal layout pose. Body height solved for four foot spheres tangent to z=0."),
        "crouch": make_pose(cfg, {leg: crouch_q[:] for leg in LEGS}, grounded_haa_height(cfg, crouch_q),
                            "Geometric crouch: body height lowered to retain foot tangency; not a commanded or validated motion."),
        "raised_FL": make_pose(cfg, raised_angles, stand_z, "FL pose changed at fixed body height; three other geometric contacts. No gait, stability, collision or force-sharing result.")}
    belt = parameters["belt"]
    radius_m = belt["teeth_each"] * belt["pitch_mm"] / (2 * math.pi * 1000)
    center_m = (belt["pitch_length_mm"] - belt["teeth_each"] * belt["pitch_mm"]) / 2000
    assert abs(center_m * 1000 - cfg["upper_length_mm"]) < 1e-9
    cases = []
    for pose_name in ["stand", "crouch"]:
        pose = poses[pose_name]
        for mass in [10, 12, 14]:
            force = [0, 0, mass * G / 2]
            for leg in LEGS:
                joint = transpose_times(pose["jacobians_m_per_rad"][leg], force)
                motor = motor_torques(joint)
                cases.append({"pose": pose_name, "leg": leg, "assumed_total_mass_kg": mass,
                              "assumed_supporting_feet": 2, "world_point_force_N": force,
                              "generalized_load_torque_nm": dict(zip(["HAA_outward", "HFE_joint", "knee_relative"], joint)),
                              "actuator_load_torque_nm": dict(zip(["HAA_outward", "HFE_commonY", "KFE_commonY"], motor)),
                              "belt_tension_difference_N": abs(motor[2]) / radius_m,
                              "largest_abs_actuator_load_nm": max(map(abs, motor)),
                              "catalog_stall_8Nm_exceeded": any(abs(v) > 8 for v in motor)})
    validation = validate(cfg, poses)
    result = {
        "revision": parameters["revision"], "checked_on": "2026-10-28", "status": "kinematic and point-force screening only",
        "source_sha256": {name: hashlib.sha256((HERE / name).read_bytes()).hexdigest() for name in ["parameters.json", "kinematics.py"]},
        "coordinate_convention": {"world": "X forward, Y left, Z up", "joint_order": ["outward HAA", "HFE about +Y", "relative knee about +Y"],
                                  "right_HAA": "Positive outward qA is rotation about global -X; left is global +X.",
                                  "motor_coordinates": ["qA (local outward)", "qH (common +Y)", "qH+qK (common +Y)"],
                                  "encoder_conversion": "Not calibrated. Opposed outputs and mirrored HAA need hardware-specific signs and zeros.",
                                  "torque_sign": "J^T F is generalized load from the stated upward foot force; balancing actuator torque has opposite sign if all other loads are omitted."},
        "nominal_haa_height_mm": stand_z * 1000, "poses": poses,
        "belt": {"profile": belt["profile"], "pitch_mm": belt["pitch_mm"], "teeth_each": belt["teeth_each"],
                 "width_mm": belt["width_mm"], "pitch_radius_mm": radius_m * 1000, "pitch_length_mm": belt["pitch_length_mm"],
                 "center_distance_mm": center_m * 1000, "ideal_wrap_deg": 180,
                 "tension_difference_at_catalog_peak_36Nm_N": 36 / radius_m,
                 "capacity_verified": False, "note": "Tension difference only. Pretension, tight/slack tension, tooth capacity, bearing loads, efficiency and tensioner travel unmodeled."},
        "point_force_cases": cases, "validation": validation,
        "limitations": ["10/12/14 kg are sensitivity assumptions, not measured or achieved robot mass.",
                        "Each leg row is an independent mg/2 load case, not four simultaneous mg/2 contact forces.",
                        "Two equal vertical support forces do not establish body equilibrium, a feasible gait or stable three-contact raised-leg motion.",
                        "Only the nominal point-foot geometry and ideal open, equal-pulley transmission are represented.",
                        "Motor/link self-weight, inertia, friction, horizontal forces, structural deformation, backlash, belt pretension, thermal duty and power electronics omitted.",
                        "8 Nm catalog stall and 36 Nm peak are manufacturer-condition values, not verified continuous or transient capability in this mount.",
                        "Explored poses are geometric examples, not validated joint limits, collision-free ranges or physical experiments."]}
    (HERE / "analysis.json").write_text(json.dumps(result, ensure_ascii=False, indent=2) + "\n")
    s12 = next(c for c in cases if c["pose"] == "stand" and c["leg"] == "FL" and c["assumed_total_mass_kg"] == 12)
    c12 = next(c for c in cases if c["pose"] == "crouch" and c["leg"] == "FL" and c["assumed_total_mass_kg"] == 12)
    torque = s12["actuator_load_torque_nm"]
    bounds = poses["stand"]["foot_center_bounds_xy_mm"]
    report = f'''# RS06 배치의 기구 계산

2026-10-28. **서 있는 배치의 발 중심 간격은 앞뒤 {bounds["length"]:.0f} mm, 좌우 {bounds["width"]:.1f} mm다.** 12 kg·두 발 하중 분담을 가정한 모터 토크 크기는 HAA {abs(torque["HAA_outward"]):.2f}, HFE {abs(torque["HFE_commonY"]):.2f}, KFE {abs(torque["KFE_commonY"]):.2f} Nm다. 12 kg은 달성 질량이 아니다.

| 확인한 항목 | 결과 |
| --- | --- |
| 기준 자세 | 상퇴 {cfg["upper_length_mm"]:g} / 하퇴 {cfg["lower_length_mm"]:g} mm, 벌림 {cfg["abduction_deg"]:g}°, hip {cfg["hip_deg"]:g}°, 상대 무릎 {cfg["knee_deg"]:g}° |
| HAA 축 높이 | {stand_z * 1000:.2f} mm · 반지름 20 mm 발이 바닥에 닿도록 계산 |
| 무릎 전달 | 벌림 캐리어에 고정된 근위 KFE 모터, 같은 40T 풀리, S5M 600 mm 벨트·폭 25 mm, 축간 200 mm |
| 모터각 | HFE = hip, KFE = hip + 상대 무릎각 · 실제 엔코더 부호/영점은 별도 |
| 모터 토크 변환 | HFE = 관절 hip 토크 − 무릎 토크, KFE = 무릎 토크 |
| 36 Nm에서 벨트 장력 차 | {36/radius_m:.0f} N · 초기 장력과 벨트 허용하중은 미검증 |

**쪼그린 자세는 부하가 커진다.** hip 60°·무릎 −120°에서 몸체를 낮춰 발을 바닥에 두면, 같은 12 kg·두 발 조건의 최대 모터 토크는 {c12["largest_abs_actuator_load_nm"]:.2f} Nm다. 카탈로그 정지 유지 8 Nm를 넘으므로 이 자세를 두 발로 지속 유지할 수 있다고 판정하지 않는다. 기본 자세도 실제 방열·자중·동적 하중 검증이 남아 있다.

기립·쪼그림·왼쪽 앞발 들기 세 자세의 발 위치와 야코비안을 계산했다. 발 들기는 지지력 배분이나 안정성 계산이 아니다. 접촉 다각형은 발 위치의 기하 정보이며 전신 간섭·보행·강성 시험 결과가 아니다.

유한차분, 별도의 회전행렬 위치 계산, 힘과 모멘트암의 외적으로 구한 토크, 관절/모터의 가상일 관계를 **{validation["samples"]}개 자세·다리 조합**에서 교차 확인했다. 이는 코드의 수학적 일관성 검사다. 모터·링크 자중과 관성, 수평력, 벨트 초기 장력, 열·전력·변형은 포함하지 않았다. 표의 토크는 하중이 만드는 크기이며 정적 균형 모터 토크의 부호는 반대다.

[계산 입력](parameters.json) · [계산 코드](kinematics.py) · [전체 결과](analysis.json) · [RS06 자료의 시험 조건](../research/robstride_sources.md)
'''
    (HERE / "analysis.md").write_text(report)
    print(json.dumps({"haa_height_mm": result["nominal_haa_height_mm"], "cases": len(cases), "validation": validation,
                      "stand_12kg_motor_torque_nm": torque}, ensure_ascii=False))


if __name__ == "__main__":
    main()