# walker.py - a 4-legged robot teaches itself to walk (MuJoCo + CMA-ES)
# pip install mujoco cma numpy      then: python walker.py
import numpy as np, mujoco, cma

LEG = """<body pos="{x} {y} 0"><joint name="hip{i}" type="hinge" axis="0 1 0" range="-60 60"/>
  <geom type="capsule" fromto="0 0 0 0 0 -0.12" size="0.02"/>
  <body pos="0 0 -0.12"><joint name="knee{i}" type="hinge" axis="0 1 0" range="-70 70"/>
    <geom type="capsule" fromto="0 0 0 0 0 -0.12" size="0.018"/></body></body>"""
CORNERS = [(0.15, 0.1), (0.15, -0.1), (-0.15, 0.1), (-0.15, -0.1)]
XML = f"""<mujoco><option timestep="0.004"/>
<default><joint damping="0.5" armature="0.01"/><geom friction="1 0.005 0.0001" density="1000"/>
  <position kp="8" ctrlrange="-1.2 1.2" ctrllimited="true"/></default>
<worldbody><geom type="plane" size="20 20 0.1"/>
 <body name="torso" pos="0 0 0.3"><freejoint/><geom type="box" size="0.18 0.12 0.03" density="300"/>
  {''.join(LEG.format(x=x, y=y, i=i) for i, (x, y) in enumerate(CORNERS))}
 </body></worldbody>
<actuator>{''.join(f'<position joint="hip{i}"/><position joint="knee{i}"/>' for i in range(4))}</actuator>
</mujoco>"""

model = mujoco.MjModel.from_xml_string(XML)
N = model.nu                      # 8 motors
SECONDS = 5

def run(genes, seconds=SECONDS):
    """Genes -> distance walked. Every motor follows amp*sin(2*pi*f*t + phase) + offset."""
    amp, phase, offset = genes[:N], genes[N:2*N], genes[2*N:3*N]
    freq = np.clip(1.5 + genes[-1], 0.5, 3.0)   # one shared frequency, 0.5-3 Hz
    d = mujoco.MjData(model)
    while d.time < seconds:
        d.ctrl[:] = amp * np.sin(2 * np.pi * freq * d.time + phase) + offset
        mujoco.mj_step(model, d)
    upright = d.xmat[1][8] > 0    # torso z-axis still pointing up?
    return d.qpos[0] if upright else d.qpos[0] - 1.0

if __name__ == "__main__":
    es = cma.CMAEvolutionStrategy(np.zeros(3 * N + 1), 0.5, {"popsize": 24, "seed": 1, "verbose": -9})
    for gen in range(1, 101):
        population = es.ask()
        distances = [run(g) for g in population]
        es.tell(population, [-x for x in distances])   # CMA-ES minimises, so negate
        if gen % 10 == 0:
            print(f"gen {gen:3d}   best {max(distances):5.2f} m   best ever {-es.result.fbest:5.2f} m")

    np.save("champion.npy", es.result.xbest)
    print("saved champion.npy  ->  python watch.py")
