Skip to content

Commit

Permalink
call symbolic::Polynomial::CheckInvariant only in debug mode. (RobotL…
Browse files Browse the repository at this point in the history 
…ocomotion#10940)

* call symbolic::Polynomial::CheckInvariant only in debug mode.
  • Loading branch information
@hongkai-dai @sammy-tri
hongkai-dai authored and sammy-tri committed Mar 19, 2019
1 parent 714d1d0 commit bbb3552
Show file tree
Hide file tree
Showing 3 changed files with 94 additions and 68 deletions.
11 changes: 7 additions & 4 deletions common/symbolic_polynomial.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -340,7 +340,7 @@ class DecomposePolynomialVisitor {

Polynomial::Polynomial(MapType init)
: monomial_to_coefficient_map_{move(init)} {
CheckInvariant();
DRAKE_ASSERT_VOID(CheckInvariant());
};

Polynomial::Polynomial(const Monomial& m)
Expand Down Expand Up @@ -517,7 +517,7 @@ Polynomial& Polynomial::operator+=(const Polynomial& p) {
const Expression& coeff{item.second};
DoAddProduct(coeff, m, &monomial_to_coefficient_map_);
}
CheckInvariant();
DRAKE_ASSERT_VOID(CheckInvariant());
return *this;
}

Expand Down Expand Up @@ -559,7 +559,7 @@ Polynomial& Polynomial::operator*=(const Polynomial& p) {
}
}
monomial_to_coefficient_map_ = std::move(new_map);
CheckInvariant();
DRAKE_ASSERT_VOID(CheckInvariant());
return *this;
}

Expand All @@ -572,7 +572,7 @@ Polynomial& Polynomial::operator*=(const Monomial& m) {
new_map.emplace(m * m_i, coeff_i);
}
monomial_to_coefficient_map_ = std::move(new_map);
CheckInvariant();
DRAKE_ASSERT_VOID(CheckInvariant());
return *this;
}

Expand Down Expand Up @@ -670,6 +670,9 @@ Polynomial Polynomial::RemoveTermsWithSmallCoefficients(
}

void Polynomial::CheckInvariant() const {
// TODO(hongkai.dai and soonho.kong): improves the computation time of
// CheckInvariant(). See github issue
// https://github.com/RobotLocomotion/drake/issues/10229
Variables vars{intersect(decision_variables(), indeterminates())};
if (!vars.empty()) {
ostringstream oss;
Expand Down
4 changes: 3 additions & 1 deletion common/test/symbolic_polynomial_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,9 @@ TEST_F(SymbolicPolynomialTest, ConstructFromMapType3) {
// We cannot construct a polynomial from the `map` because variable a is used
// as a decision variable (x ↦ -2a) and an indeterminate (a² ↦ 4b) at the same
// time.
EXPECT_THROW(Polynomial{map}, runtime_error);
if (kDrakeAssertIsArmed) {
EXPECT_THROW(Polynomial{map}, runtime_error);
}
}

TEST_F(SymbolicPolynomialTest, ConstructFromMonomial) {
Expand Down
147 changes: 84 additions & 63 deletions common/test/symbolic_rational_function_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,24 +147,35 @@ TEST_F(SymbolicRationalFunctionTest, Addition) {
EXPECT_PRED2(RationalFunctionEqual, f1_f2_sum, f1_f2_sum_expected);
// p5, p6 contains variable a in its indeterminates.
const RationalFunction f3(p1_, p3_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 + RationalFunction(p5_, p6_),
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 + RationalFunction(p5_, p2_),
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 + RationalFunction(p2_, p5_),
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p6_) + f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p2_) + f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p2_, p5_) + f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p6_) += f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p2_) += f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p2_, p5_) += f3,
std::runtime_error, polynomial_invariant_error_);
if (kDrakeAssertIsArmed) {
DRAKE_EXPECT_THROWS_MESSAGE(f3 + RationalFunction(p5_, p6_),
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 + RationalFunction(p5_, p2_),
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 + RationalFunction(p2_, p5_),
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p6_) + f3,
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p2_) + f3,
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p2_, p5_) + f3,
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p6_) += f3,
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p2_) += f3,
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p2_, p5_) += f3,
std::runtime_error,
polynomial_invariant_error_);
}

const RationalFunction f1_p3_sum_expected(p1_ + p2_ * p3_, p2_);
EXPECT_PRED2(RationalFunctionEqual, f1 + p3_, f1_p3_sum_expected);
Expand All @@ -173,10 +184,12 @@ TEST_F(SymbolicRationalFunctionTest, Addition) {
f1_p3_sum += p3_;
EXPECT_PRED2(RationalFunctionEqual, f1_p3_sum, f1_p3_sum_expected);
// p5 contains variable a in its indeterminates.
DRAKE_EXPECT_THROWS_MESSAGE(f3 + p5_, std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(p5_ + f3, std::runtime_error,
polynomial_invariant_error_);
if (kDrakeAssertIsArmed) {
DRAKE_EXPECT_THROWS_MESSAGE(f3 + p5_, std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(p5_ + f3, std::runtime_error,
polynomial_invariant_error_);
}

const double c = 2;
const RationalFunction f1_c_sum_expected(p1_ + c * p2_, p2_);
Expand All @@ -198,24 +211,35 @@ TEST_F(SymbolicRationalFunctionTest, Subtraction) {
EXPECT_PRED2(RationalFunctionEqual, f1_minus_f2, f1_minus_f2_expected);
// p5, p6 contains variable a in its indeterminates.
const RationalFunction f3(p1_, p3_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 - RationalFunction(p5_, p6_),
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 - RationalFunction(p5_, p2_),
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 - RationalFunction(p2_, p5_),
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p6_) - f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p2_) - f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p2_, p5_) - f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p6_) -= f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p2_) -= f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p2_, p5_) -= f3,
std::runtime_error, polynomial_invariant_error_);
if (kDrakeAssertIsArmed) {
DRAKE_EXPECT_THROWS_MESSAGE(f3 - RationalFunction(p5_, p6_),
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 - RationalFunction(p5_, p2_),
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 - RationalFunction(p2_, p5_),
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p6_) - f3,
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p2_) - f3,
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p2_, p5_) - f3,
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p6_) -= f3,
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p2_) -= f3,
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p2_, p5_) -= f3,
std::runtime_error,
polynomial_invariant_error_);
}

