DecomposeLumpedParameters from VectorX<Expression> (RobotLocomotion#1…

…4863)

* DecomposeLumpedParameters from VectorX<Expression>

First implementation of lumped parameter decomposition for full symbolic::Expressions.

This updates (and will soon replace) our related implementation in solvers/system_identification.h .  That implementation used the old (to be expunged) Polynomial class, and required a manual step to substitute trig functions into polynomial functions.  This substitution is difficult in general, and is not compatible with MultibodyPlant.  Furthermore, there is no fundamental reason to restrict the lumped-parameter decomposition to polynomials.

Follow-up work is tracked in RobotLocomotion#14879.

bindings/pydrake/symbolic_py.cc

@@ -832,7 +832,11 @@ PYBIND11_MODULE(symbolic, m) {
             return std::make_pair(coeffs, constant_term);
           },
           py::arg("e"), py::arg("map_var_to_index"),
-          doc.DecomposeAffineExpression.doc);
+          doc.DecomposeAffineExpression.doc)
+      .def("DecomposeLumpedParameters", &DecomposeLumpedParameters,
+          py::arg("f"), py::arg("parameters"),
+          doc.DecomposeLumpedParameters.doc);
+
   // NOLINTNEXTLINE(readability/fn_size)
 }
 }  // namespace pydrake

bindings/pydrake/test/symbolic_test.py

@@ -1308,3 +1308,18 @@ def test(self):
         self.assertEqual(b[x_idx], 3)
         self.assertEqual(b[y_idx], 2)
         self.assertEqual(b.shape, (2,))
+
+
+class TestDecomposeLumpedParameters(unittest.TestCase):
+    def test(self):
+        x = sym.Variable("x")
+        a = sym.Variable("a")
+        b = sym.Variable("b")
+
+        f = [a + x, a*a*x*x]
+        [W, alpha, w0] = sym.DecomposeLumpedParameters(f, [a, b])
+        numpy_compare.assert_equal(W,
+                                   [[sym.Expression(1), sym.Expression(0)],
+                                    [sym.Expression(0), x*x]])
+        numpy_compare.assert_equal(alpha, [sym.Expression(a), a*a])
+        numpy_compare.assert_equal(w0, [sym.Expression(x), sym.Expression(0)])

common/symbolic_decompose.cc

@@ -1,7 +1,11 @@
 #include "drake/common/symbolic_decompose.h"
 
+#include <map>
 #include <stdexcept>
 #include <string>
