Get step and cost working with batched trajectories

Currently the test on mds_sys for traj_batch, where testing a batch of input trajectories, is failing. Why? Scan needs the first index of the arrays to be the scanned axis. But additionally, there is a strange error where the output of the scan function is not consistent with the input, as it's getting cast to a batch.

Debating throwing away the inheritance-based approach for distinguishing traj/batch, and just letting the programmer worry about it?