Drake: SystemConstraint should generate solvers::Constraint

[hongkai-dai]

As mentioned in #10290, we want to add a function to generate solvers::Constraint from SystemConstraint. This function takes a Context<symbolic::Expression>, evaluate the SystemConstraint to a symbolic::Expression, and then substitute some variables in the Expression with double values, if we need to fix the values of some variables. Depending on which part of the variables are fixed, this function will generate different types of constraint. For example, for a constraint q * v * v <= 1, if we fix v = 1, it should generate a linear constraint q <= 1; if we fix q = 1, it should generate a quadratic constraint v * v <= 1.

There are two possible solutions to the problem

1. The API would be
   C++ std::unique_ptr<solvers::Constraint> SystemConstraint::GenerateSolverConstraint(const Context<symbolic::Expression>& context, const optional<symbolic::Environment>& env);
   Passing the symbolic::Environment env allows us to generate a constraint on part of the variables in context.
   To add the returned solvers::Constraint back to MathematicalProgram, we will need to use a double dispatch mechanism, to override a function AddToProgram in the base solvers::Constraint class.
   C++ class Constraint { virtual void AddToProgram(MathematicalProgram* prog, const VectorX<symbolic::Variable>& vars) const { prog->AddConstraint(*this, vars); } }; class LinearConstraint : public Constraint { void AddToProgram(MathematicalProgram* prog, const VectorX<symbolic::Variable>& vars) const override { prog->AddConstraint(*this, vars); } };
   I will also need to resolve the circular dependency issue between constraint and mathematical_program.
2. The API is
   C++ Binding<Constraint> GenerateAndAddConstraint(const Context<symbolic::Expression>& context, const optional<symbolic::Environment>& env, MathematicalProgram* prog);
   Within this function, we generate the solvers::Constraint and add it to prog. We do not need to worry about the double dispatching issue. Within this function, we will generate a linear constraint if needed, and add that linear constraint to prog by calling prog->AddLinearConstraint(const Binding<LinearConstraint>& linear_constraint).

@RussTedrake @sherm1 @jwnimmer-tri which API do you like better?

[sherm1]

I guess these only work for systems that can be instantiated with symbolic::Expression?

[hongkai-dai]

Per f2f discussion with @sherm1 . The workflow for generating solvers::Constraint would be

1. If the systems can be instantiated with symbolic::Expression, we will try to generate the symbolic version of the SystemConstraint::Calc result. We will analyze the result (linear, nonlinear, quadratic, etc), and generate either LinearConstraint or generic Constraint.
2. If the system cannot be instantiated with symbolic::Expression, but can be instantiated with AutoDiffXd, we will treat the constraint as generic nonlinear constraint, and generate a solvers::Constraint directly.

[RussTedrake]

per slack.... love the fallback to AutoDiffXd solution.

I think the method does not need to take in a symbolic::Environment. The user should pass in an Expression that has decision variables in some elements and constants (doubles) in others. If they want to obtain that via a symbolic::Environment, they could use Expression::EvaluatePartial(Environment)

I think I like API 2 (avoiding double dispatch). I would probably just call the method AddConstraint though, to be more consistent with the other methods in MathematicalProgram. But would this be a method on the SystemConstraint? I don't know that the SystemConstraint should be the one that does the transmogrification?

Maybe it's better to have an AddSystemConstraintsToProgram(System<T>, Context<Expression>, vector<SystemConstraint&>, MathematicalProgram*) that would transmogrify the System once (if T does not already equal Expression)? And since we plan to have a few relevant lists of constraints... this would be the most common use case, I think?

[RussTedrake]

