-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #7363 from jwnimmer-tri/util-robotInterfaces
Move non-general utilities into robotInterfaces QP controllers
- Loading branch information
Showing
20 changed files
with
342 additions
and
244 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,48 @@ | ||
| #include "drake/systems/controllers/polynomial_encode_decode.h" | ||
|
|
||
| #include <vector> | ||
|
|
||
| #include "drake/util/drakeUtil.h" | ||
|
|
||
| using Eigen::Dynamic; | ||
| using Eigen::Map; | ||
| using Eigen::VectorXd; | ||
|
|
||
| void encodePolynomial(const Polynomial<double>& polynomial, | ||
| // NOLINTNEXTLINE(runtime/references) | ||
| drake::lcmt_polynomial& msg) { | ||
| eigenVectorToStdVector(polynomial.GetCoefficients(), msg.coefficients); | ||
| msg.num_coefficients = polynomial.GetNumberOfCoefficients(); | ||
| } | ||
|
|
||
| Polynomial<double> decodePolynomial(const drake::lcmt_polynomial& msg) { | ||
| Map<const VectorXd> coefficients(msg.coefficients.data(), | ||
| msg.coefficients.size()); | ||
| return Polynomial<double>(coefficients); | ||
| } | ||
|
|
||
| void encodePiecewisePolynomial( | ||
| const PiecewisePolynomial<double>& 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<Eigen::Dynamic, Eigen::Dynamic>( | ||
| piecewise_polynomial.getPolynomialMatrix(i), | ||
| msg.polynomial_matrices[i]); | ||
| } | ||
| } | ||
|
|
||
| PiecewisePolynomial<double> decodePiecewisePolynomial( | ||
| const drake::lcmt_piecewise_polynomial& msg) { | ||
| typedef PiecewisePolynomial<double>::PolynomialMatrix PolynomialMatrix; | ||
| std::vector<PolynomialMatrix> polynomial_matrices; | ||
| for (size_t i = 0; i < msg.polynomial_matrices.size(); ++i) { | ||
| polynomial_matrices.push_back( | ||
| decodePolynomialMatrix<Dynamic, Dynamic>(msg.polynomial_matrices[i])); | ||
| } | ||
| return PiecewisePolynomial<double>(polynomial_matrices, msg.breaks); | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,55 @@ | ||
| #pragma once | ||
|
|
||
| #include <Eigen/Core> | ||
|
|
||
| #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<double>& polynomial, | ||
| // NOLINTNEXTLINE(runtime/references) | ||
| drake::lcmt_polynomial& msg); | ||
|
|
||
| Polynomial<double> decodePolynomial( | ||
| const drake::lcmt_polynomial& msg); | ||
|
|
||
| template <int RowsAtCompileTime, int ColsAtCompileTime> | ||
| void encodePolynomialMatrix( | ||
| const Eigen::Matrix<Polynomial<double>, 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 <int RowsAtCompileTime, int ColsAtCompileTime> | ||
| Eigen::Matrix<Polynomial<double>, RowsAtCompileTime, ColsAtCompileTime> | ||
| decodePolynomialMatrix(const drake::lcmt_polynomial_matrix& msg) { | ||
| Eigen::Matrix<Polynomial<double>, 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<double>& piecewise_polynomial, | ||
| // NOLINTNEXTLINE(runtime/references) | ||
| drake::lcmt_piecewise_polynomial& msg); | ||
|
|
||
| PiecewisePolynomial<double> decodePiecewisePolynomial( | ||
| const drake::lcmt_piecewise_polynomial& msg); |
66 changes: 66 additions & 0 deletions
66
drake/systems/controllers/test/polynomial_encode_decode_test.cc
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,66 @@ | ||
| #include "drake/systems/controllers/polynomial_encode_decode.h" | ||
|
|
||
| #include <gtest/gtest.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::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<double> 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<double>(6, 5, 8); | ||
| drake::lcmt_polynomial_matrix msg; | ||
| encodePolynomialMatrix<Eigen::Dynamic, Eigen::Dynamic>(poly_matrix, msg); | ||
| EXPECT_EQ(static_cast<int>(msg.rows), static_cast<int>(poly_matrix.rows())); | ||
| EXPECT_EQ(static_cast<int>(msg.cols), static_cast<int>(poly_matrix.cols())); | ||
| auto poly_matrix_back = decodePolynomialMatrix<Dynamic, Dynamic>(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<double> segment_times = | ||
| PiecewiseFunction::randomSegmentTimes(num_segments, generator); | ||
| PiecewisePolynomial<double> piecewise_polynomial = | ||
| test::MakeRandomPiecewisePolynomial<double>( | ||
| 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 | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.