robot.py: add WIP setFootXY based on http://robcook.eu/hexy/inverse-kinematics-part-1/

Signed-off-by: Koen Kooi <koen@dominion.thruhere.net>