Merge pull request RobotLocomotion#5043 from soonho-tri/pr-fix-expres…

…sionadd-substitute Fix ExpressionAdd::Substitute and update tests
patmarion · Feb 3, 2017 · 7d165b4 · 7d165b4
2 parents f620dc0 + 4934066
commit 7d165b4
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 11 deletions.
diff --git a/drake/common/symbolic_expression_cell.cc b/drake/common/symbolic_expression_cell.cc
@@ -396,7 +396,7 @@ double ExpressionAdd::Evaluate(const Environment& env) const {
 
 Expression ExpressionAdd::Substitute(const Substitution& s) const {
   return accumulate(
-      exp_to_coeff_map_.begin(), exp_to_coeff_map_.end(), Expression::Zero(),
+      exp_to_coeff_map_.begin(), exp_to_coeff_map_.end(), Expression{constant_},
       [&s](const Expression& init, const pair<Expression, double>& p) {
         return init + p.first.Substitute(s) * p.second;
       });

diff --git a/drake/common/test/symbolic_substitution_test.cc b/drake/common/test/symbolic_substitution_test.cc
@@ -89,12 +89,12 @@ TEST_F(SymbolicSubstitutionTest, CheckHomomorphismExpressionVarExpr) {
   fns.push_back([](const Expression& x) { return 2 * x; });
   fns.push_back([](const Expression& x) { return -x; });
   fns.push_back([](const Expression& x) { return -3 * x; });
-  fns.push_back([&](const Expression& x) { return x + y_; });
-  fns.push_back([&](const Expression& x) { return y_ + x; });
-  fns.push_back([&](const Expression& x) { return x - y_; });
-  fns.push_back([&](const Expression& x) { return y_ - x; });
-  fns.push_back([&](const Expression& x) { return x * z_; });
-  fns.push_back([&](const Expression& x) { return z_ * x; });
+  fns.push_back([&](const Expression& x) { return 5.0 + x + y_; });
+  fns.push_back([&](const Expression& x) { return 5.0 + y_ + x; });
+  fns.push_back([&](const Expression& x) { return 7.0 - x - y_; });
+  fns.push_back([&](const Expression& x) { return 7.0 - y_ - x; });
+  fns.push_back([&](const Expression& x) { return 3.0 * x * z_; });
+  fns.push_back([&](const Expression& x) { return 3.0 * z_ * x; });
   fns.push_back([&](const Expression& x) { return x / y_; });
   fns.push_back([&](const Expression& x) { return y_ / x; });
   fns.push_back([](const Expression& x) { return log(x); });
@@ -134,8 +134,8 @@ TEST_F(SymbolicSubstitutionTest, CheckHomomorphismExpressionVarExpr) {
   substs.emplace_back(var_x_, -1.0);
   substs.emplace_back(var_x_, 20.0);
   substs.emplace_back(var_x_, -30.0);
-  substs.emplace_back(var_x_, x_ + y_);
-  substs.emplace_back(var_x_, y_ + z_);
+  substs.emplace_back(var_x_, 7.0 + x_ + y_);
+  substs.emplace_back(var_x_, -3.0 + y_ + z_);
   substs.emplace_back(var_x_, x_ - y_);
   substs.emplace_back(var_x_, y_ - z_);
   substs.emplace_back(var_x_, x_ * y_);
@@ -163,8 +163,8 @@ TEST_F(SymbolicSubstitutionTest, CheckHomomorphismExpressionSubstitution) {
   fns.push_back([](const vector<Expression>& v) { return 2 * v[0]; });
   fns.push_back([](const vector<Expression>& v) { return -v[0]; });
   fns.push_back([](const vector<Expression>& v) { return -3 * v[0]; });
-  fns.push_back([](const vector<Expression>& v) { return v[0] + v[1]; });
-  fns.push_back([](const vector<Expression>& v) { return v[1] - v[2]; });
+  fns.push_back([](const vector<Expression>& v) { return 3.0 + v[0] + v[1]; });
+  fns.push_back([](const vector<Expression>& v) { return -3.0 + v[1] - v[2]; });
   fns.push_back([](const vector<Expression>& v) { return v[0] * v[2]; });
   fns.push_back([](const vector<Expression>& v) { return v[0] / v[1]; });
   fns.push_back([](const vector<Expression>& v) { return log(v[0]); });