Verify that collision constraint/cost hare handled correctly when number of contacts is less than size.

[Levi-Armstrong]

Ifopt required the constraint/cost size to not change which is only an issue currently for the collision constraints and cost. There maybe an issue when the number of contacts is less than the available size in how trust region data is calculated which needs to be checked.

I asked a question on OSQP and it seems having uninitialized or all zero rows in the csc matrix is not an issues and do not need to be pruned, but need to also verify that this calculated correctly for the condition above.