From a9cd13b2b72ebf6b4fc14e9224d7a23262efeabf Mon Sep 17 00:00:00 2001
From: Jeremy Nimmer <jeremy.nimmer@tri.global>
Date: Tue, 31 Oct 2017 18:47:59 -0400
Subject: [PATCH 1/5] Move yaml_util from /util to /systems/controllers
It's only used from within controllers, and shouldn't be used elsewhere.
We mark it private, since it's only used from .cc files, also.
---
drake/systems/controllers/BUILD.bazel | 14 +++++++++++++-
.../controllers/InstantaneousQPController.cpp | 2 +-
.../controllers/yaml_util.cc} | 17 ++++++++++++++++-
.../controllers/yaml_util.h} | 2 ++
drake/util/BUILD.bazel | 11 -----------
tools/install/libdrake/build_components.bzl | 1 -
6 files changed, 32 insertions(+), 15 deletions(-)
rename drake/{util/yaml/yamlUtil.cpp => systems/controllers/yaml_util.cc} (97%)
rename drake/{util/yaml/yamlUtil.h => systems/controllers/yaml_util.h} (95%)
diff --git a/drake/systems/controllers/BUILD.bazel b/drake/systems/controllers/BUILD.bazel
index e078ffe4dd50..dcebd5501b43 100644
--- a/drake/systems/controllers/BUILD.bazel
+++ b/drake/systems/controllers/BUILD.bazel
@@ -146,6 +146,18 @@ drake_cc_library(
],
)
+drake_cc_library(
+ name = "yaml_util",
+ srcs = ["yaml_util.cc"],
+ hdrs = ["yaml_util.h"],
+ visibility = [],
+ deps = [
+ ":qp_common",
+ "//drake/common:essential",
+ "@yaml_cpp",
+ ],
+)
+
# This library is empty unless Gurobi is available.
drake_cc_library(
name = "instantaneous_qp_controller",
@@ -160,6 +172,7 @@ drake_cc_library(
deps = [
":control_util",
":qp_common",
+ ":yaml_util",
"//drake/common:essential",
"//drake/common:find_resource",
"//drake/common:is_approx_equal_abstol",
@@ -171,7 +184,6 @@ drake_cc_library(
"//drake/solvers:gurobi_qp",
"//drake/util",
"//drake/util:lcm_util",
- "//drake/util:yaml_util",
],
)
diff --git a/drake/systems/controllers/InstantaneousQPController.cpp b/drake/systems/controllers/InstantaneousQPController.cpp
index 96c6fb1c94a0..d75bfb6bd7e2 100644
--- a/drake/systems/controllers/InstantaneousQPController.cpp
+++ b/drake/systems/controllers/InstantaneousQPController.cpp
@@ -20,9 +20,9 @@
#include "drake/multibody/parsers/urdf_parser.h"
#include "drake/solvers/fast_qp.h"
#include "drake/systems/controllers/controlUtil.h"
+#include "drake/systems/controllers/yaml_util.h"
#include "drake/util/drakeGeometryUtil.h"
#include "drake/util/lcmUtil.h"
-#include "drake/util/yaml/yamlUtil.h"
const double REG = 1e-8;
diff --git a/drake/util/yaml/yamlUtil.cpp b/drake/systems/controllers/yaml_util.cc
similarity index 97%
rename from drake/util/yaml/yamlUtil.cpp
rename to drake/systems/controllers/yaml_util.cc
index 8668793f375d..97b02489f071 100644
--- a/drake/util/yaml/yamlUtil.cpp
+++ b/drake/systems/controllers/yaml_util.cc
@@ -1,4 +1,9 @@
-#include "drake/util/yaml/yamlUtil.h"
+#include "drake/systems/controllers/yaml_util.h"
+
+#include <algorithm>
+#include <set>
+#include <utility>
+#include <vector>
#include "drake/multibody/joints/floating_base_types.h"
@@ -69,6 +74,7 @@ YAML::Node get(const YAML::Node& parent, const std::string& key) {
}
}
+// NOLINTNEXTLINE(runtime/references)
void loadBodyMotionParams(QPControllerParams& params, const YAML::Node& config,
const RigidBodyTree<double>& robot) {
for (auto body_it = robot.bodies.begin(); body_it != robot.bodies.end();
@@ -87,6 +93,7 @@ void loadBodyMotionParams(QPControllerParams& params, const YAML::Node& config,
}
}
+// NOLINTNEXTLINE(runtime/references)
void loadSingleJointParams(QPControllerParams& params,
Eigen::DenseIndex position_index,
const YAML::Node& config,
@@ -142,6 +149,7 @@ void loadSingleJointParams(QPControllerParams& params,
get(soft_limits_config, "k_logistic").as<double>();
}
+// NOLINTNEXTLINE(runtime/references)
void loadJointParams(QPControllerParams& params, const YAML::Node& config,
const RigidBodyTree<double>& robot) {
std::map<std::string, int> position_name_to_index =
@@ -176,6 +184,7 @@ void loadJointParams(QPControllerParams& params, const YAML::Node& config,
// }
}
+// NOLINTNEXTLINE(runtime/references)
void loadSingleInputParams(QPControllerParams& params,
Eigen::DenseIndex position_index,
YAML::Node config) {
@@ -209,6 +218,7 @@ void loadSingleInputParams(QPControllerParams& params,
}
}
+// NOLINTNEXTLINE(runtime/references)
void loadInputParams(QPControllerParams& params, const YAML::Node& config,
const RigidBodyTree<double>& robot) {
for (auto actuator_it = robot.actuators.begin();
@@ -244,6 +254,7 @@ void loadInputParams(QPControllerParams& params, const YAML::Node& config,
namespace YAML {
template <>
struct convert<VRefIntegratorParams> {
+ // NOLINTNEXTLINE(runtime/references)
static bool decode(const Node& node, VRefIntegratorParams& rhs) {
if (!node.IsMap()) {
return false;
@@ -257,6 +268,7 @@ struct convert<VRefIntegratorParams> {
template <>
struct convert<BodyMotionParams> {
+ // NOLINTNEXTLINE(runtime/references)
static bool decode(const Node& node, BodyMotionParams& params) {
if (!node.IsMap()) {
return false;
@@ -425,6 +437,7 @@ JointNames parseRobotJointNames(const YAML::Node& joint_names,
namespace YAML {
template <>
struct convert<FloatingBaseType> {
+ // NOLINTNEXTLINE(runtime/references)
static bool decode(const Node& node, FloatingBaseType& rhs) {
std::string joint_type = node.as<std::string>();
if (joint_type == "kFixed") {
@@ -442,6 +455,7 @@ struct convert<FloatingBaseType> {
template <>
struct convert<Attachment> {
+ // NOLINTNEXTLINE(runtime/references)
static bool decode(const Node& node, Attachment& rhs) {
if (!node.IsMap()) {
return false;
@@ -465,6 +479,7 @@ struct convert<Attachment> {
template <>
struct convert<KinematicModifications> {
+ // NOLINTNEXTLINE(runtime/references)
static bool decode(const Node& node, KinematicModifications& rhs) {
if (!node.IsMap()) {
return false;
diff --git a/drake/util/yaml/yamlUtil.h b/drake/systems/controllers/yaml_util.h
similarity index 95%
rename from drake/util/yaml/yamlUtil.h
rename to drake/systems/controllers/yaml_util.h
index 6b6eb6d054e6..048f2322bcbe 100644
--- a/drake/util/yaml/yamlUtil.h
+++ b/drake/systems/controllers/yaml_util.h
@@ -2,6 +2,7 @@
#include <fstream>
#include <map>
+#include <string>
#include "yaml-cpp/yaml.h"
@@ -25,6 +26,7 @@ drake::eigen_aligned_std_map<std::string, QPControllerParams> loadAllParamSets(
YAML::Node config, const RigidBodyTree<double>& robot);
drake::eigen_aligned_std_map<std::string, QPControllerParams> loadAllParamSets(
YAML::Node config, const RigidBodyTree<double>& robot,
+ // NOLINTNEXTLINE(runtime/references)
std::ofstream& debug_output_file);
RobotPropertyCache parseKinematicTreeMetadata(
const YAML::Node& metadata, const RigidBodyTree<double>& robot);
diff --git a/drake/util/BUILD.bazel b/drake/util/BUILD.bazel
index 952fdc48cfb5..2d7d789ae310 100644
--- a/drake/util/BUILD.bazel
+++ b/drake/util/BUILD.bazel
@@ -47,17 +47,6 @@ drake_cc_library(
],
)
-drake_cc_library(
- name = "yaml_util",
- srcs = ["yaml/yamlUtil.cpp"],
- hdrs = ["yaml/yamlUtil.h"],
- deps = [
- "//drake/common:essential",
- "//drake/systems/controllers:qp_common",
- "@yaml_cpp",
- ],
-)
-
# === test/ ===
drake_cc_googletest(
diff --git a/tools/install/libdrake/build_components.bzl b/tools/install/libdrake/build_components.bzl
index 2b94e9eace1c..90d97e8c941f 100644
--- a/tools/install/libdrake/build_components.bzl
+++ b/tools/install/libdrake/build_components.bzl
@@ -328,5 +328,4 @@ LIBDRAKE_COMPONENTS = [
"//drake/util:convex_hull",
"//drake/util:lcm_util",
"//drake/util:util",
- "//drake/util:yaml_util",
]
From c40f4a04d211934d553e99f0e5a164f4b0f806c5 Mon Sep 17 00:00:00 2001
From: Jeremy Nimmer <jeremy.nimmer@tri.global>
Date: Tue, 31 Oct 2017 18:47:59 -0400
Subject: [PATCH 2/5] Move convex_hull from /util to /systems/robotInterfaces
It's only used from within controllers, and shouldn't be used elsewhere.
We mark it private, since it's only used from .cc files, also.
---
drake/systems/robotInterfaces/BUILD.bazel | 27 +++++++++++++++++--
.../robotInterfaces/QPLocomotionPlan.cpp | 2 +-
.../robotInterfaces/convex_hull.cc} | 3 ++-
.../robotInterfaces/convex_hull.h} | 0
.../robotInterfaces/test/convex_hull_test.cc} | 4 +--
drake/util/BUILD.bazel | 17 ------------
tools/install/libdrake/build_components.bzl | 1 -
7 files changed, 29 insertions(+), 25 deletions(-)
rename drake/{util/convexHull.cpp => systems/robotInterfaces/convex_hull.cc} (97%)
rename drake/{util/convexHull.h => systems/robotInterfaces/convex_hull.h} (100%)
rename drake/{util/test/testConvexHull.cpp => systems/robotInterfaces/test/convex_hull_test.cc} (96%)
diff --git a/drake/systems/robotInterfaces/BUILD.bazel b/drake/systems/robotInterfaces/BUILD.bazel
index b01945268753..376af05d4733 100644
--- a/drake/systems/robotInterfaces/BUILD.bazel
+++ b/drake/systems/robotInterfaces/BUILD.bazel
@@ -1,6 +1,10 @@
# -*- python -*-
-load("//tools:drake.bzl", "drake_cc_library")
+load(
+ "//tools/skylark:drake_cc.bzl",
+ "drake_cc_googletest",
+ "drake_cc_library",
+)
load("//tools/lint:lint.bzl", "add_lint_tests")
package(default_visibility = ["//visibility:public"])
@@ -20,6 +24,7 @@ drake_cc_library(
hdrs = ["QPLocomotionPlan.h"],
deps = [
":body_motion_data",
+ ":convex_hull",
":side",
"//drake/common:essential",
"//drake/common:unused",
@@ -34,7 +39,6 @@ drake_cc_library(
"//drake/multibody:rigid_body_tree",
"//drake/systems/controllers:zmp_util",
"//drake/util",
- "//drake/util:convex_hull",
"//drake/util:lcm_util",
],
)
@@ -46,4 +50,23 @@ drake_cc_library(
deps = [],
)
+drake_cc_library(
+ name = "convex_hull",
+ srcs = ["convex_hull.cc"],
+ hdrs = ["convex_hull.h"],
+ visibility = [],
+ deps = [
+ "//drake/common:essential",
+ ],
+)
+
+# === test/ ===
+
+drake_cc_googletest(
+ name = "convex_hull_test",
+ deps = [
+ ":convex_hull",
+ ],
+)
+
add_lint_tests()
diff --git a/drake/systems/robotInterfaces/QPLocomotionPlan.cpp b/drake/systems/robotInterfaces/QPLocomotionPlan.cpp
index d00465126538..3a1d8920444d 100644
--- a/drake/systems/robotInterfaces/QPLocomotionPlan.cpp
+++ b/drake/systems/robotInterfaces/QPLocomotionPlan.cpp
@@ -17,7 +17,7 @@
#include "drake/math/gradient.h"
#include "drake/math/quaternion.h"
#include "drake/math/rotation_matrix.h"
-#include "drake/util/convexHull.h"
+#include "drake/systems/robotInterfaces/convex_hull.h"
#include "drake/util/drakeGeometryUtil.h"
#include "drake/util/drakeUtil.h"
#include "drake/util/lcmUtil.h"
diff --git a/drake/util/convexHull.cpp b/drake/systems/robotInterfaces/convex_hull.cc
similarity index 97%
rename from drake/util/convexHull.cpp
rename to drake/systems/robotInterfaces/convex_hull.cc
index c0baa149c2e6..3c4e66ce41fd 100644
--- a/drake/util/convexHull.cpp
+++ b/drake/systems/robotInterfaces/convex_hull.cc
@@ -4,10 +4,11 @@
// Adapted (okay, stolen) from
// http://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain
-#include "drake/util/convexHull.h"
+#include "drake/systems/robotInterfaces/convex_hull.h"
#include <algorithm>
#include <iostream>
+#include <limits>
#include <vector>
using Eigen::Dynamic;
diff --git a/drake/util/convexHull.h b/drake/systems/robotInterfaces/convex_hull.h
similarity index 100%
rename from drake/util/convexHull.h
rename to drake/systems/robotInterfaces/convex_hull.h
diff --git a/drake/util/test/testConvexHull.cpp b/drake/systems/robotInterfaces/test/convex_hull_test.cc
similarity index 96%
rename from drake/util/test/testConvexHull.cpp
rename to drake/systems/robotInterfaces/test/convex_hull_test.cc
index a1929b31eb73..517ada0e517c 100644
--- a/drake/util/test/testConvexHull.cpp
+++ b/drake/systems/robotInterfaces/test/convex_hull_test.cc
@@ -1,6 +1,4 @@
-/* clang-format off to disable clang-format-includes */
-#include "drake/util/convexHull.h"
-/* clang-format on */
+#include "drake/systems/robotInterfaces/convex_hull.h"
#include <iostream>
diff --git a/drake/util/BUILD.bazel b/drake/util/BUILD.bazel
index 2d7d789ae310..627309242682 100644
--- a/drake/util/BUILD.bazel
+++ b/drake/util/BUILD.bazel
@@ -21,15 +21,6 @@ drake_cc_library(
],
)
-drake_cc_library(
- name = "convex_hull",
- srcs = ["convexHull.cpp"],
- hdrs = ["convexHull.h"],
- deps = [
- "//drake/common:essential",
- ],
-)
-
drake_cc_library(
name = "lcm_util",
srcs = ["lcmUtil.cpp"],
@@ -49,14 +40,6 @@ drake_cc_library(
# === test/ ===
-drake_cc_googletest(
- name = "testConvexHull",
- srcs = ["test/testConvexHull.cpp"],
- deps = [
- ":convex_hull",
- ],
-)
-
drake_cc_googletest(
name = "testDrakeGeometryUtil",
srcs = ["test/testDrakeGeometryUtil.cpp"],
diff --git a/tools/install/libdrake/build_components.bzl b/tools/install/libdrake/build_components.bzl
index 90d97e8c941f..7c49e7566b59 100644
--- a/tools/install/libdrake/build_components.bzl
+++ b/tools/install/libdrake/build_components.bzl
@@ -325,7 +325,6 @@ LIBDRAKE_COMPONENTS = [
"//drake/systems/trajectory_optimization:direct_collocation",
"//drake/systems/trajectory_optimization:direct_transcription",
"//drake/systems/trajectory_optimization:multiple_shooting",
- "//drake/util:convex_hull",
"//drake/util:lcm_util",
"//drake/util:util",
]
From 0257bc25f3066eb4b016d234d1ee09fa818cda66 Mon Sep 17 00:00:00 2001
From: Jeremy Nimmer <jeremy.nimmer@tri.global>
Date: Tue, 31 Oct 2017 18:47:59 -0400
Subject: [PATCH 3/5] Move addOffset to the one place its used
---
drake/systems/robotInterfaces/QPLocomotionPlan.cpp | 10 ++++++++++
drake/util/drakeUtil.h | 6 ------
2 files changed, 10 insertions(+), 6 deletions(-)
diff --git a/drake/systems/robotInterfaces/QPLocomotionPlan.cpp b/drake/systems/robotInterfaces/QPLocomotionPlan.cpp
index 3a1d8920444d..00214dcbea73 100644
--- a/drake/systems/robotInterfaces/QPLocomotionPlan.cpp
+++ b/drake/systems/robotInterfaces/QPLocomotionPlan.cpp
@@ -2,6 +2,7 @@
#include <algorithm>
#include <cmath>
+#include <functional>
#include <limits>
#include <memory>
#include <stdexcept>
@@ -63,6 +64,15 @@ const std::map<SupportLogicType, std::vector<bool>>
QPLocomotionPlan::support_logic_maps_ =
QPLocomotionPlan::createSupportLogicMaps();
+namespace {
+template <typename T>
+// NOLINTNEXTLINE(runtime/references)
+void addOffset(std::vector<T>& v, const T& offset) {
+ std::transform(v.begin(), v.end(), v.begin(),
+ std::bind2nd(std::plus<double>(), offset));
+}
+} // namespace
+
std::string primaryBodyOrFrameName(const std::string& full_body_name) {
size_t i = 0;
for (; i < full_body_name.size(); i++) {
diff --git a/drake/util/drakeUtil.h b/drake/util/drakeUtil.h
index 82875b74d2ce..ce0e80f422a2 100644
--- a/drake/util/drakeUtil.h
+++ b/drake/util/drakeUtil.h
@@ -171,9 +171,3 @@ void stdVectorOfStdVectorsToEigen(
}
}
}
-
-template <typename T>
-void addOffset(std::vector<T>& v, const T& offset) {
- std::transform(v.begin(), v.end(), v.begin(),
- std::bind2nd(std::plus<double>(), offset));
-}
From 2e4dcb610d6a1eefd4cb47a29f5d6e07a7694267 Mon Sep 17 00:00:00 2001
From: Jeremy Nimmer <jeremy.nimmer@tri.global>
Date: Tue, 31 Oct 2017 18:47:59 -0400
Subject: [PATCH 4/5] Move verify_subtype_sizes into its own file
---
drake/systems/robotInterfaces/BUILD.bazel | 10 +++-
.../robotInterfaces/QPLocomotionPlan.cpp | 1 +
.../robotInterfaces/verify_subtype_sizes.cc | 54 +++++++++++++++++++
.../robotInterfaces/verify_subtype_sizes.h | 10 ++++
drake/util/BUILD.bazel | 1 -
drake/util/lcmUtil.cpp | 47 ----------------
drake/util/lcmUtil.h | 6 ---
7 files changed, 73 insertions(+), 56 deletions(-)
create mode 100644 drake/systems/robotInterfaces/verify_subtype_sizes.cc
create mode 100644 drake/systems/robotInterfaces/verify_subtype_sizes.h
diff --git a/drake/systems/robotInterfaces/BUILD.bazel b/drake/systems/robotInterfaces/BUILD.bazel
index 376af05d4733..2bdd91fa2101 100644
--- a/drake/systems/robotInterfaces/BUILD.bazel
+++ b/drake/systems/robotInterfaces/BUILD.bazel
@@ -20,8 +20,14 @@ drake_cc_library(
drake_cc_library(
name = "qp_locomotion_plan",
- srcs = ["QPLocomotionPlan.cpp"],
- hdrs = ["QPLocomotionPlan.h"],
+ srcs = [
+ "QPLocomotionPlan.cpp",
+ "verify_subtype_sizes.cc",
+ "verify_subtype_sizes.h",
+ ],
+ hdrs = [
+ "QPLocomotionPlan.h",
+ ],
deps = [
":body_motion_data",
":convex_hull",
diff --git a/drake/systems/robotInterfaces/QPLocomotionPlan.cpp b/drake/systems/robotInterfaces/QPLocomotionPlan.cpp
index 00214dcbea73..55930bc063f1 100644
--- a/drake/systems/robotInterfaces/QPLocomotionPlan.cpp
+++ b/drake/systems/robotInterfaces/QPLocomotionPlan.cpp
@@ -19,6 +19,7 @@
#include "drake/math/quaternion.h"
#include "drake/math/rotation_matrix.h"
#include "drake/systems/robotInterfaces/convex_hull.h"
+#include "drake/systems/robotInterfaces/verify_subtype_sizes.h"
#include "drake/util/drakeGeometryUtil.h"
#include "drake/util/drakeUtil.h"
#include "drake/util/lcmUtil.h"
diff --git a/drake/systems/robotInterfaces/verify_subtype_sizes.cc b/drake/systems/robotInterfaces/verify_subtype_sizes.cc
new file mode 100644
index 000000000000..a28d68ac1d4e
--- /dev/null
+++ b/drake/systems/robotInterfaces/verify_subtype_sizes.cc
@@ -0,0 +1,54 @@
+#include "drake/systems/robotInterfaces/verify_subtype_sizes.h"
+
+#include <iostream>
+#include <sstream>
+#include <stdexcept>
+
+// NOLINTNEXTLINE(runtime/references)
+void verifySubtypeSizes(drake::lcmt_support_data& support_data) {
+ // Check for errors in sizes of variable-length fields.
+ if (support_data.contact_pts.size() != 3) {
+ throw std::runtime_error("contact_pts must have 3 rows");
+ }
+ for (int j = 0; j < 3; ++j) {
+ if (static_cast<int32_t>(support_data.contact_pts[j].size()) !=
+ support_data.num_contact_pts) {
+ std::stringstream msg;
+ msg << "num_contact_pts must match the size of each row of contact_pts."
+ << std::endl;
+ msg << "num_contact_pts: " << support_data.num_contact_pts
+ << ", support_data.contact_pts[" << j
+ << "].size(): " << support_data.contact_pts[j].size() << std::endl;
+ throw std::runtime_error(msg.str().c_str());
+ }
+ }
+}
+
+// NOLINTNEXTLINE(runtime/references)
+void verifySubtypeSizes(drake::lcmt_qp_controller_input& qp_input) {
+ // Check (and try to fix) errors in the sizes of the variable-length fields in
+ // our message
+ if (static_cast<int32_t>(qp_input.support_data.size()) !=
+ qp_input.num_support_data) {
+ std::cerr << "WARNING: num support data doesn't match" << std::endl;
+ qp_input.num_support_data = qp_input.support_data.size();
+ }
+ if (static_cast<int32_t>(qp_input.body_motion_data.size()) !=
+ qp_input.num_tracked_bodies) {
+ std::cerr << "WARNING: num tracked bodies doesn't match" << std::endl;
+ qp_input.num_tracked_bodies = qp_input.body_motion_data.size();
+ }
+ if (static_cast<int32_t>(qp_input.body_wrench_data.size()) !=
+ qp_input.num_external_wrenches) {
+ std::cerr << "WARNING: num external wrenches doesn't match" << std::endl;
+ qp_input.num_external_wrenches = qp_input.body_wrench_data.size();
+ }
+ if (static_cast<int32_t>(qp_input.joint_pd_override.size()) !=
+ qp_input.num_joint_pd_overrides) {
+ std::cerr << "WARNING: num joint pd override doesn't match" << std::endl;
+ qp_input.num_joint_pd_overrides = qp_input.joint_pd_override.size();
+ }
+ for (int i = 0; i < qp_input.num_support_data; ++i) {
+ verifySubtypeSizes(qp_input.support_data[i]);
+ }
+}
diff --git a/drake/systems/robotInterfaces/verify_subtype_sizes.h b/drake/systems/robotInterfaces/verify_subtype_sizes.h
new file mode 100644
index 000000000000..60e529c90446
--- /dev/null
+++ b/drake/systems/robotInterfaces/verify_subtype_sizes.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#include "drake/lcmt_qp_controller_input.hpp"
+
+void verifySubtypeSizes(
+ // NOLINTNEXTLINE(runtime/references)
+ drake::lcmt_support_data& support_data);
+void verifySubtypeSizes(
+ // NOLINTNEXTLINE(runtime/references)
+ drake::lcmt_qp_controller_input& qp_input);
diff --git a/drake/util/BUILD.bazel b/drake/util/BUILD.bazel
index 627309242682..3edf51fdc988 100644
--- a/drake/util/BUILD.bazel
+++ b/drake/util/BUILD.bazel
@@ -31,7 +31,6 @@ drake_cc_library(
"//drake/lcmtypes:piecewise_polynomial",
"//drake/lcmtypes:polynomial",
"//drake/lcmtypes:polynomial_matrix",
- "//drake/lcmtypes:qp_controller_input",
"//drake/math:geometric_transform",
"@eigen",
"@lcmtypes_bot2_core",
diff --git a/drake/util/lcmUtil.cpp b/drake/util/lcmUtil.cpp
index 6d98fa2027a0..262671e996ac 100644
--- a/drake/util/lcmUtil.cpp
+++ b/drake/util/lcmUtil.cpp
@@ -96,50 +96,3 @@ PiecewisePolynomial<double> decodePiecewisePolynomial(
}
return PiecewisePolynomial<double>(polynomial_matrices, msg.breaks);
}
-
-void verifySubtypeSizes(drake::lcmt_support_data& support_data) {
- // Check for errors in sizes of variable-length fields.
- if (support_data.contact_pts.size() != 3) {
- throw std::runtime_error("contact_pts must have 3 rows");
- }
- for (int j = 0; j < 3; ++j) {
- if (static_cast<int32_t>(support_data.contact_pts[j].size()) !=
- support_data.num_contact_pts) {
- std::stringstream msg;
- msg << "num_contact_pts must match the size of each row of contact_pts."
- << std::endl;
- msg << "num_contact_pts: " << support_data.num_contact_pts
- << ", support_data.contact_pts[" << j
- << "].size(): " << support_data.contact_pts[j].size() << std::endl;
- throw std::runtime_error(msg.str().c_str());
- }
- }
-}
-
-void verifySubtypeSizes(drake::lcmt_qp_controller_input& qp_input) {
- // Check (and try to fix) errors in the sizes of the variable-length fields in
- // our message
- if (static_cast<int32_t>(qp_input.support_data.size()) !=
- qp_input.num_support_data) {
- std::cerr << "WARNING: num support data doesn't match" << std::endl;
- qp_input.num_support_data = qp_input.support_data.size();
- }
- if (static_cast<int32_t>(qp_input.body_motion_data.size()) !=
- qp_input.num_tracked_bodies) {
- std::cerr << "WARNING: num tracked bodies doesn't match" << std::endl;
- qp_input.num_tracked_bodies = qp_input.body_motion_data.size();
- }
- if (static_cast<int32_t>(qp_input.body_wrench_data.size()) !=
- qp_input.num_external_wrenches) {
- std::cerr << "WARNING: num external wrenches doesn't match" << std::endl;
- qp_input.num_external_wrenches = qp_input.body_wrench_data.size();
- }
- if (static_cast<int32_t>(qp_input.joint_pd_override.size()) !=
- qp_input.num_joint_pd_overrides) {
- std::cerr << "WARNING: num joint pd override doesn't match" << std::endl;
- qp_input.num_joint_pd_overrides = qp_input.joint_pd_override.size();
- }
- for (int i = 0; i < qp_input.num_support_data; ++i) {
- verifySubtypeSizes(qp_input.support_data[i]);
- }
-}
diff --git a/drake/util/lcmUtil.h b/drake/util/lcmUtil.h
index dbd912856431..5f2cea11c1ee 100644
--- a/drake/util/lcmUtil.h
+++ b/drake/util/lcmUtil.h
@@ -12,7 +12,6 @@
#include "drake/lcmt_piecewise_polynomial.hpp"
#include "drake/lcmt_polynomial.hpp"
#include "drake/lcmt_polynomial_matrix.hpp"
-#include "drake/lcmt_qp_controller_input.hpp"
void EncodeVector3d(
const Eigen::Ref<const Eigen::Vector3d>& vec, bot_core::vector_3d_t& msg);
@@ -83,8 +82,3 @@ void encodePiecewisePolynomial(
PiecewisePolynomial<double> decodePiecewisePolynomial(
const drake::lcmt_piecewise_polynomial& msg);
-
-void verifySubtypeSizes(
- drake::lcmt_support_data& support_data);
-void verifySubtypeSizes(
- drake::lcmt_qp_controller_input& qp_input);
From 0db1225d0ccb8a934b6d0e6feb97d4c0854e9ef1 Mon Sep 17 00:00:00 2001
From: Jeremy Nimmer <jeremy.nimmer@tri.global>
Date: Tue, 31 Oct 2017 18:47:59 -0400
Subject: [PATCH 5/5] Move polynomial_encode_decode into its own file
---
drake/systems/controllers/BUILD.bazel | 25 ++++++-
.../controllers/InstantaneousQPController.cpp | 1 +
.../controllers/polynomial_encode_decode.cc | 48 ++++++++++++++
.../controllers/polynomial_encode_decode.h | 55 ++++++++++++++++
.../test/polynomial_encode_decode_test.cc | 66 +++++++++++++++++++
drake/systems/robotInterfaces/BUILD.bazel | 1 +
.../robotInterfaces/QPLocomotionPlan.cpp | 1 +
drake/util/BUILD.bazel | 4 --
drake/util/lcmUtil.cpp | 37 -----------
drake/util/lcmUtil.h | 50 +-------------
drake/util/test/testLCMUtil.cpp | 52 ---------------
11 files changed, 198 insertions(+), 142 deletions(-)
create mode 100644 drake/systems/controllers/polynomial_encode_decode.cc
create mode 100644 drake/systems/controllers/polynomial_encode_decode.h
create mode 100644 drake/systems/controllers/test/polynomial_encode_decode_test.cc
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 <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);
+}
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 <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);
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 <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
+
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<double> DecodeTwist(const bot_core::twist_t& msg) {
ret.tail<3>() = DecodeVector3d(msg.linear_velocity);
return ret;
}
-
-void encodePolynomial(const Polynomial<double>& polynomial,
- 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,
- 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);
-}
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 <iostream>
#include <Eigen/Core>
+#include <Eigen/Geometry>
#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<const Eigen::Vector3d>& vec, bot_core::vector_3d_t& msg);
@@ -38,47 +36,3 @@ void EncodeTwist(
drake::TwistVector<double> DecodeTwist(
const bot_core::twist_t& msg);
-
-void encodePolynomial(const Polynomial<double>& polynomial,
- 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,
- 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,
- drake::lcmt_piecewise_polynomial& msg);
-
-PiecewisePolynomial<double> 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 <gtest/gtest.h>
#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<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));
-}
-
GTEST_TEST(TestLcmUtil, testVector3d) {
const int kVectorSize = 3;
const Eigen::Vector3d vec = Vector3d::LinSpaced(Sequential, 0, kVectorSize);