Explicit all goals fulfilled constraint

For a class of tasks that are maximizing reachability / workspace dexterity the best outcome would be to reach every single desired goal. But reaching only a subset is also ok and the cost function explicitly tries to maximize this fraction. Example: Case 3 from S. Liu et al., Optimizing performance in automation through modular robots, ICRA 2020.

This MR enables this by adding the constraint "All Goals Fulfilled" as described by CoBRA and embedding it in the valid calculation of a goal.