One problem I just thought of with your AutodiffXd workflow is that the System<Expression>::CreateDefaultContext()method will not be available. Maybe we can find a way to make it available? (e.g. every system has a CreateExpressionContext() method?
or 'CreateSymbolicContext`) But I don't see a good way to do this.
any thoughts @sherm1, @jwnimmer-tri , @EricCousineau-TRI , others?

[RussTedrake]

just because @hongkai-dai and I were talking past each other for a minute on slack, here is an example that helped. The Context<Expression> that we would pass in to these methods will identify the decision variables like this:

context.position = double
context.velocity = velocity_decision_variables

[EricCousineau-TRI]

For the original question:

[...] System<Expression>::CreateDefaultContext() method will not be available.

I'm not sure if I understand why this is the case; why does CreateDefaultContext not work for Expression if you've already transmogrified the system in question?

If this is in the context of MultibodyPlant, then I'd say that it should always just default to AutoDiffXd; if we want more efficient constraint representations, then we should have an optional ProgramConstraintCallback that returns the appropriate Constraint representation (e.g. LinearConstraint for position limits), and not try to force symbolic support for MultibodyPlant just yet.

The Context<Expression> that we would pass in to these methods will identify the decision variables like this [...]

I'm not sure if I understand what this would look like; can you post something with more type illustrations? (I don't know what context.position is...)

---

My two cents on the other bits:

Definitely like the AutoDiffXd fallback. In fact, it might be nice to add that as some sort of flag:

enum class Options : int {
  kFromSymbolic = 0b001,
  kFromAutoDiff = 0b010,
  // Default: kFromSymbolic | kFromAutoDiff
}

From the above recommendation, I'd say that if a SystemConstraint provides a custom Constraint conversion method, then that should always win, and should not require transmogrification.

I like Russ's version of AddSystemConstraintsToProgram in that it can transmogrify once for any of the accepted scalar types, and then generate what it needs from there.
To add to this, it'd be nice if we didn't even need to interface with MathematicalProgram, instead decoupling it with:
GenerateProgramConstraints(System<T>, ...) -> pair<DecisionVars, ConstraintBindingList>
Then we can add a free function like Add(MathematicalProgram* prog, pair<DecisionVars, ConstraintBindingList>), then usage could look like:

AddConstraints(&prog, GenerateProgramConstraints(system, ...));

I also agree that we should ideally avoid Solution 1; double dispatch might be nicer in the future, but I don't think it's necessary to implement here, and would vote that we don't make this feature dependent on it. We should single that out as a separate implementation improvement, in which case we make a client interface to handle the double dispatch and avoid excessive inheritance / coupling with MP.
(MathematicalProgram::AddConstraint(Binding<Constraint>) uses the dirty RTTI downcast hack to add things to the correct container, so it should work out of the box.)

[RussTedrake]

from f2f w/ @jwnimmer-tri and @EricCousineau-TRI

(1) Add CreateSymbolicContext
(2) Add a ContextCoordinate type to denote a scalar within the Context that should be decision variable
(3) Make it easier to support Expression for all Systems (e.g. Multibody), i.e., throw-by-default with no special Calc syntax.

_Edited by jwnimmer to clarify: these were three possible (distinct) approaches, not ordered tasks._

[RussTedrake]

@EricCousineau-TRI -- i believe you understand the intention now (after the face to face?). For instance, if I had a MBP and wanted to optimize only a single Joint, I could

x = prog.NewContinuousVariable()
sym_context = plant.CreateSymbolicContext()  // would generate a Context<Expression> populated with the default constant (double) values.
joint = plant.GetJointByName("my joint")
joint->set_angle(sym_context, x)
AddSystemConstraintsToProgram(plant, prog, sym_context)

(sorry for being quick with it). This is only if we pursue option (1) from our list above.

[jwnimmer-tri]

joint->set_angle(sym_context, x)

This assumes that Joint<double> can set values on a symbolic context. They don't do that yet, and it would be annoying to teach them to do it.

For that example at least, I am starting to suspect that option (1) (from the list two posts prior) is not viable.

[hongkai-dai]

Had a discussion with @jwnimmer-tri and @EricCousineau-TRI . Since we can't do MBP<double>.SetPosition(symbolic_context, x), we think instead of using Context<Expression>, we will use a functor. Here is a proposal

1. The key part to create solvers::Constraint is to get a Context from x in Constraint.Eval(x, &y), where x is of type VectorX<double> or VectorX<AutodiffXd>. Hence we will introduce a functor, that maps x to Context. This functor could be a generic function
   C++ template <typename T> using UpdateContextFromX = std::function<void(const System<T>&, const Eigen::Ref<const VectorX<T>>&, Context<T>*)>;