const RationalFunction f1_minus_p3_expected(p1_ - p2_ * p3_, p2_);
EXPECT_PRED2(RationalFunctionEqual, f1 - p3_, f1_minus_p3_expected);
Expand All @@ -224,10 +248,12 @@ TEST_F(SymbolicRationalFunctionTest, Subtraction) {
f1_minus_p3 -= p3_;
EXPECT_PRED2(RationalFunctionEqual, f1_minus_p3, f1_minus_p3_expected);
// p5 contains variable a in its indeterminates.
DRAKE_EXPECT_THROWS_MESSAGE(f3 - p5_, std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(p5_ - f3, std::runtime_error,
polynomial_invariant_error_);
if (kDrakeAssertIsArmed) {
DRAKE_EXPECT_THROWS_MESSAGE(f3 - p5_, std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(p5_ - f3, std::runtime_error,
polynomial_invariant_error_);
}

const double c = 2;
const RationalFunction f1_minus_c_expected(p1_ - p2_ * c, p2_);
Expand All @@ -249,33 +275,30 @@ TEST_F(SymbolicRationalFunctionTest, Product) {
EXPECT_PRED2(RationalFunctionEqual, f1_times_f2, f1_times_f2_expected);
// p5, p6 contains variable a in its indeterminates.
const RationalFunction f3(p1_, p3_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 * RationalFunction(p5_, p6_),
std::runtime_error, polynomial_invariant_error_);
if (kDrakeAssertIsArmed) {
DRAKE_EXPECT_THROWS_MESSAGE(f3 * RationalFunction(p5_, p6_),
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 * RationalFunction(p5_, p2_),
std::logic_error,
rational_function_indeterminates_error_);
}
DRAKE_EXPECT_THROWS_MESSAGE(f3 * RationalFunction(p2_, p5_),
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p6_) * f3,
std::runtime_error, polynomial_invariant_error_);
if (kDrakeAssertIsArmed) {
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p2_) * f3,
std::logic_error,
rational_function_indeterminates_error_);
}
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p2_, p5_) * f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p6_) *= f3,
std::runtime_error, polynomial_invariant_error_);
if (kDrakeAssertIsArmed) {
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p2_) *= f3,
std::logic_error,
rational_function_indeterminates_error_);
}
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p2_, p5_) *= f3,
std::runtime_error, polynomial_invariant_error_);
}

const RationalFunction f1_times_p3_expected(p1_ * p3_, p2_);
EXPECT_PRED2(RationalFunctionEqual, f1 * p3_, f1_times_p3_expected);
Expand Down Expand Up @@ -311,29 +334,27 @@ TEST_F(SymbolicRationalFunctionTest, Division) {
EXPECT_PRED2(RationalFunctionEqual, f1_divides_f2, f1_divides_f2_expected);
// p5, p6 contains variable a in its indeterminates.
const RationalFunction f3(p1_, p3_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 / RationalFunction(p5_, p6_),
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 / RationalFunction(p5_, p2_),
std::runtime_error, polynomial_invariant_error_);
if (kDrakeAssertIsArmed) {
DRAKE_EXPECT_THROWS_MESSAGE(f3 / RationalFunction(p5_, p6_),
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 / RationalFunction(p5_, p2_),
std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(f3 / RationalFunction(p2_, p5_),
std::logic_error,
rational_function_indeterminates_error_);
}
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p6_) / f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p2_) / f3,
std::runtime_error, polynomial_invariant_error_);
if (kDrakeAssertIsArmed) {
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p2_, p5_) / f3,
std::logic_error,
rational_function_indeterminates_error_);
}
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p6_) /= f3,
std::runtime_error, polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p5_, p2_) /= f3,
std::runtime_error, polynomial_invariant_error_);
if (kDrakeAssertIsArmed) {
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p2_, p5_) /= f3,
std::logic_error,
rational_function_indeterminates_error_);
Expand All @@ -345,11 +366,11 @@ TEST_F(SymbolicRationalFunctionTest, Division) {
RationalFunction p3_divides_f1 = f1;
p3_divides_f1 /= p3_;
EXPECT_PRED2(RationalFunctionEqual, p3_divides_f1, p3_divides_f1_expected);
DRAKE_EXPECT_THROWS_MESSAGE(f3 / p5_, std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(p5_ / f3, std::runtime_error,
polynomial_invariant_error_);
if (kDrakeAssertIsArmed) {
DRAKE_EXPECT_THROWS_MESSAGE(f3 / p5_, std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(p5_ / f3, std::runtime_error,
polynomial_invariant_error_);
DRAKE_EXPECT_THROWS_MESSAGE(RationalFunction(p3_, p1_) / p5_,
std::logic_error,
rational_function_indeterminates_error_);
Expand Down

0 comments on commit bbb3552

Please sign in to comment.