Support for goal TSRs

This implements goalset TSRs. Trajectory wide constraints and start sets not yet supported.
Can you explain what's going on here? Where did these offsets come from?