2. We will create a proxy class SystemConstraintAdapter to create solvers::Constraint. The class looks like this

   class SystemConstraintAdapter {
   public:
    // transmogrifies system to `AutoDiffXd` (and `symbolic::Expression` if it's available).
    SystemConstraintAdapter(const System<double>* const system);
   
    // Generate a generic constraint if it is nonlinear, and a LinearConstraint if it is linear.
   // @param context The subset of context that are not decision variables will be fixed to the value stored in context. 
    // The subset of context that are decision variables would change its value to x when calling solvers::Constraint::Eval(x, &y)
    template <typename UpdateContextFromXGeneric>
    std::shared_ptr<solvers::Constraint> Add(const Context<double>& context, SystemConstraintIndex, UpdateContextFromXGeneric selector, int x_size);
   private:
    const System<double>* const system_double_;
    // This system_autodiff_ is transmogrified from system_double_ in the constructor;
    std::unique_ptr<System<AutoDiffXd>> system_autodiff_;
    // The symbolic version of the system might not be available, like MBP<symbolic::Expression>
    std::unique_ptr<System<symbolic::Expression>> system_symbolic_; 
   };
   
   // This is the nonlinear constraint generated by SystemConstraintAdapter::Add function above
   class SystemConstraintWrapper : public solvers::Constraint {
   public:
    SystemConstraintWrapper(const System<double>* const system_double, 
                        const System<AutoDiffXd>* const system_autodiff,
                        SystemConstraintIndex index, 
                        const Context<double>& context,
                        UpdateContextFromX<double> selector_double, 
                        UpdateContextFromX<AutodiffXd> selector_autodiff
                        int x_size);
   private:
    DoEval(const Eigen::VectorXd& x, VectorXd* y) const {
      selector_double_(*system_double_, x, context_double_.get());
      system_double->constraints()[index].Calc(*context_double_, y);
    }
    DoEval(const AutodiffVecXd& x, AutodiffVecXd* y) const {
      selector_autodiff_(*system_autodiff_, x, context_autodiff_.get());
      system_autodiff_->constraints()[index].Calc(*context_autodiff_, y);
    }
    const System<double>* const system_double_;
    const System<AutoDiffXd>* const system_autodiff_;
    // This context is created by system_double_. Its value is initialized to the same as @p context in the constructor.
    std::unique_ptr<Context<double>> context_double_;
    // This context is created by system_autodiff_. Its value is initialized to the same as @p context in the constructor.
    std::unique_ptr<Context<AutodiffXd>> context_autodiff_;
   };
   

3. Here is one example we use this SystemConstraintAdapter in optimization, to find the initial pose such that the foot height is above the ground, and the initial CoM velocity is moving forward.

   template<typename T>
   void UpdateContextPosition(const MBP<T>& plant, const VectorX<T>& q, Context<T>* context) {
    plant.SetPosition(context, q);
   }
   
   template <typename T>
   void UpdateContextState(const MBP<T>& plant, const VectorX<T>& x, Context<T>* context) {
    plant.SetState(context, x);
   }
   
   int main() {
   MBP<double> plant;
   // Declare a constraint that the foot height should be non-negative.
   auto foot_constraint_index = plant.DeclareInequalityConstraint(...);
   // Declare a constraint that the com velocity along x direction should be non-negative.
   auto com_vel_constraint_index = plant.DeclareInequalityConstraint(...);
   auto context = plant.CreateDefaultContext();
   SystemConstraintAdapter adapter(plant);
   // The foot height is independent of the joint velocity, so we can set them to 0 in the context.
   plant.SetVelocity(context.get(), Eigen::VectorXd::Zero(plant.num_velocities()));
   auto foot_above_ground = adapter.Add(context, foot_constraint_index, UpdateContextPosition, nq);
   // The CoM velocity depends on joint velocity, so these 0 values will be overwritten when we evaluate the CoM velocity constraint.
   auto com_vel_constraint = adapter.Add(context, com_vel_constraint_index, UpdateContextState, nx);
   MathematicalProgram prog;
   auto x = prog.NewContinuousVariable(nx);
   auto q = x.head(nq);
   prog.AddConstraint(foot_above_ground, q);
   prog.AddConstraint(com_vel_constraint, x);
   MathematicalProgramResult result = Solve(prog);
   // Now write the result back to context.
   MapStateToContext(plant, prog.GetSolution(x, result), context.get());
   }
   

