Add Costs Analogous to, e.g., MinimumDistanceLowerBoundConstraint

[cohnt]

MinimumDistanceLowerBoundConstraint adds a constraint that requires the distance between pairs of geometries be at least a certain quantity, but it also adds an objective cost that penalizes being close to that quantity. It would be useful to me if I could just add the associated cost, but not the constraint. That way, I can bias trajectories or IK problems to avoid obstacles, without adding nonconvex constraints.

cc @shrutigarg914

[jwnimmer-tri]

... adds an objective cost that penalizes ...

Does it? As far as I can tell, it is only a constraint MinimumDistanceUpperBoundConstraint final : public solvers::Constraint.

[cohnt]

I may be misunderstanding the documentation of what the penalty function is doing -- I assumed that this described how the cost is affected when the constraint is almost active.

But whether or not this constraint also adds a cost, I would like to be able to add a cost based on the minimum distance to an obstacle.

[jwnimmer-tri]

Sounds good.

I should also broadcast the current work-around suggestion from slack: use the EvaluatorCost wrapper to convert the constraint into a cost.

[xuchenhan-tri]

Assigning @hongkai-dai per platform reviewer checklist.