+#include <tuple>
+
+#include <fmt/ostream.h>
 
 namespace drake {
 namespace symbolic {
@@ -199,5 +203,304 @@ void DecomposeAffineExpressions(const Eigen::Ref<const VectorX<Expression>>& v,
     DecomposeAffineExpression(e_i, map_var_to_index, A->row(i), b->data() + i);
   }
 }
+
+namespace {
+
+typedef std::tuple<VectorX<Expression>, VectorX<Expression>, Expression>
+    LumpedFactorization;
+
+// Visitor class to implement DecomposeLumpedParameters.
+class DecomposeLumpedParametersVisitor {
+ public:
+  LumpedFactorization Decompose(const Expression& e,
+                                const Variables& parameters) const {
+    // Note that it calls `Expression::Expand()` here.
+    return Visit(e.Expand(), parameters);
+  }
+
+ private:
+  LumpedFactorization Visit(const Expression& e,
+                            const Variables& parameters) const {
+    return VisitExpression<LumpedFactorization>(this, e, parameters);
+  }
+
+  LumpedFactorization VisitVariable(const Expression& e,
+                                    const Variables& parameters) const {
+    const Variable& var{get_variable(e)};
+    if (parameters.include(var)) {
+      // W = [1], alpha = [e], w0 = [0]
+      return LumpedFactorization{Vector1<Expression>{1}, Vector1<Expression>{e},
+                                 0};
+    } else {
+      // W = [], alpha = [], w0 = [e]
+      return LumpedFactorization{Vector0<Expression>{}, Vector0<Expression>{},
+                                 e};
+    }
+  }
+
+  LumpedFactorization VisitConstant(const Expression& e,
+                                    const Variables&) const {
+    return LumpedFactorization{Vector0<Expression>{}, Vector0<Expression>{}, e};
+  }
+
+  LumpedFactorization VisitAddition(const Expression& e,
+                                    const Variables& parameters) const {
+    // Temporary storage to hold the elements of w(n) (as a key) and the
+    // elements of α(parameters) (as a value) for e.  We use a map to avoid
+    // duplicates.
+    std::map<Expression, Expression> w_map;
+
+    // e = c₀ + ∑ᵢ (cᵢ * eᵢ)
+    //   => [c₁w₁, c₂w₂, ...]*[α₁, α₂, ...] + (c₀ + ∑ᵢ cᵢw0ᵢ)
+    // except for matching terms.
+    Expression w0 = get_constant_in_addition(e);
+    for (const std::pair<const Expression, double>& p :
+         get_expr_to_coeff_map_in_addition(e)) {
+      const Expression& e_i{p.first};
+      const double c_i{p.second};
+      const auto [w_i, alpha_i, w0_i] = Visit(e_i, parameters);
+      w0 += c_i * w0_i;
+      // TODO(russt): generalize this to matching up to a constant factor.
+      for (int j = 0; j < w_i.size(); j++) {
+        auto it = w_map.emplace(c_i * w_i[j], 0).first;
+        it->second += alpha_i[j];
+      }
+    }
+    VectorX<Expression> w(w_map.size());
+    VectorX<Expression> alpha(w_map.size());
+    int i = 0;
+    for (const auto& [key, value] : w_map) {
+      w[i] = key;
+      alpha[i++] = value;
+    }
+    return LumpedFactorization{w, alpha, w0};
+  }
+
+  // Handle basic multiplication: e = a * b
+  LumpedFactorization SimpleMultiplication(const LumpedFactorization& a,
+                                           const LumpedFactorization& b) const {
+    const auto& [w_a, alpha_a, w0_a] = a;
+    const auto& [w_b, alpha_b, w0_b] = b;
+
+    // Avoid adding terms with zero coefficients, otherwise they start to
+    // accumulate quickly.
+    const bool nonzero_w0a = !is_zero(w0_a);
+    const bool nonzero_w0b = !is_zero(w0_b);
+
+    // a*b = (wa*αa + w₀a) (wb*αb + w₀b)
+    //     = w₀a*w₀b + ∑ᵢⱼ(waᵢ*wbⱼ * αaᵢ*αbⱼ) + ∑ⱼw₀a*wbⱼ*αbⱼ + ∑ᵢw₀b*waᵢ*αaᵢ
+    const auto N = w_a.size() * w_b.size() + (nonzero_w0a ? w_b.size() : 0) +
+                   (nonzero_w0b ? w_a.size() : 0);
+    VectorX<Expression> w(N);
+    VectorX<Expression> alpha(N);
+    const Expression w0 = w0_a * w0_b;
+    if (w_a.size() * w_b.size() != 0) {
+      w.head(w_a.size() * w_b.size()) << w_a * w_b.transpose();
+      alpha.head(w_a.size() * w_b.size()) << alpha_a * alpha_b.transpose();
+    }
+    if (nonzero_w0a) {
+      const auto offset = w_a.size() * w_b.size();
+      w.segment(offset, w_b.size()) = w0_a * w_b;
+      alpha.segment(offset, w_b.size()) = alpha_b;
+    }
+    if (nonzero_w0b) {
+      w.tail(w_a.size()) = w0_b * w_a;
+      alpha.tail(w_a.size()) = alpha_a;
+    }
+    // TODO(russt): Avoid duplicates.
+    return LumpedFactorization(w, alpha, w0);
+  }
+
+  LumpedFactorization VisitMultiplication(const Expression& e,
+                                          const Variables& parameters) const {
+    const double c = get_constant_in_multiplication(e);
+    LumpedFactorization f({}, {}, {c});
+
+    // e = c * ∏ᵢ pow(baseᵢ, exponentᵢ).
+    for (const std::pair<const Expression, Expression>& p :
+         get_base_to_exponent_map_in_multiplication(e)) {
+      const Expression& base_i{p.first};
+      const Expression& exponent_i{p.second};
+      const auto [w, alpha, w0] = SimpleMultiplication(
+          f, is_one(exponent_i)
+                 ? Visit(base_i, parameters)
+                 : VisitPow(pow(base_i, exponent_i), parameters));
+      // Watch out for aliasing (do I need .eval() in this case?).
+      f = LumpedFactorization{w.eval(), alpha.eval(), w0};
+    }
+
+    return f;
+  }
+
+  LumpedFactorization VisitPow(const Expression& e,
+                               const Variables& parameters) const {
+    const Expression& exponent{get_second_argument(e)};
+    const Variables vars = e.GetVariables();
+    if (vars.IsSubsetOf(parameters)) {  // All parameters.
+      return LumpedFactorization{
+          Vector1<Expression>{1}, Vector1<Expression>{e}, {0}};
+    } else if (intersect(vars, parameters).empty()) {  // All non-parameters.
+      return LumpedFactorization{{}, {}, e};
+    } else if (is_constant(exponent)) {
+      // Note(russt): I don't *think* that this code is reachable, since the
+      // Expand() called at the beginning of the decomposition will break apart
+      // cases like this.  But we can implement this if we ever determine it is
+      // needed, e.g., repeated calls to SimpleMultiplication.
+      throw runtime_error(
+          fmt::format("{} CAN be factored into lumped parameters, but this "
+                      "case has not been implemented yet.",
+                      e));
+    } else {
+      throw runtime_error(
+          fmt::format("{} cannot be factored into lumped parameters, since it "
+                      "depends on both parameters and non-parameter variables "
+                      "in a non-multiplicative way.",
+                      e));
+    }
+  }
+
+  LumpedFactorization VisitNonPolynomialTerm(
+      const Expression& e, const Variables& parameters) const {
+    // Must be either all parameters or all non-parameters.
+    const Variables& vars = e.GetVariables();
+    if (vars.IsSubsetOf(parameters)) {
+      return LumpedFactorization{
+          Vector1<Expression>{1}, Vector1<Expression>{e}, {0}};
+    } else if (intersect(vars, parameters).empty()) {
+      return LumpedFactorization{{}, {}, e};
+    } else {
+      throw runtime_error(
+          fmt::format("{} cannot be factored into lumped parameters, since it "
+                      "depends on both parameters and non-parameter variables.",
+                      e));
+    }
+  }
+
+  LumpedFactorization VisitDivision(const Expression& e,
+                                    const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+
+  LumpedFactorization VisitAbs(const Expression& e,
+                               const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitLog(const Expression& e,
+                               const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitExp(const Expression& e,
+                               const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitSqrt(const Expression& e,
+                                const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitSin(const Expression& e,
+                               const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitCos(const Expression& e,
+                               const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitTan(const Expression& e,
+                               const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitAsin(const Expression& e,
+                                const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitAcos(const Expression& e,
+                                const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitAtan(const Expression& e,
+                                const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitAtan2(const Expression& e,
+                                 const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitSinh(const Expression& e,
+                                const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitCosh(const Expression& e,
+                                const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitTanh(const Expression& e,
+                                const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitMin(const Expression& e,
+                               const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitMax(const Expression& e,
+                               const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitCeil(const Expression& e,
+                                const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitFloor(const Expression& e,
+                                 const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitIfThenElse(const Expression& e,
+                                      const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+  LumpedFactorization VisitUninterpretedFunction(
+      const Expression& e, const Variables& parameters) const {
+    return VisitNonPolynomialTerm(e, parameters);
+  }
+
+  // Makes VisitExpression a friend of this class so that it can use private
+  // methods.
+  friend LumpedFactorization drake::symbolic::VisitExpression<
+      LumpedFactorization>(const DecomposeLumpedParametersVisitor*,
+                           const Expression&, const Variables&);
+};
+
+}  // namespace
+
+std::tuple<MatrixX<Expression>, VectorX<Expression>, VectorX<Expression>>
+DecomposeLumpedParameters(
+    const Eigen::Ref<const VectorX<Expression>>& f,
+    const Eigen::Ref<const VectorX<Variable>>& parameters) {
+  const DecomposeLumpedParametersVisitor visitor{};
+
+  // Compute Wα (avoiding duplicate α) by filling a map from alpha to the
+  // corresponding column of W.
+  std::map<Expression, VectorX<Expression>> alpha_map;
+
+  VectorX<Expression> w0(f.size());
+  for (int i = 0; i < f.size(); i++) {
+    auto const[w, alpha, this_w0] =
+        visitor.Decompose(f[i], Variables(parameters));
+    w0[i] = this_w0;
+    for (int j = 0; j < alpha.size(); j++) {
+      auto it = alpha_map.emplace(alpha[j], VectorX<Expression>::Zero(f.size()))
+                    .first;
+      (it->second)[i] += w[j];  // add to element i of column j.
+    }
+  }
+  MatrixX<Expression> W = MatrixX<Expression>::Zero(f.size(), alpha_map.size());
+  VectorX<Expression> alpha(alpha_map.size());
+  int j = 0;
+  for (const auto& [key, value] : alpha_map) {
+    alpha[j] = key;
+    W.col(j++) = value;
+  }
+  return {W, alpha, w0};
+}
+
 }  // namespace symbolic
 }  // namespace drake

common/symbolic_decompose.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <string>
+#include <tuple>
 #include <unordered_map>
 #include <utility>
 
@@ -149,5 +150,27 @@ DecomposeAffineExpression(
   }
   return num_variable;
 }
+
+/** Given a vector of Expressions @p f and a list of @p parameters we define
+all additional variables in @p f to be a vector of "non-parameter variables",
+n.  This method returns a factorization of @p f into an equivalent "data
+matrix", W, which depends only on the non-parameter variables, and a "lumped
+parameter vector", α, which depends only on @p parameters: f =
+W(n)*α(parameters) + w0(n).
+
+@note The current implementation makes some simple attempts to minimize the
+number of lumped parameters, but more simplification could be implemented
+relatively easily.  Optimal simplification, however, involves the complexity of
+comparing two arbitrary Expressions (see Expression::EqualTo for more details).
+
+@throw std::exception if @p f is not decomposable in this way (cells containing
+@p parameters may only be added or multiplied with cells containing
+non-parameter variables).
+
+@returns W(n), α(parameters), and w0(n). */
+std::tuple<MatrixX<Expression>, VectorX<Expression>, VectorX<Expression>>
+DecomposeLumpedParameters(
+    const Eigen::Ref<const VectorX<Expression>>& f,
+    const Eigen::Ref<const VectorX<Variable>>& parameters);
 }  // namespace symbolic
 }  // namespace drake