Support for goal TSRs

This implements goalset TSRs. Trajectory wide constraints and start sets not yet supported.
This will cause the planner to ignore the goal configuration if one or more goal TSR chains are specified. I am not sure if this is desired behavior. Any idea what CBiRRT does? Does it also consider the goal configuration as a goal?