Drake: symbolic::Expression constructors for AffineSystem and LinearSystem

[RussTedrake]

I was hoping to add an extra constructor to AffineSystem (and LinearSystem) that looks something like this:

  AffineSystem(const symbolic::Variables& state_vars, const symbolic::Variables& input_vars, const Eigen::Ref<const VectorX<symbolic::Expression>>& dynamics, const Eigen::Ref<const VectorX<symbolic::Expression>>& output, double time_period = 0.0)

I think this should be simple, but to do it, I need to extract the linear and constant parts of the expression. I feel like I can get A , B, C, and D from calling symbolic::Jacobian(dynamics,state_vars), but don't actually see an easy way to extract the constant terms (get_constant_value etc assume special cases). I see DecomposeLinearExpression , but it's embedded in symbolic_extraction and special cased for the solvers use case. I also need to DRAKE_DEMAND that the system is affine/linear in (only) the input variables.

What is the recommended API for this? (any chance there is something simple enough to use in a delegating constructor)? Thanks!

[soonho-tri]

@RussTedrake , I'll take the task and add the requested constructor to AffineSystem.

I'll migrate symbolic_extraction target from solvers to common directory and use it for this task.

/cc @EricCousineau-TRI : please let me know if you have comments.

[EricCousineau-TRI]

@soonho-tri No objections here!

[soonho-tri]

@RussTedrake , Could you cherry-pick my commits that I pushed at https://github.com/soonho-tri/drake/tree/pr-add-make-affine-system ?

I've added a static method to the AffineSystem<T> class:

  /// Creates an AffineSystem<T> using symbolic representations.
  static std::unique_ptr<AffineSystem<T>> MakeAffineSystem(
      const symbolic::Variables& state_vars,
      const symbolic::Variables& input_vars,
      const Eigen::Ref<const VectorX<symbolic::Expression>>& dynamics,
      const Eigen::Ref<const VectorX<symbolic::Expression>>& output,
      double time_period = 0.0);

Here is an example usage:

GTEST_TEST(SymbolicAffineSystem, MakeAffineSystem) {
  const symbolic::Variable x0{"x0"};
  const symbolic::Variable x1{"x1"};
  const symbolic::Variable x2{"x2"};
  const symbolic::Variables state_vars{x0, x1, x2};

  const symbolic::Variable u0{"u0"};
  const symbolic::Variable u1{"u1"};
  const symbolic::Variables input_vars{u0, u1};

  Eigen::MatrixXd A(3, 3);
  Eigen::MatrixXd B(3, 2);
  Eigen::VectorXd f0(3);
  VectorX<symbolic::Variable> x(3);
  // clang-format off
  A  << 1, 0, 3,
       -4, 5, 0,
        7, 0, 9;
  B  << 10, -7,
        12, 0,
        0,  15;
  f0 << 3,
        -7,
        2;
  x << x0, x1, x2;
  // clang-format on

  Eigen::MatrixXd C(2, 3);
  Eigen::MatrixXd D(2, 2);
  Eigen::VectorXd y0(2);
  VectorX<symbolic::Variable> u(2);
  // clang-format off
  C  <<  1, 2,  0,
        -4, 0, -7;
  D  << -3,  9,
         0, 13;
  y0 <<  6,
        -7;
  u << u0, u1;
  // clang-format on

  auto dut = AffineSystem<double>::MakeAffineSystem(
      state_vars, input_vars, A * x + B * u + f0, C * x + D * u + y0, 10.0);
  EXPECT_EQ(dut->A(), A);
  EXPECT_EQ(dut->B(), B);
  EXPECT_EQ(dut->C(), C);
  EXPECT_EQ(dut->D(), D);
  EXPECT_EQ(dut->f0(), f0);
  EXPECT_EQ(dut->y0(), y0);
}

I still need to add more checks for the corner cases but I hope this is good enough not to block your workflow.

[RussTedrake]

it's perfect. i will cherry pick if I need it before you push (depends on if I get back to coding tonight). i think that the other test that needs to come in is testing that if you pass in an expression that is not affine, you EXPECT_THROW.

similarly, I'm hoping that we can implement the equivalent MakeLinearSystem, which also errors if there is any non-zero constant term.

@soonho-tri - you are absolutely awesome. thanks for doing this so fast.

[soonho-tri]

the other test that needs to come in is testing that if you pass in an expression that is not affine, you EXPECT_THROW.

I'll add more checks and update my branch. I'll open a PR soon.

I'm hoping that we can implement the equivalent MakeLinearSystem, which also errors if there is any non-zero constant term.

Sure. That's the next stop!

[RussTedrake]

@soonho-tri -- just want to make sure you are happy with the order of the arguments. i had the variables coming first, but worried a bit that maybe they should go after the expressions to be more consistent with your other similar APIs? you can make the call.

[soonho-tri]

@RussTedrake , I'll make it consistent with other APIs.

[soonho-tri]

BTW, I think it's better to take Eigen vectors of variables instead of symbolic::Variables. Because A, B, C, D are determined by the ordering of state_vars and input_vars. What do you think?

[RussTedrake]

I see... because the symbolic::Variables is unordered? I'd like it to be easy to pass in the variable list, but sure I think that's possible with vectors of variables, too. Trust your judgement.

[soonho-tri]

I see... because the symbolic::Variables is unordered?

It has its own ordering (by ID of symbolic::Variable, that is the chronological order of their creations). I think it can confuse users since Variables{x, y, z} doesn't necessarily mean it will be traversed in the given order.

I think that's possible with vectors of variables, too. Trust your judgement.

I'll definitely add the version with Eigen::Vector<Variable>. I'll keep the version with symbolic::Variables for now. We can eliminate it in the review process.

[RussTedrake]

i think it's ok to eliminate it now. You're right that it might lead to unnecessary confusion. good call.