From feab1036f13601f064055dbd75b42095938fd630 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Mon, 7 Jan 2019 10:57:27 -0800 Subject: [PATCH] Update geometry utilities (#10344) * Update geometry utilities 1. Unify "detail" and "internal" namespaces to just "internal". - affects invocations of CanonicalizeStringName(). 2. Move the Isometry3 conversion methods into the common utilities. --- geometry/BUILD.bazel | 6 ++++- geometry/dev/geometry_state.cc | 6 +++-- geometry/geometry_instance.cc | 2 +- geometry/geometry_state.cc | 4 ++-- geometry/proximity_engine.cc | 20 ++-------------- geometry/test/utilities_test.cc | 21 +++++++++++++++-- geometry/utilities.cc | 4 ++-- geometry/utilities.h | 41 ++++++++++++++++++++++++++------- 8 files changed, 68 insertions(+), 36 deletions(-) diff --git a/geometry/BUILD.bazel b/geometry/BUILD.bazel index 28ac73c11acf..b59a7d6bb80e 100644 --- a/geometry/BUILD.bazel +++ b/geometry/BUILD.bazel @@ -47,6 +47,7 @@ drake_cc_library( ":geometry_ids", ":geometry_index", ":shape_specification", + ":utilities", "//common", "//common:default_scalars", "//geometry/query_results:penetration_as_point_pair", @@ -375,7 +376,10 @@ drake_cc_googletest( drake_cc_googletest( name = "utilities_test", - deps = ["utilities"], + deps = [ + "utilities", + "//common/test_utilities", + ], ) add_lint_tests() diff --git a/geometry/dev/geometry_state.cc b/geometry/dev/geometry_state.cc index ebb1261d2381..13fec7dae742 100644 --- a/geometry/dev/geometry_state.cc +++ b/geometry/dev/geometry_state.cc @@ -426,7 +426,8 @@ const std::string& GeometryState::get_name(GeometryId geometry_id) const { template GeometryId GeometryState::GetGeometryFromName( FrameId frame_id, Role role, const std::string& name) const { - const std::string canonical_name = detail::CanonicalizeStringName(name); + const std::string canonical_name = + geometry::internal::CanonicalizeStringName(name); GeometryId result; int count = 0; @@ -694,7 +695,8 @@ bool GeometryState::IsValidGeometryName( FindOrThrow(frame_id, frames_, [frame_id]() { return "Given frame id is not valid: " + to_string(frame_id); }); - const std::string name = detail::CanonicalizeStringName(candidate_name); + const std::string name = + geometry::internal::CanonicalizeStringName(candidate_name); if (name.empty()) return false; return NameIsUnique(frame_id, role, name); } diff --git a/geometry/geometry_instance.cc b/geometry/geometry_instance.cc index 45f6162ff0c0..3a156d84ec81 100644 --- a/geometry/geometry_instance.cc +++ b/geometry/geometry_instance.cc @@ -11,7 +11,7 @@ GeometryInstance::GeometryInstance(const Isometry3& X_PG, : id_(GeometryId::get_new_id()), X_PG_(X_PG), shape_(std::move(shape)), - name_(detail::CanonicalizeStringName(name)) { + name_(internal::CanonicalizeStringName(name)) { if (name_.empty()) { throw std::logic_error("GeometryInstance given the name '" + name + "' which is an empty canonical string"); diff --git a/geometry/geometry_state.cc b/geometry/geometry_state.cc index 77147ec2ee05..526b256eb728 100644 --- a/geometry/geometry_state.cc +++ b/geometry/geometry_state.cc @@ -231,7 +231,7 @@ int GeometryState::NumGeometriesWithRole(FrameId frame_id, Role role) const { template GeometryId GeometryState::GetGeometryFromName( FrameId frame_id, Role role, const std::string& name) const { - const std::string canonical_name = detail::CanonicalizeStringName(name); + const std::string canonical_name = internal::CanonicalizeStringName(name); GeometryId result; int count = 0; @@ -588,7 +588,7 @@ bool GeometryState::IsValidGeometryName( FindOrThrow(frame_id, frames_, [frame_id]() { return "Given frame id is not valid: " + to_string(frame_id); }); - const std::string name = detail::CanonicalizeStringName(candidate_name); + const std::string name = internal::CanonicalizeStringName(candidate_name); if (name.empty()) return false; return NameIsUnique(frame_id, role, name); } diff --git a/geometry/proximity_engine.cc b/geometry/proximity_engine.cc index 39189c09dfa3..c8dde9b6a257 100644 --- a/geometry/proximity_engine.cc +++ b/geometry/proximity_engine.cc @@ -21,6 +21,7 @@ #include "drake/common/default_scalars.h" #include "drake/common/drake_variant.h" #include "drake/common/sorted_vectors_have_intersection.h" +#include "drake/geometry/utilities.h" namespace drake { namespace geometry { @@ -35,24 +36,7 @@ using std::unique_ptr; namespace { -// TODO(SeanCurtis-TRI): Swap all Isometry3 for Transforms. - -// ADL-reliant helper functions for converting Isometry to Isometry. -const Isometry3& convert(const Isometry3& transform) { - return transform; -} - -template -Isometry3 convert( - const Isometry3>& transform) { - Isometry3 result; - for (int r = 0; r < 4; ++r) { - for (int c = 0; c < 4; ++c) { - result.matrix()(r, c) = ExtractDoubleOrThrow(transform.matrix()(r, c)); - } - } - return result; -} +// TODO(SeanCurtis-TRI): Swap all Isometry3 for RigidTransforms. // Utilities/functions for working with the encoding of collision object index // and mobility type in the fcl::CollisionObject user data field. The encoded diff --git a/geometry/test/utilities_test.cc b/geometry/test/utilities_test.cc index 1bf5a115af93..eadd4f96f907 100644 --- a/geometry/test/utilities_test.cc +++ b/geometry/test/utilities_test.cc @@ -4,9 +4,11 @@ #include +#include "drake/common/test_utilities/eigen_matrix_compare.h" + namespace drake { namespace geometry { -namespace detail { +namespace internal { namespace { GTEST_TEST(GeometryUtilities, CanonicalizeGeometryName) { @@ -52,7 +54,22 @@ GTEST_TEST(GeometryUtilities, CanonicalizeGeometryName) { } } +GTEST_TEST(GeometryUtilities, IsometryConversion) { + Isometry3 X_AB = Isometry3::Identity(); + X_AB.translation() << 1, 2, 3; + // NOTE: Not a valid transform; we're just looking for unique values. + X_AB.linear() << 10, 20, 30, 40, 50, 60, 70, 80, 90; + X_AB.makeAffine(); + + Isometry3 X_AB_converted = convert(X_AB); + EXPECT_TRUE(CompareMatrices(X_AB.matrix(), X_AB_converted.matrix())); + + Isometry3 X_AB_ad(X_AB); + Isometry3 X_AB_ad_converted = convert(X_AB_ad); + EXPECT_TRUE(CompareMatrices(X_AB.matrix(), X_AB_ad_converted.matrix())); +} + } // namespace -} // namespace detail +} // namespace internal } // namespace geometry } // namespace drake diff --git a/geometry/utilities.cc b/geometry/utilities.cc index aa1c7ff00282..316620444edd 100644 --- a/geometry/utilities.cc +++ b/geometry/utilities.cc @@ -6,7 +6,7 @@ namespace drake { namespace geometry { -namespace detail { +namespace internal { std::string CanonicalizeStringName(const std::string& name) { // The definition of "canonical" is based on SDF and the functionality in @@ -29,6 +29,6 @@ std::string CanonicalizeStringName(const std::string& name) { return matches[1].str(); } -} // namespace detail +} // namespace internal } // namespace geometry } // namespace drake diff --git a/geometry/utilities.h b/geometry/utilities.h index 35851bcf0417..98b55d888bd1 100644 --- a/geometry/utilities.h +++ b/geometry/utilities.h @@ -3,13 +3,14 @@ #include #include +#include "drake/common/autodiff.h" +#include "drake/common/autodiff_overloads.h" #include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" namespace drake { namespace geometry { - -// TODO(SeanCurtis-TRI): Get rid of the "detail" namespace. -namespace detail { +namespace internal { /** Canonicalizes the given geometry *candidate* name. A canonicalized name may still not be valid (as it may duplicate a previously used name). See @@ -17,10 +18,6 @@ namespace detail { details. */ std::string CanonicalizeStringName(const std::string& name); -} // namespace detail - -namespace internal { - /// A const range iterator through the keys of an unordered map. template class MapKeyRange { @@ -58,7 +55,35 @@ class MapKeyRange { const std::unordered_map* map_; }; -} // namespace internal +/** @name Isometry scalar conversion + + Some of SceneGraph's inner-workings are _not_ templated on scalar type and + always require Isometry3. These functions work in an ADL-compatible + manner to allow SceneGraph to mindlessly convert templated Isometry3 to + double-valued transforms. */ +//@{ +// TODO(SeanCurtis-TRI): Get rid of these when I finally swap for +// RigidTransforms. + +inline const Isometry3& convert(const Isometry3& transform) { + return transform; +} + +template +Isometry3 convert( + const Isometry3>& transform) { + Isometry3 result; + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + result.matrix()(r, c) = ExtractDoubleOrThrow(transform.matrix()(r, c)); + } + } + return result; +} + +//@} + +} // namespace internal } // namespace geometry } // namespace drake