Support for goal TSRs

This implements goalset TSRs. Trajectory wide constraints and start sets not yet supported.
Is there ever a legitimate reason to call `TSRRobot::construct` twice? If not, this should throw an exception.