Fix planning
rail position is closer to the bin or target location, wrist_3 goal 2pi problem fix, checking error in all steps