[solvers] Support `Expression`s in `AddBoundingBoxConstraint`

[RussTedrake]

I had an excellent question from a student, reposted below.

Attempt 3 we already track away with #8315 and #16025. But, especially since that code is so inelegant, we definitely could support AddBoundingBoxConstraint(lb, ub, Expression) (or Vector<Expression>) for the special case when each Expression depends linearly on only one Variable. That’s a bit niche, but definitely doable.

original question:

I was wondering if you could clear up some confusion I have about implementing different representations of constraints in a MathematicalProgram. For 3.10 I tried implementing the lower and upper bounds for the end effector position in a few ways:

Attempt 1 used a bounding box acting on an array of symbolic representations (Stack Exchange leads me to believe that this is not a valid way of using AddBoundingBoxConstraint at this time, but I wanted to confirm). Attempt 2 implemented a series of bounding box constraints, one for each position coordinate. Attempt 3 used the <= and >= operators within AddConstraint. Attempt 4 used ge() and le() instead. My naive understanding is that all these implementations should be equivalent, but only attempt 4 successfully ran without triggering an error. I have attached a code snippet (question kept private as a result) and the error messages below.

Thanks!

      # We can convert from joint-space velocities to task-space velocities using the Jacobian
      v_ts = J_G.dot(v) 

      # Using the task-space velocities, impose a constraint using first-order Euler finite difference
      p_next = p_now+h*v_ts[3:] # For convenience

      # Constraint 1:
      #prog.AddBoundingBoxConstraint(lower_bound,upper_bound,p_next)
      # Constraint 2:
      #for i in range(2):
      #  prog.AddBoundingBoxConstraint(lower_bound[i],upper_bound[i],p_next[i])
      # Constraint 3:
      prog.AddConstraint(lower_bound<=p_now+h*v_ts[3:])
      prog.AddConstraint(upper_bound>=p_now+h*v_ts[3:])
      # Constraint 4:
      prog.AddConstraint(le(lower_bound,p_now+h*v_ts[3:]))
      prog.AddConstraint(ge(upper_bound,p_now+h*v_ts[3:]))

Attempt 1:

TypeError: AddBoundingBoxConstraint(): incompatible function arguments. The following argument types are supported: 1. (self: pydrake.solvers.MathematicalProgram, arg0: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], arg1: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], arg2: numpy.ndarray[object[m, n], flags.f_contiguous]) -> pydrake.solvers.Binding[BoundingBoxConstraint] 2. (self: pydrake.solvers.MathematicalProgram, arg0: float, arg1: float, arg2: pydrake.symbolic.Variable) -> pydrake.solvers.Binding[BoundingBoxConstraint] 3. (self: pydrake.solvers.MathematicalProgram, arg0: float, arg1: float, arg2: numpy.ndarray[object[m, n], flags.f_contiguous]) -> pydrake.solvers.Binding[BoundingBoxConstraint]

Attempt 2:

TypeError: AddBoundingBoxConstraint(): incompatible function arguments. The following argument types are supported: 1. (self: pydrake.solvers.MathematicalProgram, arg0: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], arg1: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], arg2: numpy.ndarray[object[m, n], flags.f_contiguous]) -> pydrake.solvers.Binding[BoundingBoxConstraint] 2. (self: pydrake.solvers.MathematicalProgram, arg0: float, arg1: float, arg2: pydrake.symbolic.Variable) -> pydrake.solvers.Binding[BoundingBoxConstraint] 3. (self: pydrake.solvers.MathematicalProgram, arg0: float, arg1: float, arg2: numpy.ndarray[object[m, n], flags.f_contiguous]) -> pydrake.solvers.Binding[BoundingBoxConstraint]

Attempt 3:

