Merge pull request RobotLocomotion#8494 from soonho-tri/python-bindin…

…g-evaluate

Python binding evaluate

bindings/pydrake/symbolic_py.cc

@@ -150,12 +150,14 @@ PYBIND11_MODULE(_symbolic_py, m) {
              return fmt::format("<Expression \"{}\">", self.to_string());
            })
       .def("__copy__",
-           [](const Expression& self) -> Expression {
-             return self;
-           })
+           [](const Expression& self) -> Expression { return self; })
       .def("to_string", &Expression::to_string)
       .def("Expand", &Expression::Expand)
       .def("Evaluate", [](const Expression& self) { return self.Evaluate(); })
+      .def("Evaluate",
+           [](const Expression& self, const Environment::map& env) {
+             return self.Evaluate(Environment{env});
+           })
       // Addition
       .def(py::self + py::self)
       .def(py::self + Variable())
@@ -287,6 +289,10 @@ PYBIND11_MODULE(_symbolic_py, m) {
   py::class_<Formula>(m, "Formula")
       .def("GetFreeVariables", &Formula::GetFreeVariables)
       .def("EqualTo", &Formula::EqualTo)
+      .def("Evaluate",
+           [](const Formula& self, const Environment::map& env) {
+             return self.Evaluate(Environment{env});
+           })
       .def("Substitute",
            [](const Formula& self, const Variable& var, const Expression& e) {
              return self.Substitute(var, e);
@@ -353,6 +359,10 @@ PYBIND11_MODULE(_symbolic_py, m) {
       .def("GetVariables", &Monomial::GetVariables)
       .def("get_powers", &Monomial::get_powers, py_reference_internal)
       .def("ToExpression", &Monomial::ToExpression)
+      .def("Evaluate",
+           [](const Monomial& self, const Environment::map& env) {
+             return self.Evaluate(Environment{env});
+           })
       .def("pow_in_place", &Monomial::pow_in_place, py_reference_internal)
       .def("__pow__",
            [](const Monomial& self, const int p) { return pow(self, p); });
@@ -413,6 +423,10 @@ PYBIND11_MODULE(_symbolic_py, m) {
            })
       .def("__pow__",
            [](const Polynomial& self, const int n) { return pow(self, n); })
+      .def("Evaluate",
+           [](const Polynomial& self, const Environment::map& env) {
+             return self.Evaluate(Environment{env});
+           })
       .def("Jacobian", [](const Polynomial& p,
                           const Eigen::Ref<const VectorX<Variable>>& vars) {
         return p.Jacobian(vars);

bindings/pydrake/test/symbolic_test.py

@@ -474,10 +474,10 @@ def test_functions_with_float(self):
         self._check_scalar(sym.ceil(v_x), np.ceil(v_x))
         self._check_scalar(sym.floor(v_x), np.floor(v_x))
         self._check_scalar(
-          sym.if_then_else(
-            sym.Expression(v_x) > sym.Expression(v_y),
-            v_x, v_y),
-          v_x if v_x > v_y else v_y)
+            sym.if_then_else(
+                sym.Expression(v_x) > sym.Expression(v_y),
+                v_x, v_y),
+            v_x if v_x > v_y else v_y)
 
     def test_non_method_jacobian(self):
         # Jacobian([x * cos(y), x * sin(y), x ** 2], [x, y]) returns
@@ -507,6 +507,22 @@ def test_differentiate(self):
     def test_repr(self):
         self.assertEqual(repr(e_x), '<Expression "x">')
 
+    def test_evaluate(self):
+        env = {x: 3.0,
+               y: 4.0}
+        self.assertEqual((x + y).Evaluate(env),
+                         env[x] + env[y])
+
+    def test_evaluate_exception_np_nan(self):
+        env = {x: np.nan}
+        with self.assertRaises(RuntimeError):
+            (x + 1).Evaluate(env)
+
+    def test_evaluate_exception_python_nan(self):
+        env = {x: float('nan')}
+        with self.assertRaises(RuntimeError):
+            (x + 1).Evaluate(env)
+
     # See `math_overloads_test` for more comprehensive checks on math
     # functions.
 
@@ -552,6 +568,22 @@ def test_static_true_false(self):
     def test_repr(self):
         self.assertEqual(repr(x > y), '<Formula "(x > y)">')
 
+    def test_evaluate(self):
+        env = {x: 3.0,
+               y: 4.0}
+        self.assertEqual((x > y).Evaluate(env),
+                         env[x] > env[y])
+
+    def test_evaluate_exception_np_nan(self):
+        env = {x: np.nan}
+        with self.assertRaises(RuntimeError):
+            (x > 1).Evaluate(env)
+
+    def test_evaluate_exception_python_nan(self):
+        env = {x: float('nan')}
+        with self.assertRaises(RuntimeError):
+            (x > 1).Evaluate(env)
+
 
 class TestSymbolicMonomial(unittest.TestCase):
     def test_constructor_variable(self):
@@ -667,6 +699,25 @@ def test_monomial_basis(self):
         self.assertEqual(basis1.size, 20)
         self.assertEqual(basis2.size, 20)
 
+    def test_evaluate(self):
+        m = sym.Monomial(x, 3) * sym.Monomial(y)  # m = x³y
+        env = {x: 2.0,
+               y: 3.0}
+        self.assertEqual(m.Evaluate(env),
+                         env[x] ** 3 * env[y])
+
+    def test_evaluate_exception_np_nan(self):
+        m = sym.Monomial(x, 3)
+        env = {x: np.nan}
+        with self.assertRaises(RuntimeError):
+            m.Evaluate(env)
+
+    def test_evaluate_exception_python_nan(self):
+        m = sym.Monomial(x, 3)
+        env = {x: float('nan')}
+        with self.assertRaises(RuntimeError):
+            m.Evaluate(env)
+
 
 class TestSymbolicPolynomial(unittest.TestCase):
     def test_default_constructor(self):
@@ -805,3 +856,24 @@ def test_hash(self):
         p1 += 1
         self.assertNotEqual(p1, p2)
         self.assertNotEqual(hash(p1), hash(p2))
+
+    def test_evaluate(self):
+        p = sym.Polynomial(a * x * x + b * x + c, [x])
+        env = {a: 2.0,
+               b: 3.0,
+               c: 5.0,
+               x: 2.0}
+        self.assertEqual(p.Evaluate(env),
+                         env[a] * env[x] * env[x] + env[b] * env[x] + env[c])
+
+    def test_evaluate_exception_np_nan(self):
+        p = sym.Polynomial(x * x, [x])
+        env = {x: np.nan}
+        with self.assertRaises(RuntimeError):
+            p.Evaluate(env)
+
+    def test_evaluate_exception_python_nan(self):
+        p = sym.Polynomial(x * x, [x])
+        env = {x: float('nan')}
+        with self.assertRaises(RuntimeError):
+            p.Evaluate(env)

common/symbolic_environment.cc

@@ -5,18 +5,20 @@
 #include <sstream>
 #include <stdexcept>
 #include <string>
+#include <utility>
 
 #include "drake/common/symbolic.h"
 
 namespace drake {
 namespace symbolic {
 
 using std::endl;
+using std::initializer_list;
+using std::move;
 using std::ostream;
 using std::ostringstream;
 using std::runtime_error;
 using std::string;
-using std::initializer_list;
 
 namespace {
 void throw_if_dummy(const Variable& var) {
@@ -35,19 +37,29 @@ void throw_if_nan(const double v) {
     throw runtime_error(oss.str());
   }
 }
-}  // anonymous namespace
 
-Environment::Environment(const initializer_list<value_type> init) : map_(init) {
-  for (const auto& p : init) {
-    throw_if_dummy(p.first);
-    throw_if_nan(p.second);
+// Given a list of variables, @p vars, builds an Environment::map which maps a
+// Variable to its double value. All values are set to 0.0.
+Environment::map BuildMap(const initializer_list<Environment::key_type> vars) {
+  Environment::map m;
+  for (const Environment::key_type& var : vars) {
+    m.emplace(var, 0.0);
   }
+  return m;
 }
 
-Environment::Environment(const initializer_list<key_type> vars) {
-  for (const auto& var : vars) {
-    throw_if_dummy(var);
-    map_.emplace(var, 0.0);
+}  // anonymous namespace
+
+Environment::Environment(const initializer_list<value_type> init)
+    : Environment{map(init)} {}
+
+Environment::Environment(const initializer_list<key_type> vars)
+    : Environment{BuildMap(vars)} {}
+
+Environment::Environment(map m) : map_{move(m)} {
+  for (const auto& p : map_) {
+    throw_if_dummy(p.first);
+    throw_if_nan(p.second);
   }
 }
 

common/symbolic_environment.h

@@ -69,13 +69,27 @@ class Environment {
   Environment() = default;
 
   /** List constructor. Constructs an environment from a list of (Variable *
-   * double). */
+   * double).
+   *
+   * @throws std::runtime_error if @p init include a dummy variable or a NaN
+   * value.
+   */
   Environment(std::initializer_list<value_type> init);
 
   /** List constructor. Constructs an environment from a list of
-   * Variable. Initializes the variables with 0.0. */
+   * Variable. Initializes the variables with 0.0.
+   *
+   * @throws std::runtime_error if @p vars include a dummy variable.
+   */
   Environment(std::initializer_list<key_type> vars);
 
+  /** Constructs an environment from @p m (of `map` type, which is
+   * `std::unordered_map`).
+   *
+   * @throws std::runtime_error if @p m include a dummy variable or a NaN value.
+   */
+  explicit Environment(map m);
+
   /** Returns an iterator to the beginning. */
   iterator begin() { return map_.begin(); }
   /** Returns an iterator to the end. */

common/test/symbolic_environment_test.cc

@@ -45,6 +45,27 @@ TEST_F(EnvironmentTest, InitializerListWithoutValues) {
   }
 }
 
+TEST_F(EnvironmentTest, InitWithMap) {
+  Environment::map m;
+  m.emplace(var_x_, 3.0);
+  m.emplace(var_y_, 4.0);
+  const Expression e{var_x_ + var_y_};
+  EXPECT_EQ(e.Evaluate(Environment{m}), 7.0);
+}
+
+TEST_F(EnvironmentTest, InitWithMapExceptionDummyVariable) {
+  Environment::map m;
+  const Variable dummy;
+  m.emplace(dummy, 3.0);
+  EXPECT_THROW(Environment{m}, runtime_error);
+}
+
+TEST_F(EnvironmentTest, InitWithMapExceptionNan) {
+  Environment::map m;
+  m.emplace(var_x_, NAN);
+  EXPECT_THROW(Environment{m}, runtime_error);
+}
+
 TEST_F(EnvironmentTest, insert_find) {
   Environment env1{{var_x_, 2}, {var_y_, 3}, {var_z_, 4}};
   const Environment env2{env1};