First cut of code, full IK walking, amble gait, simple FSM with obstacle avoidance