The discussion was captured in this google doc https://docs.google.com/document/d/1N7mglr_Y2e2-5SPEuEevRoa2TlpK8Q2idyQZGCrPVTg/edit?usp=sharing

@jwnimmer-tri and @EricCousineau-TRI , does this match with what you think? I changed something from the google doc. For example, now SystemConstraintAdapter doesn't take the functor in its constructor. Instead the functors are passed to each Add function. This allows us to impose constraint on different subset of Context, but save the effort to transmogrify the system every time we call Add function.

@RussTedrake what do you think?

[jwnimmer-tri]

(1) The ContextDoubleSelector should not be function pointers (that is too limiting). It should be something more like this:

template <typename T>
using ContextSelector =
    std::function<void(const Eigen::Ref<const VectorX<T>>&, Context<T>*)>;
using ContextDoubleSelector = ContextSelector<double>;
...

(It also probably needs a better name like ContextDecisionVariableSetter or something?)

(2) The SystemConstraintAdapter::Add signature should allow the user to pass a generic lambda for the selector:

template <typename GenericContextSelector>
std::shared_ptr<solvers::Constraint> Add(
    SystemConstraintIndex system_constraint_index,
    GenericContextSelector generic_context_selector,
    int x_size);

Within the implementation of Add, we would instantiate the selector for each of the 2-3 scalar types that we need (basically casting it into ContextDoubleSelector, ContextAutoDiffXdSelector, etc.)

(3) The SystemConstraintAdapter should accept the System by-const-pointer (not reference), since it needs the system argument to outlive the adapter.

