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.

[hongkai-dai]

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.

@cohnt What is the mathematical formulation of your cost? Let's say it is a function of the minimal distance d. Could you write down explicit form of the cost as a function of d? Is it just cost=-d?

[hongkai-dai]

@cohnt could you try this code snippet?

min_distance_constraint = multibody.MinimumDistanceLowerBoundConstraint(plant, plant_context, bound=0)
# min_distance_constraint.Eval() computes ϕ(d/d_influence − 1)/ϕ(−1), where d is the minimal distance, and ϕ() is the penalty function.
# To maximize the distance, we want to minimize ϕ(d/d_influence − 1)/ϕ(−1), because ϕ() is monotonically decreasing.
prog.AddCost(lambda q: min_distance_constraint.Eval(q), q)

You might need to play with the influence_distance_offset in the constructor of MinimumDistanceLowerBoundConstraint to achieve good performance in the nonlinear solver, at TRI we use a very small value (1E-6) instead of the default 0.01.