diff --git a/drake/systems/controllers/BUILD.bazel b/drake/systems/controllers/BUILD.bazel index dcebd5501b43..e1aca255199a 100644 --- a/drake/systems/controllers/BUILD.bazel +++ b/drake/systems/controllers/BUILD.bazel @@ -158,6 +158,19 @@ drake_cc_library( ], ) +drake_cc_library( + name = "polynomial_encode_decode", + srcs = ["polynomial_encode_decode.cc"], + hdrs = ["polynomial_encode_decode.h"], + deps = [ + "//drake/common/trajectories:piecewise_polynomial", + "//drake/lcmtypes:piecewise_polynomial", + "//drake/lcmtypes:polynomial", + "//drake/lcmtypes:polynomial_matrix", + "//drake/util", + ], +) + # This library is empty unless Gurobi is available. drake_cc_library( name = "instantaneous_qp_controller", @@ -171,6 +184,7 @@ drake_cc_library( }), deps = [ ":control_util", + ":polynomial_encode_decode", ":qp_common", ":yaml_util", "//drake/common:essential", @@ -187,6 +201,8 @@ drake_cc_library( ], ) +# === test/ === + drake_cc_library( name = "zmp_test_util", testonly = 1, @@ -197,7 +213,14 @@ drake_cc_library( ], ) -# === test/ === +drake_cc_googletest( + name = "polynomial_encode_decode_test", + deps = [ + ":polynomial_encode_decode", + "//drake/common/test_utilities", + "//drake/common/trajectories:random_piecewise_polynomial", + ], +) drake_cc_googletest( name = "inverse_dynamics_test", diff --git a/drake/systems/controllers/InstantaneousQPController.cpp b/drake/systems/controllers/InstantaneousQPController.cpp index d75bfb6bd7e2..0fb83f9c9c7e 100644 --- a/drake/systems/controllers/InstantaneousQPController.cpp +++ b/drake/systems/controllers/InstantaneousQPController.cpp @@ -20,6 +20,7 @@ #include "drake/multibody/parsers/urdf_parser.h" #include "drake/solvers/fast_qp.h" #include "drake/systems/controllers/controlUtil.h" +#include "drake/systems/controllers/polynomial_encode_decode.h" #include "drake/systems/controllers/yaml_util.h" #include "drake/util/drakeGeometryUtil.h" #include "drake/util/lcmUtil.h" diff --git a/drake/systems/controllers/polynomial_encode_decode.cc b/drake/systems/controllers/polynomial_encode_decode.cc new file mode 100644 index 000000000000..a7fd9753562b --- /dev/null +++ b/drake/systems/controllers/polynomial_encode_decode.cc @@ -0,0 +1,48 @@ +#include "drake/systems/controllers/polynomial_encode_decode.h" + +#include + +#include "drake/util/drakeUtil.h" + +using Eigen::Dynamic; +using Eigen::Map; +using Eigen::VectorXd; + +void encodePolynomial(const Polynomial& polynomial, + // NOLINTNEXTLINE(runtime/references) + drake::lcmt_polynomial& msg) { + eigenVectorToStdVector(polynomial.GetCoefficients(), msg.coefficients); + msg.num_coefficients = polynomial.GetNumberOfCoefficients(); +} + +Polynomial decodePolynomial(const drake::lcmt_polynomial& msg) { + Map coefficients(msg.coefficients.data(), + msg.coefficients.size()); + return Polynomial(coefficients); +} + +void encodePiecewisePolynomial( + const PiecewisePolynomial& piecewise_polynomial, + // NOLINTNEXTLINE(runtime/references) + drake::lcmt_piecewise_polynomial& msg) { + msg.num_segments = piecewise_polynomial.getNumberOfSegments(); + msg.num_breaks = piecewise_polynomial.getNumberOfSegments() + 1; + msg.breaks = piecewise_polynomial.getSegmentTimes(); + msg.polynomial_matrices.resize(piecewise_polynomial.getNumberOfSegments()); + for (int i = 0; i < piecewise_polynomial.getNumberOfSegments(); ++i) { + encodePolynomialMatrix( + piecewise_polynomial.getPolynomialMatrix(i), + msg.polynomial_matrices[i]); + } +} + +PiecewisePolynomial decodePiecewisePolynomial( + const drake::lcmt_piecewise_polynomial& msg) { + typedef PiecewisePolynomial::PolynomialMatrix PolynomialMatrix; + std::vector polynomial_matrices; + for (size_t i = 0; i < msg.polynomial_matrices.size(); ++i) { + polynomial_matrices.push_back( + decodePolynomialMatrix(msg.polynomial_matrices[i])); + } + return PiecewisePolynomial(polynomial_matrices, msg.breaks); +} diff --git a/drake/systems/controllers/polynomial_encode_decode.h b/drake/systems/controllers/polynomial_encode_decode.h new file mode 100644 index 000000000000..99d69a872449 --- /dev/null +++ b/drake/systems/controllers/polynomial_encode_decode.h @@ -0,0 +1,55 @@ +#pragma once + +#include + +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/lcmt_piecewise_polynomial.hpp" +#include "drake/lcmt_polynomial.hpp" +#include "drake/lcmt_polynomial_matrix.hpp" + +void encodePolynomial(const Polynomial& polynomial, + // NOLINTNEXTLINE(runtime/references) + drake::lcmt_polynomial& msg); + +Polynomial decodePolynomial( + const drake::lcmt_polynomial& msg); + +template +void encodePolynomialMatrix( + const Eigen::Matrix, RowsAtCompileTime, + ColsAtCompileTime>& polynomial_matrix, + // NOLINTNEXTLINE(runtime/references) + drake::lcmt_polynomial_matrix& msg) { + msg.polynomials.clear(); + msg.polynomials.resize(polynomial_matrix.rows()); + for (int row = 0; row < polynomial_matrix.rows(); ++row) { + auto& polynomial_msg_row = msg.polynomials[row]; + polynomial_msg_row.resize(polynomial_matrix.cols()); + for (int col = 0; col < polynomial_matrix.cols(); ++col) { + encodePolynomial(polynomial_matrix(row, col), polynomial_msg_row[col]); + } + } + msg.rows = polynomial_matrix.rows(); + msg.cols = polynomial_matrix.cols(); +} + +template +Eigen::Matrix, RowsAtCompileTime, ColsAtCompileTime> +decodePolynomialMatrix(const drake::lcmt_polynomial_matrix& msg) { + Eigen::Matrix, RowsAtCompileTime, ColsAtCompileTime> ret( + msg.rows, msg.cols); + for (int row = 0; row < msg.rows; ++row) { + for (int col = 0; col < msg.cols; ++col) { + ret(row, col) = decodePolynomial(msg.polynomials[row][col]); + } + } + return ret; +} + +void encodePiecewisePolynomial( + const PiecewisePolynomial& piecewise_polynomial, + // NOLINTNEXTLINE(runtime/references) + drake::lcmt_piecewise_polynomial& msg); + +PiecewisePolynomial decodePiecewisePolynomial( + const drake::lcmt_piecewise_polynomial& msg); diff --git a/drake/systems/controllers/test/polynomial_encode_decode_test.cc b/drake/systems/controllers/test/polynomial_encode_decode_test.cc new file mode 100644 index 000000000000..142675e79541 --- /dev/null +++ b/drake/systems/controllers/test/polynomial_encode_decode_test.cc @@ -0,0 +1,66 @@ +#include "drake/systems/controllers/polynomial_encode_decode.h" + +#include + +#include "drake/common/test_utilities/random_polynomial_matrix.h" +#include "drake/common/trajectories/test/random_piecewise_polynomial.h" +#include "drake/math/random_rotation.h" + +using Eigen::Dynamic; +using Eigen::VectorXd; + +namespace drake { +namespace { + +// TODO(jwnimmer-tri) Unit tests should not use unseeded randomness. + +GTEST_TEST(TestLcmUtil, testPolynomial) { + std::default_random_engine generator; + int max_num_coefficients = 5; + std::uniform_int_distribution<> int_distribution(1, max_num_coefficients); + int num_coefficients = int_distribution(generator); + VectorXd coefficients = VectorXd::Random(num_coefficients); + Polynomial poly(coefficients); + drake::lcmt_polynomial msg; + encodePolynomial(poly, msg); + auto poly_back = decodePolynomial(msg); + EXPECT_TRUE(poly.IsApprox(poly_back, 1e-8)); +} + +GTEST_TEST(TestLcmUtil, testPolynomialMatrix) { + auto poly_matrix = drake::test::RandomPolynomialMatrix(6, 5, 8); + drake::lcmt_polynomial_matrix msg; + encodePolynomialMatrix(poly_matrix, msg); + EXPECT_EQ(static_cast(msg.rows), static_cast(poly_matrix.rows())); + EXPECT_EQ(static_cast(msg.cols), static_cast(poly_matrix.cols())); + auto poly_matrix_back = decodePolynomialMatrix(msg); + EXPECT_EQ(poly_matrix.rows(), poly_matrix_back.rows()); + EXPECT_EQ(poly_matrix.cols(), poly_matrix_back.cols()); + for (int row = 0; row < msg.rows; ++row) { + for (int col = 0; col < msg.cols; ++col) { + EXPECT_TRUE( + poly_matrix(row, col).IsApprox(poly_matrix_back(row, col), 1e-8)); + } + } +} + +GTEST_TEST(TestLcmUtil, testPiecewisePolynomial) { + std::default_random_engine generator; + int num_segments = 6; + int rows = 4; + int cols = 7; + int num_coefficients = 3; + std::vector segment_times = + PiecewiseFunction::randomSegmentTimes(num_segments, generator); + PiecewisePolynomial piecewise_polynomial = + test::MakeRandomPiecewisePolynomial( + rows, cols, num_coefficients, segment_times); + drake::lcmt_piecewise_polynomial msg; + encodePiecewisePolynomial(piecewise_polynomial, msg); + auto piecewise_polynomial_back = decodePiecewisePolynomial(msg); + EXPECT_TRUE(piecewise_polynomial_back.isApprox(piecewise_polynomial, 1e-10)); +} + +} // namespace +} // namespace drake + diff --git a/drake/systems/robotInterfaces/BUILD.bazel b/drake/systems/robotInterfaces/BUILD.bazel index 2bdd91fa2101..100c56bdabd7 100644 --- a/drake/systems/robotInterfaces/BUILD.bazel +++ b/drake/systems/robotInterfaces/BUILD.bazel @@ -43,6 +43,7 @@ drake_cc_library( "//drake/math:geometric_transform", "//drake/math:gradient", "//drake/multibody:rigid_body_tree", + "//drake/systems/controllers:polynomial_encode_decode", "//drake/systems/controllers:zmp_util", "//drake/util", "//drake/util:lcm_util", diff --git a/drake/systems/robotInterfaces/QPLocomotionPlan.cpp b/drake/systems/robotInterfaces/QPLocomotionPlan.cpp index 55930bc063f1..5875a245915f 100644 --- a/drake/systems/robotInterfaces/QPLocomotionPlan.cpp +++ b/drake/systems/robotInterfaces/QPLocomotionPlan.cpp @@ -18,6 +18,7 @@ #include "drake/math/gradient.h" #include "drake/math/quaternion.h" #include "drake/math/rotation_matrix.h" +#include "drake/systems/controllers/polynomial_encode_decode.h" #include "drake/systems/robotInterfaces/convex_hull.h" #include "drake/systems/robotInterfaces/verify_subtype_sizes.h" #include "drake/util/drakeGeometryUtil.h" diff --git a/drake/util/BUILD.bazel b/drake/util/BUILD.bazel index 3edf51fdc988..4020aebd93e4 100644 --- a/drake/util/BUILD.bazel +++ b/drake/util/BUILD.bazel @@ -27,10 +27,6 @@ drake_cc_library( hdrs = ["lcmUtil.h"], deps = [ ":util", - "//drake/common/trajectories:piecewise_polynomial", - "//drake/lcmtypes:piecewise_polynomial", - "//drake/lcmtypes:polynomial", - "//drake/lcmtypes:polynomial_matrix", "//drake/math:geometric_transform", "@eigen", "@lcmtypes_bot2_core", diff --git a/drake/util/lcmUtil.cpp b/drake/util/lcmUtil.cpp index 262671e996ac..0b32ef4bd0f2 100644 --- a/drake/util/lcmUtil.cpp +++ b/drake/util/lcmUtil.cpp @@ -59,40 +59,3 @@ drake::TwistVector DecodeTwist(const bot_core::twist_t& msg) { ret.tail<3>() = DecodeVector3d(msg.linear_velocity); return ret; } - -void encodePolynomial(const Polynomial& polynomial, - drake::lcmt_polynomial& msg) { - eigenVectorToStdVector(polynomial.GetCoefficients(), msg.coefficients); - msg.num_coefficients = polynomial.GetNumberOfCoefficients(); -} - -Polynomial decodePolynomial(const drake::lcmt_polynomial& msg) { - Map coefficients(msg.coefficients.data(), - msg.coefficients.size()); - return Polynomial(coefficients); -} - -void encodePiecewisePolynomial( - const PiecewisePolynomial& piecewise_polynomial, - drake::lcmt_piecewise_polynomial& msg) { - msg.num_segments = piecewise_polynomial.getNumberOfSegments(); - msg.num_breaks = piecewise_polynomial.getNumberOfSegments() + 1; - msg.breaks = piecewise_polynomial.getSegmentTimes(); - msg.polynomial_matrices.resize(piecewise_polynomial.getNumberOfSegments()); - for (int i = 0; i < piecewise_polynomial.getNumberOfSegments(); ++i) { - encodePolynomialMatrix( - piecewise_polynomial.getPolynomialMatrix(i), - msg.polynomial_matrices[i]); - } -} - -PiecewisePolynomial decodePiecewisePolynomial( - const drake::lcmt_piecewise_polynomial& msg) { - typedef PiecewisePolynomial::PolynomialMatrix PolynomialMatrix; - std::vector polynomial_matrices; - for (size_t i = 0; i < msg.polynomial_matrices.size(); ++i) { - polynomial_matrices.push_back( - decodePolynomialMatrix(msg.polynomial_matrices[i])); - } - return PiecewisePolynomial(polynomial_matrices, msg.breaks); -} diff --git a/drake/util/lcmUtil.h b/drake/util/lcmUtil.h index 5f2cea11c1ee..750bb2f419b7 100644 --- a/drake/util/lcmUtil.h +++ b/drake/util/lcmUtil.h @@ -3,15 +3,13 @@ #include #include +#include #include "bot_core/position_3d_t.hpp" #include "bot_core/quaternion_t.hpp" #include "bot_core/twist_t.hpp" #include "bot_core/vector_3d_t.hpp" -#include "drake/common/trajectories/piecewise_polynomial.h" -#include "drake/lcmt_piecewise_polynomial.hpp" -#include "drake/lcmt_polynomial.hpp" -#include "drake/lcmt_polynomial_matrix.hpp" +#include "drake/common/eigen_types.h" void EncodeVector3d( const Eigen::Ref& vec, bot_core::vector_3d_t& msg); @@ -38,47 +36,3 @@ void EncodeTwist( drake::TwistVector DecodeTwist( const bot_core::twist_t& msg); - -void encodePolynomial(const Polynomial& polynomial, - drake::lcmt_polynomial& msg); - -Polynomial decodePolynomial( - const drake::lcmt_polynomial& msg); - -template -void encodePolynomialMatrix( - const Eigen::Matrix, RowsAtCompileTime, - ColsAtCompileTime>& polynomial_matrix, - drake::lcmt_polynomial_matrix& msg) { - msg.polynomials.clear(); - msg.polynomials.resize(polynomial_matrix.rows()); - for (int row = 0; row < polynomial_matrix.rows(); ++row) { - auto& polynomial_msg_row = msg.polynomials[row]; - polynomial_msg_row.resize(polynomial_matrix.cols()); - for (int col = 0; col < polynomial_matrix.cols(); ++col) { - encodePolynomial(polynomial_matrix(row, col), polynomial_msg_row[col]); - } - } - msg.rows = polynomial_matrix.rows(); - msg.cols = polynomial_matrix.cols(); -} - -template -Eigen::Matrix, RowsAtCompileTime, ColsAtCompileTime> -decodePolynomialMatrix(const drake::lcmt_polynomial_matrix& msg) { - Eigen::Matrix, RowsAtCompileTime, ColsAtCompileTime> ret( - msg.rows, msg.cols); - for (int row = 0; row < msg.rows; ++row) { - for (int col = 0; col < msg.cols; ++col) { - ret(row, col) = decodePolynomial(msg.polynomials[row][col]); - } - } - return ret; -} - -void encodePiecewisePolynomial( - const PiecewisePolynomial& piecewise_polynomial, - drake::lcmt_piecewise_polynomial& msg); - -PiecewisePolynomial decodePiecewisePolynomial( - const drake::lcmt_piecewise_polynomial& msg); diff --git a/drake/util/test/testLCMUtil.cpp b/drake/util/test/testLCMUtil.cpp index 2ec78a0be5ad..569847d978bd 100644 --- a/drake/util/test/testLCMUtil.cpp +++ b/drake/util/test/testLCMUtil.cpp @@ -5,68 +5,16 @@ #include #include "drake/common/test_utilities/eigen_matrix_compare.h" -#include "drake/common/test_utilities/random_polynomial_matrix.h" -#include "drake/common/trajectories/test/random_piecewise_polynomial.h" #include "drake/math/random_rotation.h" -using Eigen::Dynamic; using Eigen::Sequential; -using Eigen::Vector2d; using Eigen::Vector3d; -using Eigen::VectorXd; namespace drake { namespace { // TODO(jwnimmer-tri) Unit tests should not use unseeded randomness. -GTEST_TEST(TestLcmUtil, testPolynomial) { - std::default_random_engine generator; - int max_num_coefficients = 5; - std::uniform_int_distribution<> int_distribution(1, max_num_coefficients); - int num_coefficients = int_distribution(generator); - VectorXd coefficients = VectorXd::Random(num_coefficients); - Polynomial poly(coefficients); - drake::lcmt_polynomial msg; - encodePolynomial(poly, msg); - auto poly_back = decodePolynomial(msg); - EXPECT_TRUE(poly.IsApprox(poly_back, 1e-8)); -} - -GTEST_TEST(TestLcmUtil, testPolynomialMatrix) { - auto poly_matrix = drake::test::RandomPolynomialMatrix(6, 5, 8); - drake::lcmt_polynomial_matrix msg; - encodePolynomialMatrix(poly_matrix, msg); - EXPECT_EQ(static_cast(msg.rows), static_cast(poly_matrix.rows())); - EXPECT_EQ(static_cast(msg.cols), static_cast(poly_matrix.cols())); - auto poly_matrix_back = decodePolynomialMatrix(msg); - EXPECT_EQ(poly_matrix.rows(), poly_matrix_back.rows()); - EXPECT_EQ(poly_matrix.cols(), poly_matrix_back.cols()); - for (int row = 0; row < msg.rows; ++row) { - for (int col = 0; col < msg.cols; ++col) { - EXPECT_TRUE( - poly_matrix(row, col).IsApprox(poly_matrix_back(row, col), 1e-8)); - } - } -} - -GTEST_TEST(TestLcmUtil, testPiecewisePolynomial) { - std::default_random_engine generator; - int num_segments = 6; - int rows = 4; - int cols = 7; - int num_coefficients = 3; - std::vector segment_times = - PiecewiseFunction::randomSegmentTimes(num_segments, generator); - PiecewisePolynomial piecewise_polynomial = - test::MakeRandomPiecewisePolynomial( - rows, cols, num_coefficients, segment_times); - drake::lcmt_piecewise_polynomial msg; - encodePiecewisePolynomial(piecewise_polynomial, msg); - auto piecewise_polynomial_back = decodePiecewisePolynomial(msg); - EXPECT_TRUE(piecewise_polynomial_back.isApprox(piecewise_polynomial, 1e-10)); -} - GTEST_TEST(TestLcmUtil, testVector3d) { const int kVectorSize = 3; const Eigen::Vector3d vec = Vector3d::LinSpaced(Sequential, 0, kVectorSize);