Skip to content

Commit

Permalink
Move Polynomial and TrigPoly into drake namespace
Browse files Browse the repository at this point in the history
  • Loading branch information
soonho-tri committed Mar 27, 2020
1 parent 301d596 commit 1570300
Show file tree
Hide file tree
Showing 15 changed files with 94 additions and 9 deletions.
1 change: 1 addition & 0 deletions bindings/pydrake/math_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
namespace drake {
namespace pydrake {

using std::pow;
using symbolic::Expression;

namespace {
Expand Down
2 changes: 1 addition & 1 deletion bindings/pydrake/polynomial_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ PYBIND11_MODULE(polynomial, m) {
{
using T = double;
using Class = Polynomial<T>;
constexpr auto& cls_doc = pydrake_doc.Polynomial;
constexpr auto& cls_doc = pydrake_doc.drake.Polynomial;
py::class_<Class>(m, "Polynomial", cls_doc.doc)
.def(py::init<>(), cls_doc.ctor.doc_0args)
.def(py::init<const T&>(), cls_doc.ctor.doc_1args_scalar)
Expand Down
2 changes: 1 addition & 1 deletion bindings/pydrake/polynomial_types_pybind.h
Original file line number Diff line number Diff line change
Expand Up @@ -9,4 +9,4 @@
// The macro `PYBIND11_NUMPY_OBJECT_DTYPE` places symbols into the namespace
// `pybind11::detail`, so we should not place these in `drake::pydrake`.

PYBIND11_NUMPY_OBJECT_DTYPE(Polynomial<double>);
PYBIND11_NUMPY_OBJECT_DTYPE(drake::Polynomial<double>);
2 changes: 2 additions & 0 deletions bindings/pydrake/symbolic_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -521,6 +521,8 @@ PYBIND11_MODULE(symbolic, m) {
},
doc.MonomialBasis.doc_2args);

using symbolic::Polynomial;

// TODO(m-chaturvedi) Add Pybind11 documentation for operator overloads, etc.
py::class_<Polynomial>(m, "Polynomial", doc.Polynomial.doc)
.def(py::init<>(), doc.Polynomial.ctor.doc_0args)
Expand Down
3 changes: 3 additions & 0 deletions common/polynomial.cc
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@ using std::runtime_error;
using std::string;
using std::vector;

namespace drake {
template <typename T>
bool Polynomial<T>::Monomial::HasSameExponents(
const Monomial& other) const {
Expand Down Expand Up @@ -613,3 +614,5 @@ template class Polynomial<double>;

// template class Polynomial<std::complex<double>>;
// doesn't work yet because the roots solver can't handle it

} // namespace drake
21 changes: 21 additions & 0 deletions common/polynomial.h
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,9 @@

#include "drake/common/autodiff.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_deprecated.h"

namespace drake {
/** A scalar multi-variate polynomial, modeled after the msspoly in spotless.
*
* Polynomial represents a list of additive Monomials, each one of which is a
Expand Down Expand Up @@ -475,3 +477,22 @@ typedef Polynomial<double> Polynomiald;

/// A column vector of polynomials; used in several optimization classes.
typedef Eigen::Matrix<Polynomiald, Eigen::Dynamic, 1> VectorXPoly;
} // namespace drake

/** Provides power function for Polynomial. */
template <typename T>
DRAKE_DEPRECATED("2020-07-01", "Use drake::pow instead.")
drake::Polynomial<T> pow(const drake::Polynomial<T>& base,
typename drake::Polynomial<T>::PowerType exponent) {
return drake::pow(base, exponent);
}

template <typename T = double>
using Polynomial DRAKE_DEPRECATED(
"2020-07-01", "Use drake::Polynomial instead.") = drake::Polynomial<T>;

using Polynomiald DRAKE_DEPRECATED("2020-07-01", "Use drake::Polynomiald.") =
drake::Polynomial<double>;

using VectorXPoly DRAKE_DEPRECATED("2020-07-01", "Use drake::VectorXPoly.") =
Eigen::Matrix<drake::Polynomiald, Eigen::Dynamic, 1>;
21 changes: 21 additions & 0 deletions common/test/polynomial_test.cc
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#include "drake/common/polynomial.h"

#include <cmath>
#include <cstddef>
#include <map>
#include <sstream>
Expand All @@ -19,6 +20,8 @@ using std::uniform_real_distribution;
namespace drake {
namespace {

using std::pow;

template <typename T>
void testIntegralAndDerivative() {
VectorXd coefficients = VectorXd::Random(5);
Expand Down Expand Up @@ -408,5 +411,23 @@ GTEST_TEST(PolynomialTest, EvaluatePartial) {
(5 * x * x * x) + 1);
}

// Checks deprecated aliases.
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"

// TODO(soonho-tri): Remove the following checks when we remove ::Polynomial.
static_assert(
std::is_same<::Polynomial<double>, drake::Polynomial<double>>::value,
"::Polynomial should be an alias of drake::Polynomial.");
static_assert(std::is_same<::Polynomiald, drake::Polynomiald>::value,
"::Polynomiald should be an alias of drake::Polynomiald.");
static_assert(std::is_same<::VectorXPoly, drake::VectorXPoly>::value,
"::VectorXPoly should be an alias of drake::VectorXPoly.");
GTEST_TEST(PolynomialTest, DeprecatedPow) {
const Polynomiald x = Polynomiald("x");
EXPECT_EQ(::pow(x, 2), x * x);
}
#pragma GCC diagnostic pop

} // anonymous namespace
} // namespace drake
14 changes: 14 additions & 0 deletions common/test/trig_poly_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,20 @@ GTEST_TEST(TrigPolyTest, EvaluatePartialTest) {
theta + cos(theta) + sin(theta + 1));
}

// Checks deprecated aliases.
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"

// TODO(soonho-tri): Remove the following checks when we remove ::TrigPoly.
static_assert(std::is_same<::TrigPoly<double>, drake::TrigPoly<double>>::value,
"::TrigPoly should be an alias of drake::TrigPoly.");
static_assert(std::is_same<::TrigPolyd, drake::TrigPolyd>::value,
"::TrigPolyd should be an alias of drake::TrigPolyd.");
static_assert(
std::is_same<::VectorXTrigPoly, drake::VectorXTrigPoly>::value,
"::VectorXTrigPoly should be an alias of drake::VectorXTrigPoly.");
#pragma GCC diagnostic pop

} // anonymous namespace
} // namespace util
} // namespace drake
16 changes: 16 additions & 0 deletions common/trig_poly.h
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,11 @@

#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/drake_deprecated.h"
#include "drake/common/polynomial.h"

namespace drake {

/** A scalar multi-variate polynomial containing sines and cosines.
*
* TrigPoly represents a Polynomial some of whose variables actually represent
Expand Down Expand Up @@ -464,3 +467,16 @@ typedef TrigPoly<double> TrigPolyd;

/// A column vector of TrigPoly; used in several optimization classes.
typedef Eigen::Matrix<TrigPolyd, Eigen::Dynamic, 1> VectorXTrigPoly;

} // namespace drake

template <typename T = double>
using TrigPoly DRAKE_DEPRECATED("2020-07-01", "Use drake::TrigPoly instead.") =
drake::TrigPoly<T>;

using TrigPolyd DRAKE_DEPRECATED("2020-07-01", "Use drake::TrigPolyd.") =
drake::TrigPoly<double>;

using VectorXTrigPoly DRAKE_DEPRECATED("2020-07-01",
"Use drake::VectorXTrigPoly.") =
Eigen::Matrix<drake::TrigPolyd, Eigen::Dynamic, 1>;
2 changes: 2 additions & 0 deletions geometry/proximity/test/hydroelastic_internal_test.cc
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#include "drake/geometry/proximity/hydroelastic_internal.h"

#include <cmath>
#include <functional>
#include <limits>

Expand All @@ -19,6 +20,7 @@ namespace {

using Eigen::Vector3d;
using std::function;
using std::pow;

// Tests the simple public API of the hydroelastic::Geometries: adding
// geometries and querying the data stored.
Expand Down
Original file line number Diff line number Diff line change
@@ -1,11 +1,15 @@
#include "drake/multibody/optimization/sliding_friction_complementarity_constraint.h"

#include <cmath>
#include <limits>
#include <memory>

#include "drake/multibody/inverse_kinematics/kinematic_constraint_utilities.h"

namespace drake {

using std::pow;

namespace multibody {
const double kInf = std::numeric_limits<double>::infinity();
namespace internal {
Expand Down
2 changes: 2 additions & 0 deletions solvers/constraint.cc
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,8 @@ std::ostream& QuadraticConstraint::DoDisplay(
template <typename DerivedX, typename ScalarY>
void LorentzConeConstraint::DoEvalGeneric(const Eigen::MatrixBase<DerivedX>& x,
VectorX<ScalarY>* y) const {
using std::pow;

const VectorX<ScalarY> z = A_dense_ * x.template cast<ScalarY>() + b_;
y->resize(num_constraints());
(*y)(0) = z(0);
Expand Down
7 changes: 4 additions & 3 deletions solvers/mixed_integer_rotation_constraint.cc
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#include "drake/solvers/mixed_integer_rotation_constraint.h"

#include <cmath>
#include <limits>

#include "drake/math/gray_code.h"
Expand Down Expand Up @@ -46,13 +47,13 @@ void AddUnitLengthConstraintWithSos2Lambda(
const symbolic::Expression x2{phi.dot(lambda2.cast<symbolic::Expression>())};
for (int phi0_idx = 0; phi0_idx < num_phi; phi0_idx++) {
const symbolic::Expression x0_square_lb{2 * phi(phi0_idx) * x0 -
pow(phi(phi0_idx), 2)};
std::pow(phi(phi0_idx), 2)};
for (int phi1_idx = 0; phi1_idx < num_phi; phi1_idx++) {
const symbolic::Expression x1_square_lb{2 * phi(phi1_idx) * x1 -
pow(phi(phi1_idx), 2)};
std::pow(phi(phi1_idx), 2)};
for (int phi2_idx = 0; phi2_idx < num_phi; phi2_idx++) {
const symbolic::Expression x2_square_lb{2 * phi(phi2_idx) * x2 -
pow(phi(phi2_idx), 2)};
std::pow(phi(phi2_idx), 2)};
symbolic::Expression x_sum_of_squares_lb{x0_square_lb + x1_square_lb +
x2_square_lb};
if (!is_constant(x_sum_of_squares_lb)) {
Expand Down
4 changes: 1 addition & 3 deletions solvers/system_identification.cc
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,6 @@
#include "drake/solvers/mathematical_program.h"
#include "drake/solvers/solve.h"

using std::pow;

namespace drake {
namespace solvers {

Expand Down Expand Up @@ -323,7 +321,7 @@ SystemIdentification<T>::EstimateParameters(
}
T error_squared = 0;
for (int i = 0; i < num_err_terms; i++) {
error_squared += pow(result.GetSolution(error_variables(i)), 2);
error_squared += std::pow(result.GetSolution(error_variables(i)), 2);
}

return std::make_pair(estimates, std::sqrt(error_squared / num_err_terms));
Expand Down
2 changes: 1 addition & 1 deletion solvers/system_identification.h
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ namespace solvers {
template <typename T>
class SystemIdentification {
public:
typedef ::Polynomial<T> PolyType;
typedef Polynomial<T> PolyType;
typedef typename PolyType::Monomial MonomialType;
typedef typename PolyType::Term TermType;
typedef typename PolyType::VarType VarType;
Expand Down

0 comments on commit 1570300

Please sign in to comment.