%plot(debug_planning.debug_planning_info.xr_desire_m_.data,debug_planning.debug_planning_info.yr_desire_m_.data)
%plot(debug_planning.debug_planning_info.xr_desire_m_.time,debug_planning.debug_planning_info.xr_desire_m_.data)
plot(debug_planning.debug_planning_info.xr_desire_m_.time,debug_planning.debug_planning_info.thetar_desire_rad_.data)
