Enable IrisNp2 to Handle Additional User-Specified Constraints

[cohnt]

This feature is already available in IrisZo and IrisInConfigurationSpace. Note that much of the PR is two move operations: placing some code from iris_zo.cc into planning/iris_common.h and planning/iris_common.cc, and placing some code from geometry/optimization/iris.cc into geometry/optimization/iris_internal.h and geometry/optimization/iris_internal.cc (and promoting them from internal linkage to an internal namespace).

+@RussTedrake for feature review, if you don't mind?

---

This change is 

[RussTedrake]

a bunch of small comments throughout.

Reviewed 9 of 9 files at r1, all commit messages.
Reviewable status: 6 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

planning/iris/iris_common.h line 269 at r1 (raw file):

    Eigen::VectorXd* b, int* num_constraints);

// Given a pointer to a MathematicalProgram and a single particle, check whether

btw -- i would typically favor "sample" over particle? but i see particle is used below, too.

---

planning/iris/iris_common.h line 284 at r1 (raw file):

// end_index). Default behavior is to check all particles -- when end_index is
// std::nullopt, it is set to ssize(particles).
std::vector<uint8_t> CheckProgConstraints(

Consider "CheckProgConstraintsParallel"? That would mirror the API in e.g. CollisionChecker a bit better.
https://drake.mit.edu/doxygen_cxx/classdrake_1_1planning_1_1_collision_checker.html#a6bb8c478f30ea78ac72ce0e557c6067d
(and would avoid having an overload with different return types)

---

planning/iris/iris_common.h line 286 at r1 (raw file):

std::vector<uint8_t> CheckProgConstraints(
    const solvers::MathematicalProgram* prog_ptr,
    const std::vector<Eigen::VectorXd>& particles, const int num_threads_to_use,

shouldn't we be preferring Parallelism instead of num_threads_to_use? https://drake.mit.edu/doxygen_cxx/classdrake_1_1_parallelism.html

(again, using CollisionChecker as a pretty good template to follow)

---

geometry/optimization/iris_internal.cc line 224 at r1 (raw file):

}

void CounterExampleConstraint::DoEval(const Eigen::Ref<const VectorXd>& x,

btw -- it's hard for me to see if anything changed in the implementation here, apart from copying over the code?

---

geometry/optimization/iris_internal.h line 187 at r1 (raw file):

  bool falsify_lower_bound_{true};

  // To find a counter-example for a constraints,

nit: I guess counterexample is actually one word (no hyphen). (and the hyphenation is incorrect here and throughout if it was too words).

So technically, the class should be CounterexampleConstraint, but I'm ok leaving it.

---

planning/iris/iris_np2.cc line 603 at r1 (raw file):

          // Find the constraint in prog_with_additional_constraints that is
          // violated.

can you not use CheckSatisfied here? https://drake.mit.edu/doxygen_cxx/classdrake_1_1solvers_1_1_mathematical_program.html#a3c2467d1ef4671955b66dd8ddfac39e4 (this method also exists in the Constraint interface).
Or even GetInfeasibleConstraints?
https://drake.mit.edu/doxygen_cxx/classdrake_1_1solvers_1_1_mathematical_program_result.html#a84309a71a58b225e4a6bba6f5a53e6ec

[cohnt]

Reviewable status: 6 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

geometry/optimization/iris_internal.h line 187 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

nit: I guess counterexample is actually one word (no hyphen). (and the hyphenation is incorrect here and throughout if it was too words).

So technically, the class should be CounterexampleConstraint, but I'm ok leaving it.

Might as well fix it now.

---

geometry/optimization/iris_internal.cc line 224 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

btw -- it's hard for me to see if anything changed in the implementation here, apart from copying over the code?

This was just a move, which you can see in commit 9face43. I needed to promote the code from an anonymous namespace to an internal one, so I could also use it in IrisNp2. (This necessitated splitting the definition into the .h and .cc files, but no other changes.)

---

planning/iris/iris_common.h line 269 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

btw -- i would typically favor "sample" over particle? but i see particle is used below, too.

We've somewhat mixed the use of particle and sample throughout. I've tended to think of the original samples drawn from the polytope as "samples", and then I think of them as "particles" as they move around. But we're definitely using them somewhat-interchangeably.

---

planning/iris/iris_common.h line 284 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

Consider "CheckProgConstraintsParallel"? That would mirror the API in e.g. CollisionChecker a bit better.
https://drake.mit.edu/doxygen_cxx/classdrake_1_1planning_1_1_collision_checker.html#a6bb8c478f30ea78ac72ce0e557c6067d
(and would avoid having an overload with different return types)

Done.

---

planning/iris/iris_common.h line 286 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

shouldn't we be preferring Parallelism instead of num_threads_to_use? https://drake.mit.edu/doxygen_cxx/classdrake_1_1_parallelism.html

(again, using CollisionChecker as a pretty good template to follow)

Part of the motivation for passing around num_threads instead of a Parallelism object is that different parts of the codebase support parallelism to different degrees. For example, we may have a collision checker that supports checking particles in parallel, but maybe the mathematical program has non-threadsafe constraints.

Right now, we have the idiom

additional_constraints_threadsafe ? options.sampled_iris_options.parallelism.num_threads() : 1

I guess we could replace it with

additional_constraints_threadsafe ? options.sampled_iris_options.parallelism : Parallelism::None()

Would that be preferable?

---

planning/iris/iris_np2.cc line 603 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

can you not use CheckSatisfied here? https://drake.mit.edu/doxygen_cxx/classdrake_1_1solvers_1_1_mathematical_program.html#a3c2467d1ef4671955b66dd8ddfac39e4 (this method also exists in the Constraint interface).
Or even GetInfeasibleConstraints?
https://drake.mit.edu/doxygen_cxx/classdrake_1_1solvers_1_1_mathematical_program_result.html#a84309a71a58b225e4a6bba6f5a53e6ec

I think we need to traverse the constraints manually, since we need to know which index of the binding is the one that's infeasible.