Support for goal TSRs

This implements goalset TSRs. Trajectory wide constraints and start sets not yet supported.
It makes me a bit nervous to put this hidden state inside the `TSRChain` class. I can easily imagine forgetting to call `setEnv` after changing environments and getting an unpredictable result. (I know that it can't happen in this particular case because the planner is attached to a specific environment.)

I would prefer to: (1) pass the environment pointer directly to sample or (2) move the functionality that depends on the environment to a small helper class. Thoughts?