[WIP] 3 link pendulum