Aha I do plan to get back into robotics soon (I never got in properly to be fair), and this is a cool algorithm I'll look into! Thanks for that!