Fixed bugs in locomotion_bends

Also, added interpolate_with_threshold_2D