Support for goal TSRs

This implements goalset TSRs. Trajectory wide constraints and start sets not yet supported.
It seems like this should also be an error. Is there ever a valid reason to construct a `Bw` matrix with flipped entries?