Path feasibility and modification based on robot workspace geometry