move SOS trajectory into drake
add method to scale time base