cartesian trajectory cost
demo scenario
benchmark model update