Fixes infeasibility when robot starts with toes pointing together

Fixis DRC issue #395