Added auxiliary command framework and joint homing -- needs testing!