Feature/goalset tsr

This implements goalset TSRs. Trajectory wide constraints and start sets not yet supported.