Support for goal TSRs

This implements goalset TSRs. Trajectory wide constraints and start sets not yet supported.
You should pass `IKFO_IgnoreSelfCollisions` to the IK solver. Otherwise, the IK solver will only return IK solutions that are not in self-collision. The geometry used to visualize the TSR chain could suppress valid solutions.