added testExecute
fixed theta motion cost