AddBoundingBoxConstraint can perform incorrectly for matrix input.

[hongkai-dai]

If we call AddBoundingBoxConstraint for matrix input like below

auto X = prog.NewContinuousVariables(2, 3, "X");
Eigen::MatrixXd X_val(2, 3);
X << 1, 2, 3, 4, 5, 6
auto constraint = prog.AddBoundingBoxConstraint(X_val, X_val, X);
EXPECT_EQ(constraint.evaluator()->num_constraints(), 6);
std::cout << constraint.evaluator()->lower_bound().transpose() << "\n";
std::cout << constraint.evaluator()->lower_bound().transpose() << "\n";
std::cout << constraint.variables().transpose();

the code will compile fine. I think it calls this overloaded function https://github.com/RobotLocomotion/drake/blob/7188def1d0a090325dece6ae7dfd179bc17f5166/solvers/mathematical_program.h#L1895-L1898 which supposedly should only work for vector input.

The code runs fine in release mode, but doesn't really impose the desired constraint on X. The code would fail with matrix size assertion error in the debug mode. Here is the output in the release mode

Expected equality of these values:
  cnstr.evaluator()->num_constraints()
    Which is: 2
  6
1 4
1 4
X(0,0) X(1,0)

It's pretty easy to make this mistake, but hard to find it. I can change my call to

prog.AddBoundingBoxConstraint(Eigen::Map<Eigen::VectorXd>(X_val.data(), 6), Eigen::Map<Eigen::VectorXd>(X_val.data(), 6), Eigen::Map<VectorX<symbolic::Variable>>(X.data(), 6));

but I wonder if this is the best solution. Should we provide an overloaded function like

template<typename DerivedLB, typename DerivedUB, typename DerivedX>
AddBoundingBoxConstraint(const Eigen::MatrixBase<DerivedLB>& lb, const Eigen::MatrixBase<DerivedUB>& ub, const Eigen::MatrixBase<DerivedX>& X);

and use some SFINAE trick to restrict the scalar type in DerivedLB, DerivedUB, DerivedX? @jwnimmer-tri what do you think?

[jwnimmer-tri]

The first question is whether we want to allow a Matrix instead of just a Vector in functions that currently accept only vectors (and then unwrap the matrix when adding the cost or constraint). If we do so, then we should do it for all of the functions, not just this one. I think the full list might be:

• AddConstraint(Expression, lb, ub)
• AddLinearConstraint(Expression, lb, ub)
• AddBoundingBoxConstraint (several overloads)
• AddPolynomialConstraint (several overloads)

That idea seems fine by me, though you should poll some other MathematicalProgram users to see if they agree.

---

The second question is how to spell the C++ functions. Your proposal is a template, but I don't see any reason to move away from Eigen::Ref? I think this would be fine:

  Binding<BoundingBoxConstraint> AddBoundingBoxConstraint(
      const Eigen::Ref<const Eigen::MatrixXd>& lb,
      const Eigen::Ref<const Eigen::MatrixXd>& ub,
      const Eigen::Ref<const MatrixXDecisionVariable>& vars);

It would not catch at compile-time any mismatches in matrix shape (that would be a runtime error instead), but I think that's fine.

[hongkai-dai]

For the moment, I would like to avoid making mistakes that I think I imposed constraint a matrix, and the compiler also compiles and the code runs fine, but it doesn't do what I want. One of such mistake occurs in AddBoundingBoxConstraint. As you point out, this mistake can occur for other functions, like AddLinearConstraint(Expression, lb, ub). I am perfectly happy with either options

1. Do not allow the matrix version, then if I pass in a matrix input, the code should fail in either compile time or runtime.
2. Allow the matrix version, and I like your Eigen::Ref code.

Maybe the first step is that in the current AddBoundingBoxConstraint(lb, ub, x) (and other functions that does accept matrix input), I should add a check

DRAKE_DEMAND(lb.cols() == 1);

such that at least the code would fail at the runtime, so the downstream users know to modify their code?

[jwnimmer-tri]

If we like the new form (spelling with MatrixXd instead of VectorXd), then I think it would be just as easy to update the code to take a Matrix, versus trying to band-aid the current form.

DRAKE_DEMAND(lb.cols() == 1);

I would be surprised if this fails at runtime. I think const Ref<const VectorXd>& would always say cols == 1, since it would have no member field to store a dynamic number of cols in the first place?

I think for this API the answer is easy -- change to Matrix. In the broader picture, in places where we do want a vector (only), I would love it there was a way to promote that Eigen assertion in our Release builds (it's currently Debug-only), but without turning on some of the other Eigen assertions related to bounds-checking of access.

[hongkai-dai]

Agreed, I think we got the same conclusion on the slack channel https://drakedevelopers.slack.com/archives/C3L92BM2Q/p1642873187009800

I will file a PR to change Eigen::VectorXd to Eigen::MatrixXd.

[hongkai-dai]

I think const Ref& would always say cols == 1, since it would have no member field to store a dynamic number of cols in the first place

I think what happens is that if we pass an actual matrix to an variable of type const Ref<const VectorXd>, it just takes the left-most column of that matrix, as shown in the example in https://github.com/RobotLocomotion/drake/issues/16421#issue-1111288000, where it only imposes the constraint on the left-most column of X.