(4) The SystemConstraintAdapter will have to store a the Context passed to its constructor somehow. (It's not yet listed as a member field.) Probably we should clone it, instead of aliasing it.

Alternatively, we could pass a context-refernce to Add (instead of the constructor), so that each constraint can have its own distinct values in the Context.

[jwnimmer-tri]

(5) I also thought that, for Add to magically elect to use the symbolic form to infer linear (etc) constraints, that Add has to return a Binding, not just a Constraint -- so that the proper decision variables are going to be bound? If we don't push decision variables down through the selector into the context, we can't retrieve the symbolic form of the constraint equation.

That might be a good reason to have a distinct AddUsingSymbolic sibling function, and we only try to use the Expression scalar if the user calls AddUsingSymbolic (not during a plain Add). I have a guess that teaching our plain Add function to try to use Expression iff it's available, would be a bit of an implementation challenge.

[sherm1]

I found the name MapXToContext confusing -- at first I thought you were literally going to use the Vector address as a key to find the Context that contains it. If I understand it correctly now it could be called something like UpdateContextFromX or SetXInContext, right?

[hongkai-dai]

(5) I also thought that, for Add to magically elect to use the symbolic form to infer linear (etc) constraints, that Add has to return a Binding, not just a Constraint -- so that the proper decision variables are going to be bound? If we don't push decision variables down through the selector into the context, we can't retrieve the symbolic form of the constraint equation.

I think it is possible to parse the symbolic variable, without returning the Binding. Inside Add function, we can create a vector of symbolic variables var_dummy, pass these var_dummy to UpdateContextFromX, and then evaluate the constraint in the symbolic form. If the constraint is linear, then we can extract the A matrix in the linear constraint lb <= A * var_dummy <= ub. Then we only need to return the linear constraint, without returning var_dummy as the bound variables.

The advantage of returning shared_ptr<Constraint> over Binding<Constraint> is that we can create one constraint, and bind it to different variables. For example, in trajectory optimization, the dynamic constraint is generated for once, but then bound with the state/control at each of the knot point for N times (where N is the number of knot point).

[EricCousineau-TRI]

[signature nit] If possible, it'd be nice to avoid having large signatures to accommodate multiple scalar type functions (possibly optional) via inlined arguments.

I think it should be simple enough to leverage template functor type aliases (like what Jeremy suggested), stuff it into a generic container like this, and allow it to bind to generic functors (like lambdas):
https://github.com/EricCousineau-TRI/repro/blob/81c6f722f80f1221df4db2ec9f98c340feab07d8/cpp/scalar_functor_set.cc#L13-L26

Relevant bits:

ScalarFunctorSet<MyFunc> all(
    [](auto x, auto y){ return generic_func(x, y); });

all.a(1, {3, 4});
// Outputs: "generic_func<int>(1, vec[2])"
all.b(1.5, {5., 6., 7.});
// Outputs: "generic_func<float>(1.5, vec[3])"

[RussTedrake]

can we have the discussion either completely here or (preferrably, I think) completely on the doc? I can't tell what the current proposal is anymore.

[jwnimmer-tri]

I think it is possible to parse the symbolic variable, without returning the Binding.

Ah yes, you are correct.

[hongkai-dai]

I think one caveat in this design is that it is not thread safe. When we call SystemConstraintWrapper::DoEval, we will need to update the value of the context, pointed by SystemConstraintWrapper.context_double_ or SystemConstraintWrapper.context_autodiff_. When we have multiple threads, all using the same SystemConstraintWrapper object, then that context value is modified by all thread.

[jwnimmer-tri]

We could keep the Contexts in a MITLS member field, or maybe its time we figure out a way to have per-Solve mutable data pre-allocated and available to all Constraints in generic way.

For now, though, maybe it's good enough to write the code and disclaim that it's not allows to use one instance across multiple threads.

[sherm1]

(TIL MITLS -- that could be a good cure for SignalLogger's thread safety problems also.)

[hongkai-dai]

Another caveat is that SystemConstraintAdapter::Add function returns one constraint. When we start to support parsing constraint in the symbolic form, this "one constraint" could be restrictive, if the constraint should better be parsed to multiple constraints in different types. For example, if we have a constraint

0 <= x(0) <= 1 A bounding box constraint
1 <= x(0) + x(1) <= 2 A linear constraint.

It is better to return two constraints: a BoundingBoxConstraint 0 <= x(0) <= 1, and a LinearConstraint 1<= x(0) + x(1) <= 2.

Shall we change the return type of SystemConstraintWrapper to std::vector<std::share_ptr<Constraint>>?

[jwnimmer-tri]

Is it true, that the solver back-ends which we care about, really won't automatically look at the linear constraint coefficients A (for columns with only a single non-zero entry) and tease out the bounding box? For the first bbox constraint in your example, if it were its own Constraint, we would still have to make a Binding that would be passing in values for x(1) (since we can't change the Constaint dimensions -- it's determined by the X => Context functor) -- does our bbox constraint support double-infinite bounds for certain decision variables?

Anyway, in general I guess if that's an important optimization then I guess we could allow for it, but perhaps again a SystemConstraintAdapter::Add method that uses Expression should be distinct from the one that doesn't try to use Expression, and only the Expression one has to return a vector<shared_ptr<Constraint>>. So I think it could still be in a second pass, with a new overload.

(We also probably shouldn't call it Add, since nothing is retained by this; we should call it Create or Make or etc.)

[jwnimmer-tri]

Another option would be to have SystemConstraintAdapter::Add return void, and then later on we can grab _all_ of the constraints from the adapter all at once, and/or offer something like void SystemConstraintAdapter::AddAllConstraintsToProgram(..., MP* program);. Then the user doesn't have to shepherd the constraints around manually.

[jwnimmer-tri]

As I study the SystemConstraint and solvers::Constraint (and EvaluatorBase, etc.) classes in more depth, I am actually coming around to the idea that there should only be a single Constraint class tree, not factories that copy between two different class trees. The two trees have much more in common than they have differences.

[jwnimmer-tri]

I am actually coming around to the idea that there should only be a single Constraint class tree, not factories that copy between two different class trees.

I chatted with Hongkai. It seems like keeping the two classes distinct is going to be okay, once my #10374 lands. I will continue prototyping what #10339 might look like, which should help learn about any design challenges.

[hongkai-dai]

Resolved by #10613 #10391