RuntimeError: You should not call `__bool__` / `__nonzero__` on `Formula`. If you are trying to make a map with `Variable`, `Expression`, or `Polynomial` as keys (and then access the map in Python), please use pydrake.common.containers.EqualToDict`.

[hongkai-dai]

Currently we can call AddLinearConstraint(expressions, lower, upper) which returns a Binding<LinearConstraint> object. Is it important here to force the constraint to be a BoundingBoxConstraint?

[buoyancy99]

I hope Vector<Formula> would be supported for AddConstraint too (like in constraint 3 prog.AddConstraint(lower_bound<=p_now+h*v_ts[3:])), this is a very handy thing that people always do in cvxpy.

[RussTedrake]

@hongkai-dai -- i agree we can do it with linear constraints already. yes, the request is to do it as a bounding box. Partly because this can be better for the solver. But also because users can (reasonably) be surprised that calling AddBoundingBoxConstraint in this case does not work.

[buoyancy99]

@RussTedrake I might have found a fix for constraint 3, see my solution here

[cohnt]

Commenting here to check if there's any update on whether or not vectorized constraint definitions are possible in the future (as @buoyancy99 mentioned above).

[jwnimmer-tri]

The discussion above is a bit twisty. To be very clear about your request, is that that we bind this existing C++ function into pydrake?

  /**
   * Adds constraints represented by symbolic expressions to the program. It
   * throws if <tt>lb <= v <= ub</tt> includes trivial/unsatisfiable
   * constraints.
   *
   * @overload Binding<Constraint> AddConstraint(const symbolic::Expression& e,
   *    double lb, double ub)
   *
   * @exclude_from_pydrake_mkdoc{Not bound in pydrake.}
   */
  Binding<Constraint> AddConstraint(
      const Eigen::Ref<const MatrixX<symbolic::Expression>>& v,
      const Eigen::Ref<const Eigen::MatrixXd>& lb,
      const Eigen::Ref<const Eigen::MatrixXd>& ub);

For reference, we already have this function bound (which nicely describes the semantics):

  /**
   * Adds one row of constraint lb <= e <= ub where @p e is a symbolic
   * expression.
   * @throws std::exception if
   * 1. <tt>lb <= e <= ub</tt> is a trivial constraint such as 1 <= 2 <= 3.
   * 2. <tt>lb <= e <= ub</tt> is unsatisfiable such as 1 <= -5 <= 3
   *
   * @param e A symbolic expression of the decision variables.
   * @param lb A scalar, the lower bound.
   * @param ub A scalar, the upper bound.
   *
   * The resulting constraint may be a BoundingBoxConstraint, LinearConstraint,
   * LinearEqualityConstraint, or ExpressionConstraint, depending on the
   * arguments.  Constraints of the form x == 1 (which could be created as a
   * BoundingBoxConstraint or LinearEqualityConstraint) will be
   * constructed as a LinearEqualityConstraint.
   */
  Binding<Constraint> AddConstraint(const symbolic::Expression& e, double lb,
                                    double ub);

[hongkai-dai]

Reading the original post https://github.com/RobotLocomotion/drake/issues/17976#issue-1384853052 I don't believe lower_bound<=p_now+h*(J_g * v)[3:] is a bounding box constraint. This is a linear constraint, but not a bounding box constraint on v. I believe the solution posted by Jeremy should solve this issue, to add that extra python binding.

[cohnt]

My apologies for the confusion. Perhaps it would have been better for me to comment on #16025 instead of this issue. Binding the first example Jeremy listed would be helpful, since there are cases where it's helpful to assign different bounds to different components. (I've made use of this feature in cvxpy before.)

In general, it's frustrating that we can't write expressions such as prog.AddConstraint(x <= 3), where x is a vector decision variable. But my impression is that numpy is to blame for that one.

[jwnimmer-tri]

Per https://github.com/RobotLocomotion/drake/issues/16025#issuecomment-960302980 we already have AddConstraint(Matrix<Formula>) bound, so the le(x, 3) should work OK.

My impression is that numpy is to blame for that one.

At the time we wrote the code a few years ago, that was true. Since that time, there might be new numpy hooks we could use to make it better. I've been meaning to investigate that again but haven't found the time.