diff --git a/attic/manipulation/util/frame_pose_tracker.cc b/attic/manipulation/util/frame_pose_tracker.cc index 74e499e50a5d..24290b384935 100644 --- a/attic/manipulation/util/frame_pose_tracker.cc +++ b/attic/manipulation/util/frame_pose_tracker.cc @@ -11,6 +11,7 @@ namespace manipulation { namespace util { using drake::systems::KinematicsResults; +using drake::math::RigidTransformd; FramePoseTracker::FramePoseTracker( const RigidBodyTree& tree, @@ -120,8 +121,8 @@ void FramePoseTracker::OutputStatus( it != frame_name_to_frame_map_.end(); ++it) { output->set_value( frame_name_to_id_map_.at(it->first), - tree_->CalcFramePoseInWorldFrame( - kinematic_results->get_cache(), *(it->second.get()))); + RigidTransformd(tree_->CalcFramePoseInWorldFrame( + kinematic_results->get_cache(), *(it->second.get())))); } } diff --git a/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc b/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc index 5855b57b3e7a..7370741b90e9 100644 --- a/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc +++ b/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc @@ -31,6 +31,7 @@ using geometry::render::RenderLabel; using geometry::SceneGraph; using geometry::Shape; using geometry::Sphere; +using math::RigidTransform; namespace { @@ -148,23 +149,23 @@ void RigidBodyPlantBridge::RegisterTree(SceneGraph* scene_graph) { // Default to the world body configuration. FrameId body_id = scene_graph->world_frame_id(); - RenderLabel label; if (body.get_body_index() != tree_->world().get_body_index()) { - // All other bodies register a frame and (possibly) get a unique label. + // All other bodies register a frame. body_id = scene_graph->RegisterFrame( source_id_, GeometryFrame(body.get_name(), body.get_model_instance_id())); - - if (body.get_visual_elements().size() > 0) { - // We'll have the render label map to the body index. - // NOTE: This is only valid if the RBT is the only source of geometry. - // But given that the RBT is on the way out, why not? - label = RenderLabel(static_cast(body.get_body_index())); - label_to_index_[label] = body.get_body_index(); - } } body_ids_.push_back(body_id); + // By default, the render label value is the body index value. + RenderLabel label(static_cast(body.get_body_index())); + if (body.get_visual_elements().size() > 0) { + // We'll have the render label map to the body index. + // NOTE: This is only valid if the RBT is the only source of geometry. + // But given that the RBT is on the way out, why not? + label_to_index_[label] = body.get_body_index(); + } + // TODO(SeanCurtis-TRI): Detect if equivalent shapes are used for visual // and collision and then simply assign it additional roles. This is an // optimization. @@ -231,8 +232,8 @@ void RigidBodyPlantBridge::CalcFramePoseOutput( // When we start skipping welded frames, or frames without geometry, this // mapping won't be so trivial. for (size_t i = 1; i < tree_->get_bodies().size(); ++i) { - poses->set_value(body_ids_[i], - tree_->relativeTransform(cache, world_body, i)); + poses->set_value(body_ids_[i], RigidTransform(tree_->relativeTransform( + cache, world_body, i))); } } diff --git a/bindings/pydrake/test/geometry_test.py b/bindings/pydrake/test/geometry_test.py index e03e832feb73..1d38c0dcc57b 100644 --- a/bindings/pydrake/test/geometry_test.py +++ b/bindings/pydrake/test/geometry_test.py @@ -8,10 +8,9 @@ from pydrake.autodiffutils import AutoDiffXd from pydrake.common import FindResourceOrThrow -from pydrake.common.eigen_geometry import Isometry3_ from pydrake.common.test_utilities import numpy_compare from pydrake.lcm import DrakeMockLcm -from pydrake.math import RigidTransform +from pydrake.math import RigidTransform_ from pydrake.symbolic import Expression from pydrake.systems.framework import DiagramBuilder_, InputPort_, OutputPort_ from pydrake.systems.sensors import ( @@ -75,13 +74,13 @@ def test_connect_drake_visualizer(self): @numpy_compare.check_nonsymbolic_types def test_frame_pose_vector_api(self, T): FramePoseVector = mut.FramePoseVector_[T] - Isometry3 = Isometry3_[T] + RigidTransform = RigidTransform_[T] obj = FramePoseVector() frame_id = mut.FrameId.get_new_id() - obj.set_value(id=frame_id, value=Isometry3.Identity()) + obj.set_value(id=frame_id, value=RigidTransform.Identity()) self.assertEqual(obj.size(), 1) - self.assertIsInstance(obj.value(id=frame_id), Isometry3) + self.assertIsInstance(obj.value(id=frame_id), RigidTransform) self.assertTrue(obj.has_id(id=frame_id)) self.assertIsInstance(obj.frame_ids(), list) self.assertIsInstance(obj.frame_ids()[0], mut.FrameId) @@ -197,6 +196,7 @@ def test_render_label(self): @numpy_compare.check_nonsymbolic_types def test_query_object(self, T): + RigidTransform = RigidTransform_[float] SceneGraph = mut.SceneGraph_[T] QueryObject = mut.QueryObject_[T] SceneGraphInspector = mut.SceneGraphInspector_[T] diff --git a/examples/contact_model/bowling_ball.cc b/examples/contact_model/bowling_ball.cc index cbab2ca5f5c4..59b279d80aa9 100644 --- a/examples/contact_model/bowling_ball.cc +++ b/examples/contact_model/bowling_ball.cc @@ -31,7 +31,6 @@ #include "drake/common/eigen_types.h" #include "drake/common/find_resource.h" #include "drake/geometry/geometry_visualization.h" -#include "drake/lcm/drake_lcm.h" #include "drake/multibody/parsers/urdf_parser.h" #include "drake/multibody/rigid_body_plant/rigid_body_plant.h" #include "drake/multibody/rigid_body_plant/rigid_body_plant_bridge.h" @@ -43,9 +42,7 @@ namespace drake { namespace systems { -using drake::lcm::DrakeLcm; using multibody::joints::kQuaternion; -using Eigen::VectorXd; using std::make_unique; // Simulation parameters. diff --git a/examples/pendulum/pendulum_geometry.cc b/examples/pendulum/pendulum_geometry.cc index dcecefa72989..42dbbd6ee594 100644 --- a/examples/pendulum/pendulum_geometry.cc +++ b/examples/pendulum/pendulum_geometry.cc @@ -103,7 +103,7 @@ void PendulumGeometry::OutputGeometryPose( const double theta = input.theta(); const math::RigidTransformd pose(math::RotationMatrixd::MakeYRotation(theta)); - *poses = {{frame_id_, pose.GetAsIsometry3()}}; + *poses = {{frame_id_, pose}}; } } // namespace pendulum diff --git a/examples/planar_gripper/gripper_brick.cc b/examples/planar_gripper/gripper_brick.cc index 4e11107166ee..c4271ec663ae 100644 --- a/examples/planar_gripper/gripper_brick.cc +++ b/examples/planar_gripper/gripper_brick.cc @@ -90,7 +90,7 @@ GripperBrickHelper::GripperBrickHelper() { inspector.GetShape(finger_tip_geometry_id); finger_tip_radius_ = dynamic_cast(fingertip_shape).get_radius(); - p_L2Tip_ = inspector.X_FG(finger_tip_geometry_id).translation(); + p_L2Tip_ = inspector.GetPoseInFrame(finger_tip_geometry_id).translation(); const geometry::Shape& brick_shape = inspector.GetShape(inspector.GetGeometryIdByName( plant_->GetBodyFrameIdOrThrow( diff --git a/examples/quadrotor/quadrotor_geometry.cc b/examples/quadrotor/quadrotor_geometry.cc index 79852f0910ba..33507dcb1f6a 100644 --- a/examples/quadrotor/quadrotor_geometry.cc +++ b/examples/quadrotor/quadrotor_geometry.cc @@ -69,7 +69,7 @@ void QuadrotorGeometry::OutputGeometryPose( math::RollPitchYawd(state.segment<3>(3)), state.head<3>()); - *poses = {{frame_id_, pose.GetAsIsometry3()}}; + *poses = {{frame_id_, pose}}; } } // namespace quadrotor diff --git a/examples/scene_graph/bouncing_ball_plant.cc b/examples/scene_graph/bouncing_ball_plant.cc index 82d7e6c0fab2..7f1f902cf1e3 100644 --- a/examples/scene_graph/bouncing_ball_plant.cc +++ b/examples/scene_graph/bouncing_ball_plant.cc @@ -27,6 +27,8 @@ using geometry::render::RenderLabel; using geometry::SceneGraph; using geometry::SourceId; using geometry::Sphere; +using math::RigidTransform; +using math::RigidTransformd; using std::make_unique; using systems::Context; @@ -55,7 +57,7 @@ BouncingBallPlant::BouncingBallPlant(SourceId source_id, source_id, GeometryFrame("ball_frame")); ball_id_ = scene_graph->RegisterGeometry( source_id, ball_frame_id_, - make_unique(Isometry3::Identity(), /*X_FG*/ + make_unique(RigidTransformd::Identity(), /*X_FG*/ make_unique(diameter_ / 2.0), "ball")); // Use the default material. @@ -107,9 +109,9 @@ void BouncingBallPlant::CopyStateToOutput( template void BouncingBallPlant::CalcFramePoseOutput( const Context& context, FramePoseVector* poses) const { - Isometry3 pose = Isometry3::Identity(); + RigidTransform pose = RigidTransform::Identity(); const BouncingBallVector& state = get_state(context); - pose.translation() << p_WB_.x(), p_WB_.y(), state.z(); + pose.set_translation({p_WB_.x(), p_WB_.y(), state.z()}); *poses = {{ball_frame_id_, pose}}; } diff --git a/examples/scene_graph/solar_system.cc b/examples/scene_graph/solar_system.cc index ef52b50013cc..e8e4f422bf96 100644 --- a/examples/scene_graph/solar_system.cc +++ b/examples/scene_graph/solar_system.cc @@ -17,6 +17,9 @@ namespace drake { namespace examples { namespace solar_system { +using Eigen::AngleAxisd; +using Eigen::Translation3d; +using Eigen::Vector3d; using Eigen::Vector4d; using geometry::Box; using geometry::Convex; @@ -31,6 +34,7 @@ using geometry::SceneGraph; using geometry::Mesh; using geometry::SourceId; using geometry::Sphere; +using math::RigidTransformd; using systems::BasicVector; using systems::Context; using systems::ContinuousState; @@ -123,15 +127,13 @@ void MakeArm(SourceId source_id, ParentId parent_id, double length, SceneGraph* scene_graph) { // tilt it horizontally const math::RigidTransform arm_pose( - Eigen::AngleAxis(M_PI / 2, Vector3::UnitY()), - Vector3(length / 2, 0, 0)); + AngleAxisd(M_PI / 2, Vector3d::UnitY()), Vector3d(length / 2, 0, 0)); scene_graph->RegisterGeometry( source_id, parent_id, MakeShape(arm_pose.GetAsIsometry3(), "HorzArm", material, radius, length)); - const math::RigidTransform post_pose( - Vector3(length, 0, height / 2)); + const math::RigidTransform post_pose(Vector3d(length, 0, height / 2)); scene_graph->RegisterGeometry( source_id, parent_id, MakeShape(post_pose.GetAsIsometry3(), "VertArm", material, @@ -152,13 +154,13 @@ void SolarSystem::AllocateGeometry(SceneGraph* scene_graph) { // NOTE: we don't store the id of the sun geometry because we have no need // for subsequent access (the same is also true for dynamic geometries). scene_graph->RegisterAnchoredGeometry( - source_id_, MakeShape(Isometry3::Identity(), "Sun", + source_id_, MakeShape(RigidTransformd::Identity(), "Sun", Vector4d(1, 1, 0, 1), 1.0 /* radius */)); // The fixed post on which Sun sits and around which all planets rotate. const double post_height = 1; - Isometry3 post_pose = Isometry3::Identity(); - post_pose.translation() << 0, 0, (orrery_bottom + post_height / 2); + const RigidTransformd post_pose( + Translation3d{0, 0, orrery_bottom + post_height / 2}); scene_graph->RegisterAnchoredGeometry( source_id_, MakeShape(post_pose, "Post", post_material, pipe_radius, post_height)); @@ -171,18 +173,17 @@ void SolarSystem::AllocateGeometry(SceneGraph* scene_graph) { // Earth's orbital frame Oe lies directly *below* the sun (to account for the // orrery arm). const double kEarthBottom = orrery_bottom + 0.25; - Isometry3 X_SOe{Translation3{0, 0, kEarthBottom}}; + const RigidTransformd X_SOe{Translation3d{0, 0, kEarthBottom}}; FrameId planet_id = scene_graph->RegisterFrame(source_id_, GeometryFrame("EarthOrbit")); body_ids_.push_back(planet_id); body_offset_.push_back(X_SOe); - axes_.push_back(Vector3::UnitZ()); + axes_.push_back(Vector3d::UnitZ()); // The geometry is rigidly affixed to Earth's orbital frame so that it moves // in a circular path. const double kEarthOrbitRadius = 3.0; - Isometry3 X_OeE{ - Translation3{kEarthOrbitRadius, 0, -kEarthBottom}}; + RigidTransformd X_OeE{Translation3d{kEarthOrbitRadius, 0, -kEarthBottom}}; scene_graph->RegisterGeometry( source_id_, planet_id, MakeShape(X_OeE, "Earth", Vector4d(0, 0, 1, 1), 0.25)); @@ -192,12 +193,12 @@ void SolarSystem::AllocateGeometry(SceneGraph* scene_graph) { // Luna's orbital frame Ol is at the center of Earth's geometry (E). // So, X_OeOl = X_OeE. - const Isometry3& X_OeOl = X_OeE; - FrameId luna_id = scene_graph->RegisterFrame(source_id_, planet_id, - GeometryFrame("LunaOrbit")); + const RigidTransformd& X_OeOl = X_OeE; + FrameId luna_id = scene_graph->RegisterFrame( + source_id_, planet_id, GeometryFrame("LunaOrbit")); body_ids_.push_back(luna_id); body_offset_.push_back(X_OeOl); - const Vector3 luna_axis_Oe{1, 1, 1}; + const Vector3d luna_axis_Oe{1, 1, 1}; axes_.push_back(luna_axis_Oe.normalized()); // The geometry is rigidly affixed to Luna's orbital frame so that it moves @@ -206,9 +207,9 @@ void SolarSystem::AllocateGeometry(SceneGraph* scene_graph) { // Pick a position at kLunaOrbitRadius distance from the Earth's origin on // the plane _perpendicular_ to the moon's normal (<1, 1, 1>). // luna_position.dot(luna_axis_Oe) will be zero. - Vector3 luna_position = - Vector3(-1, 0.5, 0.5).normalized() * kLunaOrbitRadius; - Isometry3 X_OlL{Translation3{luna_position}}; + Vector3d luna_position = + Vector3d(-1, 0.5, 0.5).normalized() * kLunaOrbitRadius; + RigidTransformd X_OlL{Translation3d{luna_position}}; scene_graph->RegisterGeometry( source_id_, luna_id, MakeShape(X_OlL, "Luna", Vector4d(0.5, 0.5, 0.35, 1.0), 0.075)); @@ -243,28 +244,28 @@ void SolarSystem::AllocateGeometry(SceneGraph* scene_graph) { // Mars's orbital frame Om lies directly *below* the sun (to account for the // orrery arm). - Isometry3 X_SOm{Translation3{0, 0, orrery_bottom}}; + RigidTransformd X_SOm{Translation3d{0, 0, orrery_bottom}}; planet_id = scene_graph->RegisterFrame(source_id_, GeometryFrame("MarsOrbit")); body_ids_.push_back(planet_id); body_offset_.push_back(X_SOm); - Vector3 mars_axis_S{0, 0.1, 1}; + Vector3d mars_axis_S{0, 0.1, 1}; axes_.push_back(mars_axis_S.normalized()); // The geometry is rigidly affixed to Mars's orbital frame so that it moves // in a circular path. const double kMarsOrbitRadius = 5.0; const double kMarsSize = 0.24; - Isometry3 X_OmM{ - Translation3{kMarsOrbitRadius, 0, -orrery_bottom}}; + RigidTransformd X_OmM{ + Translation3d{kMarsOrbitRadius, 0, -orrery_bottom}}; GeometryId mars_geometry_id = scene_graph->RegisterGeometry( source_id_, planet_id, MakeShape(X_OmM, "Mars", Vector4d(0.9, 0.1, 0, 1), kMarsSize)); std::string rings_absolute_path = FindResourceOrThrow("drake/examples/scene_graph/planet_rings.obj"); - Vector3 axis = Vector3(1, 1, 1).normalized(); - Isometry3 X_MR(AngleAxis(M_PI / 3, axis)); + Vector3d axis = Vector3d(1, 1, 1).normalized(); + RigidTransformd X_MR(AngleAxisd(M_PI / 3, axis), Vector3d{0, 0, 0}); scene_graph->RegisterGeometry( source_id_, mars_geometry_id, MakeShape(X_MR, "MarsRings", Vector4d(0.45, 0.9, 0, 1), @@ -277,7 +278,7 @@ void SolarSystem::AllocateGeometry(SceneGraph* scene_graph) { // Phobos's orbital frame Op is at the center of Mars (M). // So, X_OmOp = X_OmM. The normal of the plane is negated so it orbits in the // opposite direction. - const Isometry3& X_OmOp = X_OmM; + const RigidTransformd& X_OmOp = X_OmM; FrameId phobos_id = scene_graph->RegisterFrame(source_id_, planet_id, GeometryFrame("PhobosOrbit")); body_ids_.push_back(phobos_id); @@ -287,7 +288,7 @@ void SolarSystem::AllocateGeometry(SceneGraph* scene_graph) { // The geometry is displaced from the Phobos's frame so that it orbits. const double kPhobosOrbitRadius = 0.34; - Isometry3 X_OpP{Translation3{kPhobosOrbitRadius, 0, 0}}; + const RigidTransformd X_OpP{Translation3d{kPhobosOrbitRadius, 0, 0}}; scene_graph->RegisterGeometry( source_id_, phobos_id, MakeShape(X_OpP, "Phobos", Vector4d(0.65, 0.6, 0.8, 1), 0.06)); @@ -306,7 +307,7 @@ void SolarSystem::CalcFramePoseOutput(const Context& context, // rotation value. T rotation{state[i]}; pose.set_rotation(AngleAxis(rotation, axes_[i])); - poses->set_value(body_ids_[i], pose.GetAsIsometry3()); + poses->set_value(body_ids_[i], pose); } } diff --git a/examples/scene_graph/solar_system.h b/examples/scene_graph/solar_system.h index 8bd57d87222a..0360d8b0afa6 100644 --- a/examples/scene_graph/solar_system.h +++ b/examples/scene_graph/solar_system.h @@ -4,6 +4,7 @@ #include "drake/common/drake_copyable.h" #include "drake/geometry/scene_graph.h" +#include "drake/math/rigid_transform.h" #include "drake/systems/framework/basic_vector.h" #include "drake/systems/framework/context.h" #include "drake/systems/framework/leaf_system.h" @@ -151,7 +152,7 @@ class SolarSystem : public systems::LeafSystem { // The axes around each body revolves (expressed in its parent's frame) std::vector> axes_; // The translational offset of each body from its parent frame - std::vector> body_offset_; + std::vector body_offset_; }; } // namespace solar_system diff --git a/geometry/BUILD.bazel b/geometry/BUILD.bazel index fe6456f12906..e7260cd2ab9d 100644 --- a/geometry/BUILD.bazel +++ b/geometry/BUILD.bazel @@ -227,6 +227,7 @@ drake_cc_library( ":shape_specification", "//common:copyable_unique_ptr", "//common:essential", + "//math:geometric_transform", ], ) @@ -244,7 +245,10 @@ drake_cc_library( name = "shape_specification", srcs = ["shape_specification.cc"], hdrs = ["shape_specification.h"], - deps = ["//common:essential"], + deps = [ + "//common:essential", + "//math:geometric_transform", + ], ) drake_cc_library( diff --git a/geometry/frame_kinematics_vector.cc b/geometry/frame_kinematics_vector.cc index 3bb123fd809d..495fc327f627 100644 --- a/geometry/frame_kinematics_vector.cc +++ b/geometry/frame_kinematics_vector.cc @@ -7,14 +7,15 @@ #include "drake/common/autodiff.h" #include "drake/common/symbolic.h" +#include "drake/math/rigid_transform.h" namespace drake { namespace geometry { namespace { template -void InitializeKinematicsValue(Isometry3* value) { - value->setIdentity(); +void InitializeKinematicsValue(math::RigidTransform* value) { + value->SetIdentity(); } } // namespace @@ -108,9 +109,10 @@ void FrameKinematicsVector::CheckInvariants() const { } // Explicitly instantiates on the most common scalar types. -template class FrameKinematicsVector>; -template class FrameKinematicsVector>; -template class FrameKinematicsVector>; +template class FrameKinematicsVector>; +template class FrameKinematicsVector>; +template class FrameKinematicsVector< + math::RigidTransform>; } // namespace geometry } // namespace drake diff --git a/geometry/frame_kinematics_vector.h b/geometry/frame_kinematics_vector.h index 0febdeb3e2e9..4053068921ca 100644 --- a/geometry/frame_kinematics_vector.h +++ b/geometry/frame_kinematics_vector.h @@ -46,7 +46,7 @@ namespace geometry { } std::vector frame_ids_; - std::vector> poses_; + std::vector> poses_; }; ``` @@ -56,7 +56,7 @@ namespace geometry { ``` void CalcFramePoseOutput(const Context& context, geometry::FramePoseVector* poses) const { - const Isometry3& pose = ...; + const RigidTransform& pose = ...; *poses = {{frame_id_, pose}}; } ``` @@ -74,19 +74,20 @@ namespace geometry { One should never interact with the %FrameKinematicsVector class directly. Instead, the FramePoseVector, FrameVelocityVector, and FrameAccelerationVector classes are aliases of the %FrameKinematicsVector instantiated on specific - data types (Isometry3, SpatialVector, and SpatialAcceleration, respectively). - Each of these data types are templated on Eigen scalars. All supported - combinations of data type and scalar type are already available to link against - in the containing library. No other values for KinematicsValue are supported. + data types (RigidTransform, SpatialVector, and SpatialAcceleration, + respectively). Each of these data types are templated on Eigen scalars. All + supported combinations of data type and scalar type are already available to + link against in the containing library. No other values for KinematicsValue are + supported. Currently, the following data types with the following scalar types are supported: - Alias | Instantiation | Scalar types - -----------------|------------------------------------------|-------------- - FramePoseVector | FrameKinematicsVector> | double - FramePoseVector | FrameKinematicsVector> | AutoDiffXd - FramePoseVector | FrameKinematicsVector> | Expression + Alias | Instantiation | Scalar types + -----------------|-----------------------------------------------|------------- + FramePoseVector | FrameKinematicsVector> | double + FramePoseVector | FrameKinematicsVector> | AutoDiffXd + FramePoseVector | FrameKinematicsVector> | Expression */ template class FrameKinematicsVector { @@ -169,7 +170,7 @@ class FrameKinematicsVector { No other values for T are currently supported. */ template -using FramePoseVector = FrameKinematicsVector>; +using FramePoseVector = FrameKinematicsVector>; } // namespace geometry } // namespace drake diff --git a/geometry/geometry_instance.cc b/geometry/geometry_instance.cc index 3a156d84ec81..2826040b7eae 100644 --- a/geometry/geometry_instance.cc +++ b/geometry/geometry_instance.cc @@ -8,6 +8,11 @@ namespace geometry { GeometryInstance::GeometryInstance(const Isometry3& X_PG, std::unique_ptr shape, const std::string& name) + : GeometryInstance(math::RigidTransformd(X_PG), std::move(shape), name) {} + +GeometryInstance::GeometryInstance(const math::RigidTransform& X_PG, + std::unique_ptr shape, + const std::string& name) : id_(GeometryId::get_new_id()), X_PG_(X_PG), shape_(std::move(shape)), diff --git a/geometry/geometry_instance.h b/geometry/geometry_instance.h index 3ddfde13b2b4..55e8692a607c 100644 --- a/geometry/geometry_instance.h +++ b/geometry/geometry_instance.h @@ -11,6 +11,7 @@ #include "drake/geometry/geometry_ids.h" #include "drake/geometry/geometry_roles.h" #include "drake/geometry/shape_specification.h" +#include "drake/math/rigid_transform.h" namespace drake { namespace geometry { @@ -87,9 +88,20 @@ class GeometryInstance { @param name The name of the geometry (must satisfy the name requirements). @throws std::logic_error if the canonicalized version of `name` is empty. */ + DRAKE_DEPRECATED("2019-11-01", + "Please use the RigidTransform-based constructor.") GeometryInstance(const Isometry3& X_PG, std::unique_ptr shape, const std::string& name); + /** Constructs a geometry instance specification. + @param X_PG The pose of this geometry (`G`) in its parent's frame (`P`). + @param shape The underlying shape for this geometry instance. + @param name The name of the geometry (must satisfy the name requirements). + @throws std::logic_error if the canonicalized version of `name` is empty. + */ + GeometryInstance(const math::RigidTransform& X_PG, + std::unique_ptr shape, const std::string& name); + /** Returns the globally unique id for this geometry specification. Every instantiation of %GeometryInstance will contain a unique id value. The id value is preserved across copies. After successfully registering this @@ -97,8 +109,11 @@ class GeometryInstance { representation as well. */ GeometryId id() const { return id_; } - const Isometry3& pose() const { return X_PG_; } - void set_pose(const Isometry3& X_PG) { X_PG_ = X_PG; } + /** Returns the instance geometry's pose in its parent frame. */ + const math::RigidTransformd& pose() const { return X_PG_; } + + /** Sets the pose of this instance in its parent's frame. */ + void set_pose(const math::RigidTransformd& X_PG) { X_PG_ = X_PG; } const Shape& shape() const { DRAKE_DEMAND(shape_ != nullptr); @@ -175,7 +190,7 @@ class GeometryInstance { GeometryId id_{}; // The pose of the geometry relative to the parent frame it hangs on. - Isometry3 X_PG_; + math::RigidTransform X_PG_; // The shape associated with this instance. copyable_unique_ptr shape_; diff --git a/geometry/geometry_state.cc b/geometry/geometry_state.cc index 571aa46c5c03..c487cfbac41e 100644 --- a/geometry/geometry_state.cc +++ b/geometry/geometry_state.cc @@ -120,8 +120,8 @@ GeometryState::GeometryState() FrameIndex(0), world, InternalFrame::world_frame_clique()); frame_index_to_id_map_.push_back(world); - X_WF_.push_back(Isometry3::Identity()); - X_PF_.push_back(Isometry3::Identity()); + X_WF_.push_back(RigidTransform::Identity()); + X_PF_.push_back(RigidTransform::Identity()); source_frame_id_map_[self_source_] = {world}; source_root_frame_map_[self_source_] = {world}; @@ -343,14 +343,14 @@ const Shape& GeometryState::GetShape(GeometryId id) const { } template -const Isometry3& GeometryState::GetPoseInFrame( +const math::RigidTransform& GeometryState::GetPoseInFrame( GeometryId geometry_id) const { const auto& geometry = GetValueOrThrow(geometry_id, geometries_); return geometry.X_FG(); } template -const Isometry3& GeometryState::GetPoseInParent( +const math::RigidTransform& GeometryState::GetPoseInParent( GeometryId geometry_id) const { const auto& geometry = GetValueOrThrow(geometry_id, geometries_); return geometry.X_PG(); @@ -418,7 +418,7 @@ bool GeometryState::CollisionFiltered(GeometryId id1, GeometryId id2) const { } template -const Isometry3& GeometryState::get_pose_in_world( +const math::RigidTransform& GeometryState::get_pose_in_world( FrameId frame_id) const { FindOrThrow(frame_id, frames_, [frame_id]() { return "No world pose available for invalid frame id: " + @@ -428,7 +428,7 @@ const Isometry3& GeometryState::get_pose_in_world( } template -const Isometry3& GeometryState::get_pose_in_world( +const math::RigidTransform& GeometryState::get_pose_in_world( GeometryId geometry_id) const { FindOrThrow(geometry_id, geometries_, [geometry_id]() { return "No world pose available for invalid geometry id: " + @@ -438,7 +438,7 @@ const Isometry3& GeometryState::get_pose_in_world( } template -const Isometry3& GeometryState::get_pose_in_parent( +const math::RigidTransform& GeometryState::get_pose_in_parent( FrameId frame_id) const { FindOrThrow(frame_id, frames_, [frame_id]() { return "No pose available for invalid frame id: " + to_string(frame_id); @@ -498,8 +498,8 @@ FrameId GeometryState::RegisterFrame(SourceId source_id, FrameId parent_id, DRAKE_ASSERT(X_PF_.size() == frame_index_to_id_map_.size()); FrameIndex index(X_PF_.size()); - X_PF_.emplace_back(Isometry3::Identity()); - X_WF_.emplace_back(Isometry3::Identity()); + X_PF_.emplace_back(RigidTransform::Identity()); + X_WF_.emplace_back(RigidTransform::Identity()); frame_index_to_id_map_.push_back(frame_id); f_set.insert(frame_id); int clique = GeometryStateCollisionFilterAttorney::get_next_clique( @@ -552,16 +552,14 @@ GeometryId GeometryState::RegisterGeometry( InternalFrame& frame = frames_[frame_id]; frame.add_child(geometry_id); - // NOTE: No implicit conversion from Isometry3 to Isometry3. - // However, we can implicitly assign Matrix to Matrix. - Isometry3 X_WG; - X_WG.matrix() = geometry->pose().matrix(); - X_WGs_[geometry_id] = X_WG; + // pose() is always RigidTransform. To account for + // GeometryState, we need to cast it to the common type T. + X_WGs_[geometry_id] = geometry->pose().cast(); - geometries_.emplace(geometry_id, - InternalGeometry(source_id, geometry->release_shape(), - frame_id, geometry_id, geometry->name(), - geometry->pose())); + geometries_.emplace( + geometry_id, + InternalGeometry(source_id, geometry->release_shape(), frame_id, + geometry_id, geometry->name(), geometry->pose())); // Any roles defined on the geometry instance propagate through automatically. if (geometry->illustration_properties()) { @@ -620,8 +618,8 @@ GeometryId GeometryState::RegisterGeometryWithParent( // X_FG_ vector assuming the parent was the frame. Replace it by concatenating // its pose in parent, with its parent's pose in frame. NOTE: the pose is no // longer available from geometry because of the `move(geometry)`. - const Isometry3& X_PG = new_geometry.X_FG(); - const Isometry3& X_FP = parent_geometry.X_FG(); + const RigidTransform& X_PG = new_geometry.X_FG(); + const RigidTransform& X_FP = parent_geometry.X_FG(); new_geometry.set_geometry_parent(parent_id, X_FP * X_PG); parent_geometry.add_child(new_id); return new_id; @@ -1024,7 +1022,7 @@ void GeometryState::SetFramePoses( // TODO(SeanCurtis-TRI): Down the road, make this validation depend on // ASSERT_ARMED. ValidateFrameIds(source_id, poses); - const Isometry3 world_pose = Isometry3::Identity(); + const RigidTransform world_pose = RigidTransform::Identity(); for (auto frame_id : source_root_frame_map_[source_id]) { UpdatePosesRecursively(frames_[frame_id], world_pose, poses); } @@ -1058,15 +1056,8 @@ void GeometryState::ValidateFrameIds( template void GeometryState::FinalizePoseUpdate() { geometry_engine_->UpdateWorldPoses(X_WGs_); - // TODO(SeanCurtis-TRI): Kill this horrible copy once Isometry3 is removed - // and X_WGs_ is RigidTransform typed. - std::unordered_map> Xrt_WG; - for (const auto& id_geometry_pair : geometries_) { - const GeometryId id = id_geometry_pair.first; - Xrt_WG[id] = RigidTransform(X_WGs_[id]); - } for (auto& pair : render_engines_) { - pair.second->UpdatePoses(Xrt_WG); + pair.second->UpdatePoses(X_WGs_); } } @@ -1130,30 +1121,21 @@ void GeometryState::RemoveGeometryUnchecked(GeometryId geometry_id, template void GeometryState::UpdatePosesRecursively( - const internal::InternalFrame& frame, const Isometry3& X_WP, + const internal::InternalFrame& frame, const RigidTransform& X_WP, const FramePoseVector& poses) { const auto frame_id = frame.id(); const auto& X_PF = poses.value(frame_id); // Cache this transform for later use. X_PF_[frame.index()] = X_PF; - Isometry3 X_WF = X_WP * X_PF; - // TODO(SeanCurtis-TRI): Replace this when we have a transform object that - // allows proper multiplication between an AutoDiff type and a double type. - // For now, it allows me to perform the multiplication by multiplying the - // fully-defined transformation (with [0 0 0 1] on the bottom row). - X_WF.makeAffine(); + RigidTransform X_WF = X_WP * X_PF; X_WF_[frame.index()] = X_WF; // Update the geometry which belong to *this* frame. for (auto child_id : frame.child_geometries()) { auto& child_geometry = geometries_[child_id]; - // TODO(SeanCurtis-TRI): See note above about replacing this when we have a - // transform that supports autodiff * double. - Isometry3 X_FG(child_geometry.X_FG()); - X_FG.makeAffine(); - // TODO(SeanCurtis-TRI): These matrix() shenanigans are here because I can't - // assign a an Isometry3 to an Isometry3. Replace this - // when I can. - X_WGs_[child_id].matrix() = X_WF.matrix() * X_FG.matrix(); + // X_FG() is always RigidTransform, to account for + // GeometryState, we need to cast it to the common type T. + RigidTransform X_FG(child_geometry.X_FG()); + X_WGs_[child_id] = X_WF * X_FG.cast(); } // Update each child frame. @@ -1365,7 +1347,7 @@ RigidTransformd GeometryState::GetDoubleWorldPose(FrameId frame_id) const { return RigidTransformd::Identity(); } const internal::InternalFrame& frame = GetValueOrThrow(frame_id, frames_); - return RigidTransformd(internal::convert_to_double(X_WF_[frame.index()])); + return internal::convert_to_double(X_WF_[frame.index()]); } } // namespace geometry diff --git a/geometry/geometry_state.h b/geometry/geometry_state.h index 3dfed5c00e53..7787ea1a35f6 100644 --- a/geometry/geometry_state.h +++ b/geometry/geometry_state.h @@ -107,9 +107,7 @@ class GeometryState { int get_num_frames() const { return static_cast(frames_.size()); } /** Implementation of SceneGraphInspector::all_frame_ids(). */ - FrameIdRange get_frame_ids() const { - return FrameIdRange(&frames_); - } + FrameIdRange get_frame_ids() const { return FrameIdRange(&frames_); } /** Implementation of SceneGraphInspector::num_geometries(). */ int get_num_geometries() const { @@ -189,8 +187,7 @@ class GeometryState { int NumGeometriesWithRole(FrameId frame_id, Role role) const; /** Implementation of SceneGraphInspector::GetGeometryIdByName(). */ - GeometryId GetGeometryFromName(FrameId frame_id, - Role role, + GeometryId GetGeometryFromName(FrameId frame_id, Role role, const std::string& name) const; //@} @@ -216,10 +213,12 @@ class GeometryState { const Shape& GetShape(GeometryId id) const; /** Implementation of SceneGraphInspector::X_FG(). */ - const Isometry3& GetPoseInFrame(GeometryId geometry_id) const; + const math::RigidTransform& GetPoseInFrame( + GeometryId geometry_id) const; /** Implementation of SceneGraphInspector::X_PG(). */ - const Isometry3& GetPoseInParent(GeometryId geometry_id) const; + const math::RigidTransform& GetPoseInParent( + GeometryId geometry_id) const; /** Implementation of SceneGraphInspector::GetProximityProperties(). */ const ProximityProperties* GetProximityProperties(GeometryId id) const; @@ -242,13 +241,14 @@ class GeometryState { //@{ /** Implementation of QueryObject::X_WF(). */ - const Isometry3& get_pose_in_world(FrameId frame_id) const; + const math::RigidTransform& get_pose_in_world(FrameId frame_id) const; /** Implementation of QueryObject::X_WG(). */ - const Isometry3& get_pose_in_world(GeometryId geometry_id) const; + const math::RigidTransform& get_pose_in_world( + GeometryId geometry_id) const; /** Implementation of QueryObject::X_PF(). */ - const Isometry3& get_pose_in_parent(FrameId frame_id) const; + const math::RigidTransform& get_pose_in_parent(FrameId frame_id) const; //@} @@ -292,13 +292,15 @@ class GeometryState { const GeometryFrame& frame); /** Implementation of - @ref SceneGraph::RegisterGeometry(SourceId,FrameId,std::unique_ptr) + @ref + SceneGraph::RegisterGeometry(SourceId,FrameId,std::unique_ptr) "SceneGraph::RegisterGeometry()" with parent FrameId. */ GeometryId RegisterGeometry(SourceId source_id, FrameId frame_id, std::unique_ptr geometry); /** Implementation of - @ref SceneGraph::RegisterGeometry(SourceId,GeometryId,std::unique_ptr) + @ref + SceneGraph::RegisterGeometry(SourceId,GeometryId,std::unique_ptr) "SceneGraph::RegisterGeometry()" with parent GeometryId. */ GeometryId RegisterGeometryWithParent( SourceId source_id, GeometryId parent_id, @@ -308,8 +310,7 @@ class GeometryState { // wrapper for the more general `RegisterGeometry()`. /** Implementation of SceneGraph::RegisterAnchoredGeometry(). */ GeometryId RegisterAnchoredGeometry( - SourceId source_id, - std::unique_ptr geometry); + SourceId source_id, std::unique_ptr geometry); /** Removes the given geometry from the the indicated source's geometries. Any geometry that was hung from the indicated geometry will _also_ be removed. @@ -470,10 +471,8 @@ class GeometryState { } /** Supporting function for QueryObject::ComputeSignedDistanceToPoint(). */ - std::vector> - ComputeSignedDistanceToPoint( - const Vector3 &p_WQ, - double threshold) const { + std::vector> ComputeSignedDistanceToPoint( + const Vector3& p_WQ, double threshold) const { return geometry_engine_->ComputeSignedDistanceToPoint(p_WQ, X_WGs_, threshold); } @@ -502,23 +501,20 @@ class GeometryState { /** Implementation support for QueryObject::RenderColorImage(). @pre All poses have already been updated. */ void RenderColorImage(const render::CameraProperties& camera, - FrameId parent_frame, - const math::RigidTransformd& X_PC, + FrameId parent_frame, const math::RigidTransformd& X_PC, bool show_window, systems::sensors::ImageRgba8U* color_image_out) const; /** Implementation support for QueryObject::RenderDepthImage(). @pre All poses have already been updated. */ void RenderDepthImage(const render::DepthCameraProperties& camera, - FrameId parent_frame, - const math::RigidTransformd& X_PC, + FrameId parent_frame, const math::RigidTransformd& X_PC, systems::sensors::ImageDepth32F* depth_image_out) const; /** Implementation support for QueryObject::RenderLabelImage(). @pre All poses have already been updated. */ void RenderLabelImage(const render::CameraProperties& camera, - FrameId parent_frame, - const math::RigidTransformd& X_PC, + FrameId parent_frame, const math::RigidTransformd& X_PC, bool show_window, systems::sensors::ImageLabel16I* label_image_out) const; @@ -557,28 +553,25 @@ class GeometryState { frame_index_to_id_map_(source.frame_index_to_id_map_), geometry_engine_(std::move(source.geometry_engine_->ToAutoDiffXd())), render_engines_(source.render_engines_) { - // NOTE: Can't assign Isometry3 to Isometry3. But we _can_ - // assign Matrix to Matrix, so that's what we're doing. - auto convert_pose_vector = [](const std::vector>& s, - std::vector>* d) { - std::vector>& dest = *d; + auto convert_pose_vector = [](const std::vector>& s, + std::vector>* d) { + std::vector>& dest = *d; dest.resize(s.size()); for (size_t i = 0; i < s.size(); ++i) { - dest[i].matrix() = s[i].matrix(); + dest[i] = s[i].template cast(); } }; convert_pose_vector(source.X_PF_, &X_PF_); convert_pose_vector(source.X_WF_, &X_WF_); // Now convert the id -> pose map. - std::unordered_map>& dest = X_WGs_; - const std::unordered_map>& s = source.X_WGs_; + std::unordered_map>& dest = X_WGs_; + const std::unordered_map>& s = + source.X_WGs_; for (const auto& id_pose_pair : s) { const GeometryId id = id_pose_pair.first; - const Isometry3& X_WG_source = id_pose_pair.second; - Isometry3 X_WG; - X_WG.matrix() = X_WG_source.matrix(); - dest.insert({id, std::move(X_WG)}); + const math::RigidTransform& X_WG_source = id_pose_pair.second; + dest.insert({id, X_WG_source.template cast()}); } } @@ -592,7 +585,8 @@ class GeometryState { // Friend declaration so that the internals of the state can be confirmed in // unit tests. - template friend class GeometryStateTester; + template + friend class GeometryStateTester; // Function to facilitate testing. int peek_next_clique() const { @@ -652,8 +646,8 @@ class GeometryState { // TODO(SeanCurtis-TRI): Add `kFrame` when this can be invoked by removing // a frame. enum class RemoveGeometryOrigin { - kGeometry, // Invoked by RemoveGeometry(). - kRecurse // Invoked by recursive call in RemoveGeometryUnchecked. + kGeometry, // Invoked by RemoveGeometry(). + kRecurse // Invoked by recursive call in RemoveGeometryUnchecked. }; // Performs the work necessary to remove the identified geometry from @@ -674,7 +668,7 @@ class GeometryState { // rooted at the given frame, whose parent's pose in the world frame is given // as `X_WP`. void UpdatePosesRecursively(const internal::InternalFrame& frame, - const Isometry3& X_WP, + const math::RigidTransform& X_WP, const FramePoseVector& poses); // Reports true if the given id refers to a _dynamic_ geometry. Assumes the @@ -714,7 +708,7 @@ class GeometryState { // nothing to remove). This does no checking on ownership. // @pre geometry_id maps to a registered geometry. bool RemoveFromRendererUnchecked(const std::string& renderer_name, - GeometryId id); + GeometryId id); bool RemoveProximityRole(GeometryId geometry_id); bool RemoveIllustrationRole(GeometryId geometry_id); @@ -735,7 +729,7 @@ class GeometryState { // and, assuming the ids and relationships are valid, returns the frame // requested. const internal::InternalFrame& ValidateAndGetFrame(SourceId source_id, - FrameId frame_id) const; + FrameId frame_id) const; // Retrieves the requested renderer (if supported), throwing otherwise. const render::RenderEngine& GetRenderEngineOrThrow( @@ -807,7 +801,7 @@ class GeometryState { // Map from a frame's index to the _current_ pose of the frame F it identifies // relative to its parent frame P, i.e., X_PF. // TODO(SeanCurtis-TRI): Rename this to X_PFs_ to reflect multiplicity. - std::vector> X_PF_; + std::vector> X_PF_; // The pose of every geometry relative to the _world_ frame (regardless of // roles) keyed by the corresponding geometry's id. After a complete state @@ -817,7 +811,7 @@ class GeometryState { // frame Fₖ, and the world frame W is the parent of frame Fₙ. // In other words, it is the full evaluation of the kinematic chain from the // geometry to the world frame. - std::unordered_map> X_WGs_; + std::unordered_map> X_WGs_; // The pose of each frame relative to the _world_ frame. // frames_.size() == X_WF_.size() is an invariant. Furthermore, after a @@ -828,7 +822,7 @@ class GeometryState { // In other words, it is the full evaluation of the kinematic chain from // frame i to the world frame. // TODO(SeanCurtis-TRI): Rename this to X_WFs_ to reflect multiplicity. - std::vector> X_WF_; + std::vector> X_WF_; // The underlying geometry engine. The topology of the engine does _not_ // change with respect to time. But its values do. This straddles the two diff --git a/geometry/geometry_visualization.cc b/geometry/geometry_visualization.cc index fb07aa0cb49c..82a1253766ef 100644 --- a/geometry/geometry_visualization.cc +++ b/geometry/geometry_visualization.cc @@ -24,6 +24,11 @@ namespace geometry { namespace { +using Eigen::Quaterniond; +using Eigen::Vector3d; +using Eigen::Vector4d; +using math::RigidTransformd; + // Simple class for converting shape specifications into LCM-compatible shapes. class ShapeToLcm : public ShapeReifier { public: @@ -33,8 +38,8 @@ class ShapeToLcm : public ShapeReifier { ~ShapeToLcm() override = default; lcmt_viewer_geometry_data Convert(const Shape& shape, - const Isometry3& X_PG, - const Eigen::Vector4d& in_color) { + const RigidTransformd& X_PG, + const Vector4d& in_color) { X_PG_ = X_PG; // NOTE: Reify *may* change X_PG_ based on the shape. For example, the // half-space requires an additional offset to shift the box representing @@ -47,7 +52,7 @@ class ShapeToLcm : public ShapeReifier { Eigen::Map position(geometry_data_.position); position = X_PG_.translation().cast(); // LCM quaternion must be w, x, y, z. - Eigen::Quaternion q(X_PG_.linear()); + Quaterniond q(X_PG_.rotation().ToQuaternion()); geometry_data_.quaternion[0] = q.w(); geometry_data_.quaternion[1] = q.x(); geometry_data_.quaternion[2] = q.y(); @@ -88,9 +93,8 @@ class ShapeToLcm : public ShapeReifier { // The final pose of the box is the half-space's pose pre-multiplied by // an offset sufficient to move the box down so it's top face lies on the // z = 0 plane. - Isometry3 box_xform = Isometry3::Identity(); // Shift it down so that the origin lies on the top surface. - box_xform.translation() << 0, 0, -thickness / 2; + RigidTransformd box_xform{Vector3d{0, 0, -thickness / 2}}; X_PG_ = X_PG_ * box_xform; } @@ -125,12 +129,12 @@ class ShapeToLcm : public ShapeReifier { private: lcmt_viewer_geometry_data geometry_data_{}; // The transform from the geometry frame to its parent frame. - Eigen::Isometry3d X_PG_; + RigidTransformd X_PG_; }; lcmt_viewer_geometry_data MakeGeometryData(const Shape& shape, - const Isometry3& X_PG, - const Eigen::Vector4d& in_color) { + const RigidTransformd& X_PG, + const Vector4d& in_color) { ShapeToLcm converter; return converter.Convert(shape, X_PG, in_color); } @@ -169,7 +173,7 @@ lcmt_viewer_load_robot GeometryVisualizationImpl::BuildLoadMessage( message.num_links = frame_count; message.link.resize(frame_count); - const Eigen::Vector4d default_color({0.9, 0.9, 0.9, 1.0}); + const Vector4d default_color({0.9, 0.9, 0.9, 1.0}); auto get_properties = [](const InternalGeometry& geometry, Role role_for_visualization) { @@ -205,7 +209,7 @@ lcmt_viewer_load_robot GeometryVisualizationImpl::BuildLoadMessage( const GeometryProperties* props = get_properties(geometry, role); if (props != nullptr) { const Shape& shape = geometry.shape(); - const Eigen::Vector4d& color = props->GetPropertyOrDefault( + const Vector4d& color = props->GetPropertyOrDefault( "phong", "diffuse", default_color); message.link[0].geom[geom_index] = MakeGeometryData( shape, geometry.X_FG(), color); @@ -235,7 +239,7 @@ lcmt_viewer_load_robot GeometryVisualizationImpl::BuildLoadMessage( const GeometryProperties* props = get_properties(geometry, role); if (props != nullptr) { const Shape& shape = geometry.shape(); - const Eigen::Vector4d& color = props->GetPropertyOrDefault( + const Vector4d& color = props->GetPropertyOrDefault( "phong", "diffuse", default_color); message.link[link_index].geom[geom_index] = MakeGeometryData(shape, geometry.X_FG(), color); diff --git a/geometry/internal_geometry.cc b/geometry/internal_geometry.cc index cec653a72f87..9ea6e21c6483 100644 --- a/geometry/internal_geometry.cc +++ b/geometry/internal_geometry.cc @@ -8,18 +8,19 @@ namespace drake { namespace geometry { namespace internal { +using math::RigidTransform; using std::move; InternalGeometry::InternalGeometry( SourceId source_id, std::unique_ptr shape, FrameId frame_id, - GeometryId geometry_id, std::string name, const Isometry3& X_FG) + GeometryId geometry_id, std::string name, RigidTransform X_FG) : shape_spec_(std::move(shape)), id_(geometry_id), name_(std::move(name)), source_id_(source_id), frame_id_(frame_id), - X_PG_(X_FG), - X_FG_(X_FG), + X_PG_(move(X_FG)), + X_FG_(X_PG_), parent_geometry_id_(nullopt) {} bool InternalGeometry::has_role(Role role) const { diff --git a/geometry/internal_geometry.h b/geometry/internal_geometry.h index cef355d70ad8..de2613c78a02 100644 --- a/geometry/internal_geometry.h +++ b/geometry/internal_geometry.h @@ -14,6 +14,7 @@ #include "drake/geometry/geometry_roles.h" #include "drake/geometry/internal_frame.h" #include "drake/geometry/shape_specification.h" +#include "drake/math/rigid_transform.h" namespace drake { namespace geometry { @@ -45,7 +46,7 @@ class InternalGeometry { @param X_FG The pose of the geometry G in the parent frame F. */ InternalGeometry(SourceId source_id, std::unique_ptr shape, FrameId frame_id, GeometryId geometry_id, std::string name, - const Isometry3& X_FG); + math::RigidTransform X_FG); /** Compares two %InternalGeometry instances for "equality". Two internal geometries are considered equal if they have the same geometry identifier. @@ -95,11 +96,11 @@ class InternalGeometry { /** Returns the pose of this geometry in the declared *parent* frame -- note if this geometry was registered as a child of another geometry it will *not* be the same as X_FG(). */ - const Isometry3& X_PG() const { return X_PG_; } + const math::RigidTransform& X_PG() const { return X_PG_; } /** Returns the pose of this geometry in the frame to which it is ultimately rigidly attached. This is in contrast to X_PG(). */ - const Isometry3& X_FG() const { return X_FG_; } + const math::RigidTransform& X_FG() const { return X_FG_; } // TODO(SeanCurtis-TRI): Determine if tracking this parent geometry is // necessary for now or if that only exists to facilitate removal later on. @@ -111,10 +112,10 @@ class InternalGeometry { be updated. @param id The id of the parent geometry. @param X_FG The new value for X_FG (assuming the constructed value is to be - interpreted as X_PG. */ - void set_geometry_parent(GeometryId id, const Isometry3& X_FG) { + interpreted as X_PG). */ + void set_geometry_parent(GeometryId id, math::RigidTransform X_FG) { parent_geometry_id_ = id; - X_FG_ = X_FG; + X_FG_ = std::move(X_FG); } /** Returns true if this geometry has a geometry parent and the parent has the @@ -265,11 +266,11 @@ class InternalGeometry { // The pose of this geometry in the registered parent frame. The parent may be // a frame or another registered geometry. - Isometry3 X_PG_; + math::RigidTransform X_PG_; // The pose of this geometry in the ultimate frame to which this geometry is // rigidly affixed. If there is no parent geometry, X_PG_ == X_FG_. - Isometry3 X_FG_; + math::RigidTransform X_FG_; // The identifier for this frame's parent frame. optional parent_geometry_id_{}; diff --git a/geometry/proximity/distance_to_point.h b/geometry/proximity/distance_to_point.h index bc64654713de..129fc0bd7ee4 100644 --- a/geometry/proximity/distance_to_point.h +++ b/geometry/proximity/distance_to_point.h @@ -51,7 +51,7 @@ struct CallbackData { fcl::CollisionObjectd* query_in, const double threshold_in, const Vector3& p_WQ_W_in, - const std::unordered_map>* X_WGs_in, + const std::unordered_map>* X_WGs_in, std::vector>* distances_in) : query_point(*query_in), threshold(threshold_in), @@ -73,7 +73,7 @@ struct CallbackData { const Vector3 p_WQ_W; /** The T-valued pose of every geometry. */ - const std::unordered_map>& X_WGs; + const std::unordered_map>& X_WGs; /** The accumulator for results. */ std::vector>& distances; diff --git a/geometry/proximity/distance_to_shape.h b/geometry/proximity/distance_to_shape.h index 27cb46547b1d..727568a291e5 100644 --- a/geometry/proximity/distance_to_shape.h +++ b/geometry/proximity/distance_to_shape.h @@ -49,7 +49,8 @@ struct CallbackData { reported. @param nearest_pairs_in[out] The output results. Aliased. */ CallbackData(const CollisionFilterLegacy* collision_filter_in, - const std::unordered_map>* X_WGs_in, + const std::unordered_map>* X_WGs_in, const double max_distance_in, std::vector>* nearest_pairs_in) : collision_filter(*collision_filter_in), @@ -65,7 +66,7 @@ struct CallbackData { const CollisionFilterLegacy& collision_filter; /** The T-valued poses of all geometries. */ - const std::unordered_map>& X_WGs; + const std::unordered_map>& X_WGs; /** The maximum distance at which a pair's distance will be reported. */ const double max_distance; @@ -264,9 +265,9 @@ void CalcDistanceFallback(const fcl::CollisionObjectd& a, @pre The pair should *not* be (Halfspace, X), unless X is Sphere. */ template void ComputeNarrowPhaseDistance(const fcl::CollisionObjectd& a, - const Isometry3& X_WA, + const math::RigidTransform& X_WA, const fcl::CollisionObjectd& b, - const Isometry3& X_WB, + const math::RigidTransform& X_WB, const fcl::DistanceRequestd& request, SignedDistancePair* result) { DRAKE_DEMAND(result != nullptr); diff --git a/geometry/proximity/test/distance_sphere_to_shape_test.cc b/geometry/proximity/test/distance_sphere_to_shape_test.cc index 5838f43d7e7b..22cc4196e2ac 100644 --- a/geometry/proximity/test/distance_sphere_to_shape_test.cc +++ b/geometry/proximity/test/distance_sphere_to_shape_test.cc @@ -58,8 +58,6 @@ void DistancePairGeometry::operator()(const fcl::Sphered&, namespace { -using Eigen::Isometry3d; -using Eigen::Translation3d; using Eigen::Vector3d; using fcl::Boxd; using fcl::CollisionObjectd; @@ -390,8 +388,8 @@ GTEST_TEST(ComputeNarrowPhaseDistance, NoFallbackInvocation) { // T-valued parameters. using T = float; - const Isometry3 X_WA = Isometry3::Identity(); - const Isometry3 X_WB = Isometry3::Identity(); + const RigidTransform X_WA = RigidTransform::Identity(); + const RigidTransform X_WB = RigidTransform::Identity(); SignedDistancePair result; // These scenarios should *not* invoke the fallback; they will be exercised @@ -422,8 +420,8 @@ GTEST_TEST(ComputeNarrowPhaseDistance, FallbackInvocation) { // T-valued parameters. using T = float; - const Isometry3 X_WA = Isometry3::Identity(); - const Isometry3 X_WB = Isometry3::Identity(); + const RigidTransform X_WA = RigidTransform::Identity(); + const RigidTransform X_WB = RigidTransform::Identity(); SignedDistancePair result; // TODO(SeanCurtis-TRI): Add convex-* pairs to this list. @@ -463,9 +461,10 @@ GTEST_TEST(ComputeNarrowPhaseDistance, OrderInvariance) { EncodedData(box_id, true).write_to(&box); fcl::DistanceRequestd request{}; - const Isometry3 X_WS(Translation3d{2, 2, 2}); - const Isometry3 X_WB( - AngleAxis(M_PI / 5, Vector3d{2, 4, 7}.normalized())); + const RigidTransformd X_WS(Vector3d{2, 2, 2}); + const RigidTransformd X_WB( + AngleAxis(M_PI / 5, Vector3d{2, 4, 7}.normalized()), + Vector3d::Zero()); SignedDistancePair result_BS; SignedDistancePair result_SB; @@ -502,11 +501,11 @@ GTEST_TEST(ComputeNarrowPhaseDistance, sphere_touches_shape) { CollisionObjectd box(make_shared(side, side, side)); const GeometryId box_id = GeometryId::get_new_id(); EncodedData(box_id, true).write_to(&box); - const Isometry3 X_WB(Isometry3::Identity()); + const RigidTransformd X_WB(RigidTransformd::Identity()); const fcl::DistanceRequestd request{}; // The sphere touches the box in the middle of a face of the box. - const Isometry3 X_WS(Translation3d{radius + side / 2., 0, 0}); + const RigidTransformd X_WS(Vector3d{radius + side / 2., 0, 0}); SignedDistancePair result; ComputeNarrowPhaseDistance(sphere, X_WS, box, X_WB, request, &result); const auto p_WCs = X_WS * result.p_ACa; @@ -531,14 +530,14 @@ GTEST_TEST(ComputeNarrowPhaseDistance, is_nhat_BA_W_well_defined) { CollisionObjectd box(make_shared(2, 2, 2)); const GeometryId box_id = GeometryId::get_new_id(); EncodedData(box_id, true).write_to(&box); - const Isometry3 X_WB(Isometry3::Identity()); + const RigidTransformd X_WB(RigidTransformd::Identity()); const fcl::DistanceRequestd request{}; // Tests when `is_nhat_BA_W_unique` is true. { // The center of the sphere is outside the box. - const Isometry3 X_WS(Translation3d{3, 3, 3}); + const RigidTransformd X_WS(Vector3d{3, 3, 3}); SignedDistancePair result; ComputeNarrowPhaseDistance(sphere, X_WS, box, X_WB, request, &result); @@ -547,7 +546,7 @@ GTEST_TEST(ComputeNarrowPhaseDistance, is_nhat_BA_W_well_defined) { // Tests when `is_nhat_BA_W_unique` is false. { // The center of the sphere is at a corner of the box. - const Isometry3 X_WS(Translation3d{1, 1, 1}); + const RigidTransformd X_WS(Vector3d{1, 1, 1}); SignedDistancePair result; ComputeNarrowPhaseDistance(sphere, X_WS, box, X_WB, request, &result); @@ -574,8 +573,8 @@ class CallbackScalarSupport : public ::testing::Test { EncodedData data_B(id_B, true); collision_filter_.AddGeometry(data_A.encoding()); collision_filter_.AddGeometry(data_B.encoding()); - X_WGs_[id_A] = Isometry3{Translation3{10, 11, 12}}; - X_WGs_[id_B] = Isometry3::Identity(); + X_WGs_[id_A] = RigidTransform{Translation3{10, 11, 12}}; + X_WGs_[id_B] = RigidTransform::Identity(); auto apply_data = [&data_A, &data_B](auto& shapes) { data_A.write_to(&shapes[0]); @@ -615,7 +614,7 @@ class CallbackScalarSupport : public ::testing::Test { protected: CollisionFilterLegacy collision_filter_; - std::unordered_map> X_WGs_; + std::unordered_map> X_WGs_; std::vector> results_; CallbackData data_; std::vector spheres_; @@ -704,8 +703,9 @@ GTEST_TEST(Callback, ScalarSupportWithFilters) { // Filter the pair (A, B) by adding them to the same clique. collision_filter.AddToCollisionClique(data_A.encoding(), 1); collision_filter.AddToCollisionClique(data_B.encoding(), 1); - std::unordered_map> X_WGs{ - {id_A, Isometry3::Identity()}, {id_B, Isometry3::Identity()}}; + const std::unordered_map> X_WGs{ + {id_A, RigidTransform::Identity()}, + {id_B, RigidTransform::Identity()}}; CollisionObjectd box_A(make_shared(0.25, 0.3, 0.4)); data_A.write_to(&box_A); @@ -728,11 +728,10 @@ GTEST_TEST(Callback, RespectCollisionFiltering) { EncodedData data_B(id_B, true); data_A.write_to(&sphere_A); data_B.write_to(&sphere_B); - std::unordered_map X_WGs{ - {id_A, Isometry3d{Translation3d{10, 11, 12}}}, - {id_B, Isometry3d::Identity()}}; - std::vector geometry_map{GeometryId::get_new_id(), - GeometryId::get_new_id()}; + const std::unordered_map X_WGs{ + {id_A, RigidTransformd{Vector3d{10, 11, 12}}}, + {id_B, RigidTransformd::Identity()}}; + std::vector> results; CollisionFilterLegacy collision_filter; collision_filter.AddGeometry(data_A.encoding()); @@ -765,9 +764,9 @@ GTEST_TEST(Callback, ABOrdering) { EncodedData data_B(id_B, true); data_A.write_to(&sphere_A); data_B.write_to(&sphere_B); - std::unordered_map X_WGs{ - {id_A, Isometry3d{Translation3d{10, 11, 12}}}, - {id_B, Isometry3d::Identity()}}; + const std::unordered_map X_WGs{ + {id_A, RigidTransformd{Vector3d{10, 11, 12}}}, + {id_B, RigidTransformd::Identity()}}; CollisionFilterLegacy collision_filter; collision_filter.AddGeometry(data_A.encoding()); collision_filter.AddGeometry(data_B.encoding()); @@ -829,9 +828,9 @@ TYPED_TEST(CallbackMaxDistanceTest, MaxDistanceThreshold) { data_B.write_to(&sphere_B); const Vector3 p_WB = Vector3(2, 3, 4).normalized() * (kMaxDistance + radius_A + radius_B - kEps); - std::unordered_map> X_WGs{ - {id_A, Isometry3::Identity()}, - {id_B, Isometry3{Translation3{p_WB}}}}; + std::unordered_map> X_WGs{ + {id_A, RigidTransform::Identity()}, + {id_B, RigidTransform{Translation3{p_WB}}}}; // Case: just inside the max distance. { @@ -846,8 +845,9 @@ TYPED_TEST(CallbackMaxDistanceTest, MaxDistanceThreshold) { // Case: just outside the max distance. { - X_WGs[id_B] = Translation3{Vector3(2, 3, 4).normalized() * - (kMaxDistance + radius_A + radius_B + kEps)}; + X_WGs.at(id_B) = RigidTransform( + Translation3{Vector3(2, 3, 4).normalized() * + (kMaxDistance + radius_A + radius_B + kEps)}); std::vector> results; CallbackData data(&collision_filter, &X_WGs, kMaxDistance, &results); // NOTE: When done, this should match kMaxDistance. diff --git a/geometry/proximity/test/distance_to_point_test.cc b/geometry/proximity/test/distance_to_point_test.cc index 6d3b5dac8366..74e4f5496b51 100644 --- a/geometry/proximity/test/distance_to_point_test.cc +++ b/geometry/proximity/test/distance_to_point_test.cc @@ -20,12 +20,10 @@ namespace { // TODO(SeanCurtis-TRI): Run through proximity_engine_test and pull the tests // from there that better belong here. -using Eigen::Isometry3d; using Eigen::Vector3d; using math::RigidTransform; using math::RigidTransformd; using math::RotationMatrix; -using Eigen::Translation3d; using std::make_shared; // Performs a point-to-shape signed-distance query and tests the result. This @@ -450,7 +448,7 @@ template void TestScalarShapeSupport() { // Configure the basic query. Vector3 p_WQ{10, 10, 10}; - Isometry3 X_WQ{Translation3{p_WQ}}; + RigidTransform X_WQ{Translation3{p_WQ}}; auto point_geometry = make_shared(0); const GeometryId point_id = GeometryId::get_new_id(); fcl::CollisionObjectd query_point(point_geometry); @@ -460,8 +458,8 @@ void TestScalarShapeSupport() { std::vector> distances; double threshold = std::numeric_limits::max(); const GeometryId other_id = GeometryId::get_new_id(); - std::unordered_map> X_WGs{ - {point_id, X_WQ}, {other_id, Isometry3::Identity()}}; + std::unordered_map> X_WGs{ + {point_id, X_WQ}, {other_id, RigidTransform::Identity()}}; CallbackData data{&query_point, threshold, p_WQ, &X_WGs, &distances}; // The Drake-supported geometries (minus Mesh which isn't supported by diff --git a/geometry/proximity_engine.cc b/geometry/proximity_engine.cc index a3db10aa40a4..fe2919ab0d57 100644 --- a/geometry/proximity_engine.cc +++ b/geometry/proximity_engine.cc @@ -27,6 +27,8 @@ namespace internal { using Eigen::Vector3d; using fcl::CollisionObjectd; +using math::RigidTransform; +using math::RigidTransformd; using std::make_shared; using std::make_unique; using std::move; @@ -36,8 +38,6 @@ using std::unordered_map; namespace { -// TODO(SeanCurtis-TRI): Swap all Isometry3 for RigidTransforms. - // Struct for use in SingleCollisionCallback(). Contains the collision request // and accumulates results in a drake::multibody::collision::PointPair vector. struct CollisionData { @@ -308,13 +308,13 @@ class ProximityEngine::Impl : public ShapeReifier { collision_filter_.AddGeometry(encoding.encoding()); } - void AddAnchoredGeometry(const Shape& shape, const Isometry3& X_WG, + void AddAnchoredGeometry(const Shape& shape, const RigidTransformd& X_WG, GeometryId id) { // The collision object gets instantiated in the reification process and // placed in this unique pointer. std::unique_ptr fcl_object; shape.Reify(this, &fcl_object); - fcl_object->setTransform(X_WG); + fcl_object->setTransform(X_WG.GetAsIsometry3()); fcl_object->computeAABB(); anchored_tree_.registerObject(fcl_object.get()); anchored_tree_.update(); @@ -352,13 +352,14 @@ class ProximityEngine::Impl : public ShapeReifier { // 2. I could simply have a method that returns a mutable reference to such // a vector and the caller sets values there directly. void UpdateWorldPoses( - const std::unordered_map>& X_WGs) { + const std::unordered_map>& X_WGs) { for (const auto& id_object_pair : dynamic_objects_) { const GeometryId id = id_object_pair.first; - const Isometry3& X_WG = X_WGs.at(id); + const RigidTransform& X_WG = X_WGs.at(id); // The FCL broadphase requires double-valued poses; so we use ADL to // efficiently get double-valued poses out of arbitrary T-valued poses. - dynamic_objects_[id]->setTransform(convert_to_double(X_WG)); + dynamic_objects_[id]->setTransform( + convert_to_double(X_WG).GetAsIsometry3()); dynamic_objects_[id]->computeAABB(); } dynamic_tree_.update(); @@ -533,7 +534,7 @@ class ProximityEngine::Impl : public ShapeReifier { } std::vector> ComputeSignedDistancePairwiseClosestPoints( - const std::unordered_map>& X_WGs, + const std::unordered_map>& X_WGs, const double max_distance) const { std::vector> witness_pairs; // All these quantities are aliased in the callback data. @@ -554,7 +555,7 @@ class ProximityEngine::Impl : public ShapeReifier { std::vector> ComputeSignedDistanceToPoint( const Vector3& p_WQ, - const std::unordered_map>& X_WGs, + const std::unordered_map>& X_WGs, const double threshold) const { // We create a sphere of zero radius centered at the query point and put // it into a CollisionObject. @@ -770,12 +771,11 @@ class ProximityEngine::Impl : public ShapeReifier { int peek_next_clique() const { return collision_filter_.peek_next_clique(); } - const Isometry3& GetX_WG(GeometryId id, - bool is_dynamic) const { + const RigidTransformd GetX_WG(GeometryId id, bool is_dynamic) const { if (is_dynamic) { - return dynamic_objects_.at(id)->getTransform(); + return RigidTransformd(dynamic_objects_.at(id)->getTransform()); } else { - return anchored_objects_.at(id)->getTransform(); + return RigidTransformd(anchored_objects_.at(id)->getTransform()); } } @@ -888,7 +888,7 @@ void ProximityEngine::AddDynamicGeometry( template void ProximityEngine::AddAnchoredGeometry( - const Shape& shape, const Isometry3& X_WG, GeometryId id) { + const Shape& shape, const RigidTransformd& X_WG, GeometryId id) { impl_->AddAnchoredGeometry(shape, X_WG, id); } @@ -931,14 +931,14 @@ std::unique_ptr> ProximityEngine::ToAutoDiffXd() template void ProximityEngine::UpdateWorldPoses( - const unordered_map>& X_WGs) { + const unordered_map>& X_WGs) { impl_->UpdateWorldPoses(X_WGs); } template std::vector> ProximityEngine::ComputeSignedDistancePairwiseClosestPoints( - const std::unordered_map>& X_WGs, + const std::unordered_map>& X_WGs, const double max_distance) const { return impl_->ComputeSignedDistancePairwiseClosestPoints(X_WGs, max_distance); } @@ -947,7 +947,7 @@ template std::vector> ProximityEngine::ComputeSignedDistanceToPoint( const Vector3& query, - const std::unordered_map>& X_WGs, + const std::unordered_map>& X_WGs, const double threshold) const { return impl_->ComputeSignedDistanceToPoint(query, X_WGs, threshold); } @@ -1019,8 +1019,8 @@ int ProximityEngine::peek_next_clique() const { } template -const Isometry3& ProximityEngine::GetX_WG(GeometryId id, - bool is_dynamic) const { +const RigidTransformd ProximityEngine::GetX_WG(GeometryId id, + bool is_dynamic) const { return impl_->GetX_WG(id, is_dynamic); } diff --git a/geometry/proximity_engine.h b/geometry/proximity_engine.h index 1e16673cb61a..8385666eb4bc 100644 --- a/geometry/proximity_engine.h +++ b/geometry/proximity_engine.h @@ -15,6 +15,7 @@ #include "drake/geometry/query_results/signed_distance_pair.h" #include "drake/geometry/query_results/signed_distance_to_point.h" #include "drake/geometry/shape_specification.h" +#include "drake/math/rigid_transform.h" namespace drake { namespace geometry { @@ -29,8 +30,6 @@ namespace internal { class GeometryStateCollisionFilterAttorney; #endif -// TODO(SeanCurtis-TRI): Swap Isometry3 for the new Transform class. - /** The underlying engine for performing geometric _proximity_ queries. It owns the geometry instances and, once it has been provided with the poses of the geometry, it provides geometric queries on that geometry. @@ -91,8 +90,8 @@ class ProximityEngine { @param X_WG The pose of the shape in the world frame. @param id The id of the geometry in SceneGraph to which this shape belongs. */ - void AddAnchoredGeometry(const Shape& shape, const Isometry3& X_WG, - GeometryId id); + void AddAnchoredGeometry(const Shape& shape, + const math::RigidTransformd& X_WG, GeometryId id); // TODO(SeanCurtis-TRI): Decide if knowing whether something is dynamic or not // is *actually* sufficiently helpful to justify this act. @@ -135,7 +134,7 @@ class ProximityEngine { // 2. I could simply have a method that returns a mutable reference to such // a vector and the caller sets values there directly. void UpdateWorldPoses( - const std::unordered_map>& X_WGs); + const std::unordered_map>& X_WGs); // ---------------------------------------------------------------------- /**@name Signed Distance Queries @@ -166,7 +165,7 @@ class ProximityEngine { */ std::vector> ComputeSignedDistancePairwiseClosestPoints( - const std::unordered_map>& X_WGs, + const std::unordered_map>& X_WGs, const double max_distance) const; /** Performs work in support of GeometryState::ComputeSignedDistanceToPoint(). @@ -181,7 +180,7 @@ class ProximityEngine { std::vector> ComputeSignedDistanceToPoint( const Vector3& p_WQ, - const std::unordered_map>& X_WGs, + const std::unordered_map>& X_WGs, const double threshold = std::numeric_limits::infinity()) const; //@} @@ -315,7 +314,8 @@ class ProximityEngine { int peek_next_clique() const; // Reports the pose (X_WG) of the geometry with the given id. - const Isometry3& GetX_WG(GeometryId id, bool is_dynamic) const; + const math::RigidTransform GetX_WG(GeometryId id, + bool is_dynamic) const; //////////////////////////////////////////////////////////////////////////// diff --git a/geometry/query_object.cc b/geometry/query_object.cc index 84b3a85d6167..e76cb0ab6ebb 100644 --- a/geometry/query_object.cc +++ b/geometry/query_object.cc @@ -45,30 +45,30 @@ QueryObject& QueryObject::operator=(const QueryObject& query_object) { } template -RigidTransform QueryObject::X_WF(FrameId id) const { +const RigidTransform& QueryObject::X_WF(FrameId id) const { ThrowIfNotCallable(); FullPoseUpdate(); const GeometryState& state = geometry_state(); - return RigidTransform(state.get_pose_in_world(id)); + return state.get_pose_in_world(id); } template -RigidTransform QueryObject::X_PF(FrameId id) const { +const RigidTransform& QueryObject::X_PF(FrameId id) const { ThrowIfNotCallable(); FullPoseUpdate(); const GeometryState& state = geometry_state(); - return RigidTransform(state.get_pose_in_parent(id)); + return state.get_pose_in_parent(id); } template -RigidTransform QueryObject::X_WG(GeometryId id) const { +const RigidTransform& QueryObject::X_WG(GeometryId id) const { ThrowIfNotCallable(); FullPoseUpdate(); const GeometryState& state = geometry_state(); - return RigidTransform(state.get_pose_in_world(id)); + return state.get_pose_in_world(id); } template diff --git a/geometry/query_object.h b/geometry/query_object.h index 1bc7ddac2f21..58f30546f281 100644 --- a/geometry/query_object.h +++ b/geometry/query_object.h @@ -114,7 +114,7 @@ class QueryObject { /** Reports the position of the frame indicated by `id` relative to the world frame. @throws std::logic_error if the frame `id` is not valid. */ - math::RigidTransform X_WF(FrameId id) const; + const math::RigidTransform& X_WF(FrameId id) const; /** Reports the position of the frame indicated by `id` relative to its parent frame. If the frame was registered with the world frame as its parent frame, @@ -122,12 +122,12 @@ class QueryObject { @note This is analogous to but distinct from SceneGraphInspector::X_PG(). In this case, the pose will *always* be relative to another frame. @throws std::logic_error if the frame `id` is not valid. */ - math::RigidTransform X_PF(FrameId id) const; + const math::RigidTransform& X_PF(FrameId id) const; /** Reports the position of the geometry indicated by `id` relative to the world frame. @throws std::logic_error if the geometry `id` is not valid. */ - math::RigidTransform X_WG(GeometryId id) const; + const math::RigidTransform& X_WG(GeometryId id) const; //@} diff --git a/geometry/render/render_engine.h b/geometry/render/render_engine.h index a24ff67f2b3a..3df46404be06 100644 --- a/geometry/render/render_engine.h +++ b/geometry/render/render_engine.h @@ -156,7 +156,7 @@ class RenderEngine : public ShapeReifier { const std::unordered_map>& X_WGs) { for (const GeometryId id : update_ids_) { const math::RigidTransformd X_WG = - geometry::internal::convert(X_WGs.at(id)); + geometry::internal::convert_to_double(X_WGs.at(id)); DoUpdateVisualPose(id, X_WG); } } diff --git a/geometry/scene_graph.cc b/geometry/scene_graph.cc index de0e2845544a..d271976fc383 100644 --- a/geometry/scene_graph.cc +++ b/geometry/scene_graph.cc @@ -415,7 +415,9 @@ void SceneGraph::CalcPoseBundle(const Context& context, } for (FrameId f_id : dynamic_frames) { - output->set_pose(i, g_state.get_pose_in_world(f_id)); + // TODO(#11888): Remove GetAsIsometry3() when PoseBundle supports + // RigidTransform. + output->set_pose(i, g_state.get_pose_in_world(f_id).GetAsIsometry3()); // TODO(SeanCurtis-TRI): Handle velocity. ++i; } diff --git a/geometry/scene_graph_inspector.h b/geometry/scene_graph_inspector.h index ebef033e3e63..4156b138d9a4 100644 --- a/geometry/scene_graph_inspector.h +++ b/geometry/scene_graph_inspector.h @@ -318,7 +318,18 @@ class SceneGraphInspector { _topological parent_ P. That topological parent may be a frame F or another geometry. If the geometry was registered directly to F, then `X_PG = X_FG`. @throws std::logic_error if `id` does not map to a registered geometry. */ + DRAKE_DEPRECATED("2019-11-01", "Simply use GetPoseInParent()") const Isometry3 X_PG(GeometryId id) const { + return GetPoseInParent(id).GetAsIsometry3(); + } + + /** Reports the pose of the geometry G with the given `id` in its registered + _topological parent_ P, `X_PG`. That topological parent may be a frame F or + another geometry. If the geometry was registered directly to F, then + `X_PG = X_FG`. + @sa GetPoseInFrame() + @throws std::logic_error if `id` does not map to a registered geometry. */ + const math::RigidTransform& GetPoseInParent(GeometryId id) const { DRAKE_DEMAND(state_ != nullptr); return state_->GetPoseInParent(id); } @@ -328,7 +339,18 @@ class SceneGraphInspector { or not). If the geometry was registered directly to the frame F, then `X_PG = X_FG`. @throws std::logic_error if `id` does not map to a registered geometry. */ + DRAKE_DEPRECATED("2019-11-01", "Simply use GetPoseInFrame()") const Isometry3 X_FG(GeometryId id) const { + return GetPoseInFrame(id).GetAsIsometry3(); + } + + /** Reports the pose of the geometry G with the given `id` in its registered + frame F (regardless of whether its _topological parent_ is another geometry P + or not). If the geometry was registered directly to the frame F, then + `X_PG = X_FG`. + @sa GetPoseInParent() + @throws std::logic_error if `id` does not map to a registered geometry. */ + const math::RigidTransform& GetPoseInFrame(GeometryId id) const { DRAKE_DEMAND(state_ != nullptr); return state_->GetPoseInFrame(id); } diff --git a/geometry/shape_specification.cc b/geometry/shape_specification.cc index 6379261a5818..76c00ab7ab64 100644 --- a/geometry/shape_specification.cc +++ b/geometry/shape_specification.cc @@ -3,6 +3,8 @@ namespace drake { namespace geometry { +using math::RigidTransform; + Shape::~Shape() {} void Shape::Reify(ShapeReifier* reifier, void* user_data) const { @@ -20,42 +22,35 @@ Cylinder::Cylinder(double radius, double length) HalfSpace::HalfSpace() : Shape(ShapeTag()) {} -Isometry3 HalfSpace::MakePose(const Vector3& Cz_F, - const Vector3& p_FC) { - // TODO(SeanCurtis-TRI): Ultimately, use RotationMatrix to create the basis. - double norm = Cz_F.norm(); +RigidTransform HalfSpace::MakePose(const Vector3& Hz_dir_F, + const Vector3& p_FB) { + const double norm = Hz_dir_F.norm(); // Note: this value of epsilon is somewhat arbitrary. It's merely a minor // fence over which ridiculous vectors will trip. - if (norm < 1e-10) + if (norm < 1e-10) { throw std::logic_error("Can't make pose from a zero vector."); + } // First create basis. // Projects the normal into the first quadrant in order to identify the // *smallest* component of the normal. - const Vector3 u(Cz_F.cwiseAbs()); - int minAxis; - u.minCoeff(&minAxis); + const Vector3 u(Hz_dir_F.cwiseAbs()); + int min_axis; + u.minCoeff(&min_axis); // The axis corresponding to the smallest component of the normal will be // *most* perpendicular. - Vector3 perpAxis; - perpAxis << (minAxis == 0 ? 1 : 0), (minAxis == 1 ? 1 : 0), - (minAxis == 2 ? 1 : 0); - // Now define x-, y-, and z-axes. The z-axis lies in the normal direction. - Vector3 z_axis_F = Cz_F / norm; - Vector3 x_axis_F = Cz_F.cross(perpAxis).normalized(); - Vector3 y_axis_F = z_axis_F.cross(x_axis_F); + Vector3 perp_axis{0, 0, 0}; + perp_axis(min_axis) = 1; + // Now define x-, y-, and z-axes. The z-axis lies in the given direction. + Vector3 Hz_F = Hz_dir_F / norm; + Vector3 Hx_F = Hz_F.cross(perp_axis).normalized(); + Vector3 Hy_F = Hz_F.cross(Hx_F); + // Transformation from canonical frame C to target frame F. - Matrix3 R_FC; - R_FC.col(0) = x_axis_F; - R_FC.col(1) = y_axis_F; - R_FC.col(2) = z_axis_F; - - // Construct pose from basis and point. - Isometry3 X_FC = Isometry3::Identity(); - X_FC.linear() = R_FC; - // Find the *minimum* translation to make sure the point lies on the plane. - X_FC.translation() = z_axis_F.dot(p_FC) * z_axis_F; - return X_FC; + const auto R_FH = + math::RotationMatrixd::MakeFromOrthonormalColumns(Hx_F, Hy_F, Hz_F); + const Vector3 p_FH = Hz_F.dot(p_FB) * Hz_F; + return RigidTransform(R_FH, p_FH); } Box::Box(double width, double depth, double height) diff --git a/geometry/shape_specification.h b/geometry/shape_specification.h index 37225639bc8e..397994619ae6 100644 --- a/geometry/shape_specification.h +++ b/geometry/shape_specification.h @@ -8,6 +8,7 @@ #include "drake/common/drake_assert.h" #include "drake/common/drake_copyable.h" #include "drake/common/eigen_types.h" +#include "drake/math/rigid_transform.h" /** @file Provides the classes through which geometric shapes are introduced into @@ -174,19 +175,20 @@ class HalfSpace final : public Shape { /** Creates the pose of a canonical half space in frame F. The half space's normal is aligned to the positive z-axis of its canonical - frame C. Given the measure of that axis in frame F (Cz_F) and a position - vector to a point on the plane expressed in the same frame, `p_FC`, creates - the pose of the half space in frame F: `X_FC`. - @param Cz_F The positive z-axis of the canonical frame expressed in - frame F. It must be a non-zero vector but need not be unit - length. - @param p_FC A point lying on the half-space's boundary measured + frame H. Given a vector that points in the same direction, measured in the + F frame (Hz_dir_F) and a position vector to a point on the half space's + *boundary* expressed in the same frame, `p_FB`, creates + the pose of the half space in frame F: `X_FH`. + @param Hz_dir_F A vector in the direction of the positive z-axis of the + canonical frame expressed in frame F. It must be a non-zero + vector but need not be unit length. + @param p_FB A point B lying on the half space's boundary measured and expressed in frame F. - @retval X_FC The pose of the canonical half-space in frame F. + @retval X_FH The pose of the canonical half-space in frame F. @throws std::logic_error if the normal is _close_ to a zero-vector (e.g., ‖normal_F‖₂ < ε). */ - static Isometry3 MakePose(const Vector3& Cz_F, - const Vector3& p_FC); + static math::RigidTransform MakePose(const Vector3& Hz_dir_F, + const Vector3& p_FB); }; // TODO(SeanCurtis-TRI): Update documentation when the level of support for diff --git a/geometry/test/frame_kinematics_vector_test.cc b/geometry/test/frame_kinematics_vector_test.cc index 68aee42e37e2..35cebdbb228d 100644 --- a/geometry/test/frame_kinematics_vector_test.cc +++ b/geometry/test/frame_kinematics_vector_test.cc @@ -17,10 +17,12 @@ namespace drake { namespace geometry { namespace test { -::testing::AssertionResult ExpectExactIdentity(const Isometry3& pose) { - const Isometry3 I = Isometry3::Identity(); - return CompareMatrices(pose.matrix().block<3, 4>(0, 0), - I.matrix().block<3, 4>(0, 0)); +using math::RigidTransform; +using math::RigidTransformd; + +::testing::AssertionResult ExpectExactIdentity(const RigidTransformd& pose) { + const RigidTransformd I = RigidTransformd::Identity(); + return CompareMatrices(pose.GetAsMatrix34(), I.GetAsMatrix34()); } GTEST_TEST(FrameKinematicsVector, DefaultConstructor) { @@ -30,11 +32,10 @@ GTEST_TEST(FrameKinematicsVector, DefaultConstructor) { GTEST_TEST(FrameKinematicsVector, InitializerListCtor) { const auto& id_0 = FrameId::get_new_id(); - const Isometry3 pose_0 = Isometry3::Identity(); + const RigidTransformd pose_0 = RigidTransformd::Identity(); const auto& id_1 = FrameId::get_new_id(); - const Isometry3 pose_1 = - math::RigidTransformd{Eigen::Translation3d(0.1, 0.2, 0.3)}. - GetAsIsometry3(); + const RigidTransformd pose_1 = + math::RigidTransformd{Eigen::Translation3d(0.1, 0.2, 0.3)}; const FramePoseVector dut{{id_0, pose_0}, {id_1, pose_1}}; ASSERT_EQ(dut.size(), 2); @@ -42,32 +43,29 @@ GTEST_TEST(FrameKinematicsVector, InitializerListCtor) { EXPECT_TRUE(dut.has_id(id_1)); EXPECT_FALSE(dut.has_id(FrameId::get_new_id())); EXPECT_TRUE(ExpectExactIdentity(dut.value(id_0))); - EXPECT_TRUE(CompareMatrices( - dut.value(id_1).matrix().block<3, 4>(0, 0), - pose_1.matrix().block<3, 4>(0, 0))); + EXPECT_TRUE( + CompareMatrices(dut.value(id_1).GetAsMatrix34(), pose_1.GetAsMatrix34())); } GTEST_TEST(FrameKinematicsVector, InitializerListAssign) { const auto& id_0 = FrameId::get_new_id(); - const Isometry3 pose_0 = Isometry3::Identity(); + const RigidTransformd pose_0 = RigidTransformd::Identity(); const auto& id_1 = FrameId::get_new_id(); - const Isometry3 pose_1 = - math::RigidTransformd{Eigen::Translation3d(0.1, 0.2, 0.3)}. - GetAsIsometry3(); + const RigidTransformd pose_1 = + math::RigidTransformd{Eigen::Translation3d(0.1, 0.2, 0.3)}; // Start with a non-empty dut, so we confirm that assignment replaces all of // the existing values. (An STL map insert defaults to a no-op when the key // exists already; we don't want that!) - FramePoseVector dut{{id_1, Isometry3::Identity()}}; + FramePoseVector dut{{id_1, RigidTransformd::Identity()}}; dut = {{id_0, pose_0}, {id_1, pose_1}}; ASSERT_EQ(dut.size(), 2); EXPECT_TRUE(dut.has_id(id_0)); EXPECT_TRUE(dut.has_id(id_1)); EXPECT_FALSE(dut.has_id(FrameId::get_new_id())); EXPECT_TRUE(ExpectExactIdentity(dut.value(id_0))); - EXPECT_TRUE(CompareMatrices( - dut.value(id_1).matrix().block<3, 4>(0, 0), - pose_1.matrix().block<3, 4>(0, 0))); + EXPECT_TRUE( + CompareMatrices(dut.value(id_1).GetAsMatrix34(), pose_1.GetAsMatrix34())); dut = {}; ASSERT_EQ(dut.size(), 0); @@ -79,10 +77,11 @@ GTEST_TEST(FrameKinematicsVector, WorkingWithValues) { for (int i = 0; i < kPoseCount; ++i) ids.push_back(FrameId::get_new_id()); FramePoseVector poses; - std::vector> recorded_poses; + std::vector> recorded_poses; for (int i = 0; i < kPoseCount; ++i) { - Isometry3 pose = Isometry3::Identity(); - pose.translation() << i, i, i; + RigidTransformd pose = RigidTransformd::Identity(); + const double d = i; + pose.set_translation({d, d, d}); recorded_poses.push_back(pose); EXPECT_NO_THROW(poses.set_value(ids[i], pose)); } @@ -90,8 +89,9 @@ GTEST_TEST(FrameKinematicsVector, WorkingWithValues) { // Confirm that poses get recorded properly. for (int i = 0; i < kPoseCount; ++i) { EXPECT_TRUE(poses.has_id(ids[i])); - const Isometry3& pose = poses.value(ids[i]); - EXPECT_TRUE(CompareMatrices(pose.matrix(), recorded_poses[i].matrix())); + const RigidTransformd& pose = poses.value(ids[i]); + EXPECT_TRUE(CompareMatrices(pose.GetAsMatrix34(), + recorded_poses[i].GetAsMatrix34())); } // Confirm that poses get cleared properly. @@ -106,8 +106,9 @@ GTEST_TEST(FrameKinematicsVector, WorkingWithValues) { } for (int i = 0; i < kPoseCount; ++i) { EXPECT_TRUE(poses.has_id(ids[i])); - const Isometry3& pose = poses.value(ids[i]); - EXPECT_TRUE(CompareMatrices(pose.matrix(), recorded_poses[i].matrix())); + const RigidTransformd& pose = poses.value(ids[i]); + EXPECT_TRUE(CompareMatrices(pose.GetAsMatrix34(), + recorded_poses[i].GetAsMatrix34())); } // Ask for the pose of an id that does not belong to the set. @@ -123,10 +124,10 @@ GTEST_TEST(FrameKinematicsVector, SetWithoutAllocations) { FrameId::get_new_id(), FrameId::get_new_id(), }; - const std::vector> poses{ - Isometry3::Identity(), - Isometry3::Identity(), - Isometry3::Identity(), + const std::vector> poses{ + RigidTransformd::Identity(), + RigidTransformd::Identity(), + RigidTransformd::Identity(), }; // For the initial setting, we'd expect to see allocations. @@ -147,7 +148,8 @@ GTEST_TEST(FrameKinematicsVector, SetWithoutAllocations) { GTEST_TEST(FrameKinematicsVector, AutoDiffInstantiation) { FramePoseVector poses; - poses.set_value(FrameId::get_new_id(), Isometry3::Identity()); + poses.set_value(FrameId::get_new_id(), + RigidTransform::Identity()); EXPECT_EQ(poses.size(), 1); } @@ -159,7 +161,7 @@ GTEST_TEST(FrameKinematicsVector, SymbolicInstantiation) { FramePoseVector poses; // Set and retrieve a simple symbolic::Expression. - poses.set_value(ids[0], Isometry3::Identity()); + poses.set_value(ids[0], RigidTransform::Identity()); const Variable var_x_{"x"}; const Variable var_y_{"y"}; @@ -168,7 +170,7 @@ GTEST_TEST(FrameKinematicsVector, SymbolicInstantiation) { const Expression y_{var_y_}; const Expression z_{var_z_}; - const Isometry3 pose = Isometry3 + const RigidTransform pose = RigidTransform (Translation3(x_, y_, z_)); poses.set_value(ids[1], pose); @@ -182,7 +184,7 @@ GTEST_TEST(FrameKinematicsVector, FrameIdRange) { std::vector ids; for (int i = 0; i < 3; ++i) { ids.push_back(FrameId::get_new_id()); - poses.set_value(ids.back(), Isometry3::Identity()); + poses.set_value(ids.back(), RigidTransformd::Identity()); } std::set actual_ids; diff --git a/geometry/test/geometry_instance_test.cc b/geometry/test/geometry_instance_test.cc index aa72145d6588..8eb5cc6c3a11 100644 --- a/geometry/test/geometry_instance_test.cc +++ b/geometry/test/geometry_instance_test.cc @@ -12,6 +12,7 @@ namespace drake { namespace geometry { namespace { +using math::RigidTransformd; using std::make_unique; using std::move; @@ -21,12 +22,12 @@ GTEST_TEST(GeometryInstanceTest, IsCopyable) { // have a runtime check available but this will fail to compile if the class // is not copyable. copyable_unique_ptr geo(make_unique - (Isometry3(), make_unique(1), "sphere")); + (RigidTransformd(), make_unique(1), "sphere")); EXPECT_TRUE(geo->id().is_valid()); } GTEST_TEST(GeometryInstanceTest, IdCopies) { - Isometry3 pose = Isometry3::Identity(); + RigidTransformd pose = RigidTransformd::Identity(); auto shape = make_unique(1.0); GeometryInstance geometry_a{pose, move(shape), "geometry_a"}; GeometryInstance geometry_b(geometry_a); @@ -46,7 +47,7 @@ GTEST_TEST(GeometryInstanceTest, IdCopies) { // version of the given name. This doesn't test the definition of // canonicalization merely the fact of the act. GTEST_TEST(GeometryInstanceTest, CanonicalName) { - Isometry3 pose = Isometry3::Identity(); + RigidTransformd pose = RigidTransformd::Identity(); auto make_instance = [&pose](const std::string& name) { auto shape = make_unique(1.0); diff --git a/geometry/test/geometry_state_test.cc b/geometry/test/geometry_state_test.cc index e16bd5361dac..1b11dc5ced54 100644 --- a/geometry/test/geometry_state_test.cc +++ b/geometry/test/geometry_state_test.cc @@ -25,13 +25,14 @@ namespace drake { namespace geometry { -using Eigen::Isometry3d; using Eigen::Translation3d; using Eigen::Vector3d; using internal::DummyRenderEngine; using internal::InternalFrame; using internal::InternalGeometry; using internal::ProximityEngine; +using math::RigidTransform; +using math::RigidTransformd; using render::RenderLabel; using std::make_unique; using std::map; @@ -45,7 +46,7 @@ using std::unordered_set; using std::vector; template -using IdPoseMap = unordered_map>; +using IdPoseMap = unordered_map>; // Implementation of friend class that allows me to peek into the geometry state // to confirm invariants on the state's internal workings as a result of @@ -95,11 +96,11 @@ class GeometryStateTester { return state_->X_WGs_; } - const vector>& get_frame_world_poses() const { + const vector>& get_frame_world_poses() const { return state_->X_WF_; } - const vector>& get_frame_parent_poses() const { + const vector>& get_frame_parent_poses() const { return state_->X_PF_; } @@ -160,7 +161,7 @@ class ShapeMatcher final : public ShapeReifier { FrameId frame_id) { GeometryId g_id = state->RegisterGeometry( source_id, frame_id, - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(expected_), "shape")); state->GetShape(g_id).Reify(this); @@ -336,7 +337,7 @@ class GeometryStateTestBase { // should invoke this in its SetUp() method. void TestInit(bool add_renderer = true) { frame_ = make_unique("ref_frame"); - instance_pose_.translation() << 10, 20, 30; + instance_pose_.set_translation({10, 20, 30}); instance_ = make_unique( instance_pose_, make_unique(1.0), "instance"); gs_tester_.set_state(&geometry_state_); @@ -403,17 +404,18 @@ class GeometryStateTestBase { source_id_ = NewSource(); // Create f0. - Isometry3d pose = Isometry3d::Identity(); - pose.translation() << 1, 2, 3; - pose.linear() << 1, 0, 0, 0, 0, 1, 0, -1, 0; // 90° around x-axis. + // 90° around x-axis. + RigidTransformd pose(AngleAxis(M_PI_2, Vector3d::UnitX()), + Vector3d{1, 2, 3}); frames_.push_back(geometry_state_.RegisterFrame( source_id_, GeometryFrame("f0"))); X_WFs_.push_back(pose); X_PFs_.push_back(pose); // Create f1. - pose.translation() << 10, 20, 30; - pose.linear() << 0, 0, -1, 0, 1, 0, 1, 0, 0; // 90° around y-axis. + // 90° around y-axis. + pose = RigidTransformd(AngleAxis(M_PI_2, Vector3d::UnitY()), + Vector3d{10, 20, 30}); frames_.push_back(geometry_state_.RegisterFrame( source_id_, GeometryFrame("f1"))); X_WFs_.push_back(pose); @@ -433,12 +435,10 @@ class GeometryStateTestBase { int g_count = 0; for (auto frame_id : frames_) { for (int i = 0; i < kGeometryCount; ++i) { - pose.translation() << g_count + 1, 0, 0; - pose.linear() = - AngleAxis(g_count * M_PI / 2.0, x_axis).matrix(); + pose = RigidTransformd(AngleAxis(g_count * M_PI_2, x_axis), + Vector3d{g_count + 1.0, 0, 0}); // Have the name reflect the frame and the index in the geometry. - const string& name = - to_string(frame_id) + "_g" + to_string(i); + const string& name = to_string(frame_id) + "_g" + to_string(i); geometry_names_[g_count] = name; geometries_[g_count] = geometry_state_.RegisterGeometry( source_id_, frame_id, @@ -449,7 +449,7 @@ class GeometryStateTestBase { } // Create anchored geometry. - X_WA_ = Isometry3d{Translation3d{0, 0, -1}}; + X_WA_ = RigidTransformd{Translation3d{0, 0, -1}}; // This simultaneously tests the named registration function and // _implicitly_ tests registration of geometry against the world frame id // (as that is how `RegisterAnchoredGeometry()` works. @@ -503,7 +503,7 @@ class GeometryStateTestBase { // Members owned by the test class. unique_ptr frame_; unique_ptr instance_; - Isometry3d instance_pose_{Isometry3d::Identity()}; + RigidTransformd instance_pose_{RigidTransformd::Identity()}; GeometryState geometry_state_; GeometryStateTester gs_tester_; DummyRenderEngine* render_engine_{}; @@ -531,13 +531,13 @@ class GeometryStateTestBase { SourceId source_id_; // The poses of the frames in the world frame. - vector X_WFs_; + vector X_WFs_; // The poses of the frames in the parent's frame. - vector X_PFs_; + vector X_PFs_; // The poses of the dynamic geometries in the parent frame. - vector X_FGs_; + vector X_FGs_; // The pose of the anchored geometry in the world frame. - Isometry3d X_WA_; + RigidTransformd X_WA_; // The default source name. const string kSourceName{"default_source"}; // The name of the dummy renderer added to the geometry state. @@ -743,28 +743,29 @@ void ExpectSuccessfulTransmogrification( EXPECT_EQ(ad_tester.get_frame_index_id_map(), d_tester.get_frame_index_id_map()); - // 3. Compare Isometry3d with Isometry3 + // 3. Compare RigidTransformd with RigidTransform. for (const auto& id_geom_pair : ad_tester.get_geometries()) { const GeometryId id = id_geom_pair.first; const auto& ad_geometry = id_geom_pair.second; EXPECT_TRUE(CompareMatrices( - ad_geometry.X_FG().matrix().block<3, 4>(0, 0), - d_tester.get_geometries().at(id).X_FG().matrix().block<3, 4>(0, 0))); + ad_geometry.X_FG().GetAsMatrix34(), + d_tester.get_geometries().at(id).X_FG().GetAsMatrix34())); } - // 4. Compare Isometry3 with Isometry3d - auto test_ad_vs_double_pose = [](const Isometry3& test, - const Isometry3d& ref) { + // 4. Compare RigidTransform with RigidTransformd. + auto test_ad_vs_double_pose = [](const RigidTransform& test, + const RigidTransformd& ref) { for (int row = 0; row < 3; ++row) { for (int col = 0; col < 4; ++col) { - EXPECT_EQ(test(row, col).value(), ref(row, col)); + EXPECT_EQ(test.GetAsMatrix34()(row, col).value(), + ref.GetAsMatrix34()(row, col)); } } }; auto test_ad_vs_double = [test_ad_vs_double_pose]( - const vector>& test, - const vector& ref) { + const vector>& test, + const vector& ref) { EXPECT_EQ(test.size(), ref.size()); for (size_t i = 0; i < ref.size(); ++i) { test_ad_vs_double_pose(test[i], ref[i]); @@ -777,7 +778,7 @@ void ExpectSuccessfulTransmogrification( ASSERT_EQ(test.size(), ref.size()); for (const auto& id_pose_pair : ref) { const GeometryId id = id_pose_pair.first; - const Isometry3d& ref_pose = id_pose_pair.second; + const RigidTransformd& ref_pose = id_pose_pair.second; test_ad_vs_double_pose(test.at(id), ref_pose); } }; @@ -870,16 +871,17 @@ TEST_F(GeometryStateTest, ValidateSingleSourceTree) { child_geometries.end()); const auto& frame_in_parent = gs_tester_.get_frame_parent_poses(); EXPECT_TRUE( - CompareMatrices(frame_in_parent[frame.index()].matrix(), - X_PFs_[i].matrix())); + CompareMatrices(frame_in_parent[frame.index()].GetAsMatrix34(), + X_PFs_[i].GetAsMatrix34())); }; // When added, all frames' poses w.r.t. their parents are the identity. const auto& frame_in_parent = gs_tester_.get_frame_parent_poses(); for (FrameId frame_id : frames_) { const auto& frame = internal_frames.at(frame_id); - EXPECT_TRUE(CompareMatrices(frame_in_parent[frame.index()].matrix(), - Isometry3d::Identity().matrix())); + EXPECT_TRUE( + CompareMatrices(frame_in_parent[frame.index()].GetAsMatrix34(), + RigidTransformd::Identity().GetAsMatrix34())); } // Confirm posing positions the frames properly. @@ -913,11 +915,11 @@ TEST_F(GeometryStateTest, ValidateSingleSourceTree) { // of GetPoseInFrame() and GetPoseInParent() must be the identical (as // the documentation for GeometryState::GetPoseInParent() indicates). EXPECT_TRUE(CompareMatrices( - geometry_state_.GetPoseInFrame(geometry.id()).matrix(), - X_FGs_[i].matrix())); + geometry_state_.GetPoseInFrame(geometry.id()).GetAsMatrix34(), + X_FGs_[i].GetAsMatrix34())); EXPECT_TRUE(CompareMatrices( - geometry_state_.GetPoseInParent(geometry.id()).matrix(), - X_FGs_[i].matrix())); + geometry_state_.GetPoseInParent(geometry.id()).GetAsMatrix34(), + X_FGs_[i].GetAsMatrix34())); } } EXPECT_EQ(static_cast(gs_tester_.get_geometry_world_poses().size()), @@ -1074,8 +1076,9 @@ TEST_F(GeometryStateTest, RegisterGeometryGoodSource) { EXPECT_EQ(g_id, expected_g_id); EXPECT_EQ(geometry_state_.GetFrameId(g_id), f_id); EXPECT_TRUE(geometry_state_.BelongsToSource(g_id, s_id)); - const Isometry3d& X_FG = geometry_state_.GetPoseInFrame(g_id); - EXPECT_TRUE(CompareMatrices(X_FG.matrix(), instance_pose_.matrix())); + const RigidTransformd& X_FG = geometry_state_.GetPoseInFrame(g_id); + EXPECT_TRUE( + CompareMatrices(X_FG.GetAsMatrix34(), instance_pose_.GetAsMatrix34())); EXPECT_TRUE(gs_tester_.get_frames().at(f_id).has_child(g_id)); const auto& geometry = gs_tester_.get_geometries().at(g_id); @@ -1133,7 +1136,7 @@ TEST_F(GeometryStateTest, RegisterGeometryonValidGeometry) { const double x = 3; const double y = 2; const double z = 1; - Isometry3d pose{Translation3d{x, y, z}}; + RigidTransformd pose{Translation3d{x, y, z}}; const int parent_index = 0; const GeometryId parent_id = geometries_[parent_index]; const FrameId frame_id = geometry_state_.GetFrameId(parent_id); @@ -1150,15 +1153,16 @@ TEST_F(GeometryStateTest, RegisterGeometryonValidGeometry) { // geometry is at [parent_index + 1, 0, 0] and this is at [3, 2, 1]. They // simply sum up. The parent has *no* rotation so the resultant transform is // simply the sum of the translation vectors. - const Isometry3d expected_pose_in_frame{ + const RigidTransformd expected_pose_in_frame{ Translation3d{(parent_index + 1) + x, y, z}}; EXPECT_EQ(frame_id, geometry_state_.GetFrameId(g_id)); - const Isometry3d& X_FG = geometry_state_.GetPoseInFrame(g_id); - EXPECT_TRUE(CompareMatrices(X_FG.matrix(), expected_pose_in_frame.matrix(), - 1e-14, MatrixCompareType::absolute)); - const Isometry3d& X_PG = geometry_state_.GetPoseInParent(g_id); - EXPECT_TRUE(CompareMatrices(X_PG.matrix(), pose.matrix(), + const RigidTransformd& X_FG = geometry_state_.GetPoseInFrame(g_id); + EXPECT_TRUE(CompareMatrices(X_FG.GetAsMatrix34(), + expected_pose_in_frame.GetAsMatrix34(), 1e-14, + MatrixCompareType::absolute)); + const RigidTransformd& X_PG = geometry_state_.GetPoseInParent(g_id); + EXPECT_TRUE(CompareMatrices(X_PG.GetAsMatrix34(), pose.GetAsMatrix34(), 1e-14, MatrixCompareType::absolute)); EXPECT_TRUE(gs_tester_.get_frames().at(frame_id).has_child(g_id)); @@ -1179,7 +1183,7 @@ TEST_F(GeometryStateTest, RegisterGeometryonValidGeometry) { TEST_F(GeometryStateTest, RegisterGeometryonInvalidGeometry) { const SourceId s_id = SetUpSingleSourceTree(); auto instance = make_unique( - Isometry3d::Identity(), make_unique(1), "sphere"); + RigidTransformd::Identity(), make_unique(1), "sphere"); const GeometryId junk_id = GeometryId::get_new_id(); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.RegisterGeometryWithParent(s_id, junk_id, move(instance)), @@ -1202,7 +1206,7 @@ TEST_F(GeometryStateTest, RegisterNullGeometryonGeometry) { TEST_F(GeometryStateTest, RegisterAnchoredGeometry) { const SourceId s_id = NewSource("new source"); auto instance = make_unique( - Isometry3d::Identity(), make_unique(1), "sphere"); + RigidTransformd::Identity(), make_unique(1), "sphere"); const GeometryId expected_g_id = instance->id(); const auto g_id = geometry_state_.RegisterAnchoredGeometry(s_id, move(instance)); @@ -1214,20 +1218,21 @@ TEST_F(GeometryStateTest, RegisterAnchoredGeometry) { EXPECT_EQ(s_id, g->source_id()); // Assigned directly to the world frame means the pose in parent and frame // should match. - EXPECT_TRUE(CompareMatrices(g->X_FG().matrix(), g->X_PG().matrix())); + EXPECT_TRUE( + CompareMatrices(g->X_FG().GetAsMatrix34(), g->X_PG().GetAsMatrix34())); } // Tests the registration of a new geometry on another geometry. TEST_F(GeometryStateTest, RegisterAnchoredOnAnchoredGeometry) { // Add an anchored geometry. const SourceId s_id = NewSource("new source"); - Isometry3d pose{Translation3d{1, 2, 3}}; + RigidTransformd pose{Translation3d{1, 2, 3}}; auto instance = make_unique( pose, make_unique(1), "sphere1"); auto parent_id = geometry_state_.RegisterAnchoredGeometry(s_id, move(instance)); - pose = Isometry3d::Identity(); + pose = RigidTransformd::Identity(); instance = make_unique( pose, make_unique(1), "sphere2"); auto child_id = geometry_state_.RegisterGeometryWithParent(s_id, parent_id, @@ -1237,9 +1242,11 @@ TEST_F(GeometryStateTest, RegisterAnchoredOnAnchoredGeometry) { EXPECT_TRUE(parent->has_child(child_id)); EXPECT_TRUE(static_cast(child->parent_id())); EXPECT_EQ(parent_id, *child->parent_id()); - EXPECT_TRUE(CompareMatrices(pose.matrix(), child->X_PG().matrix())); - const Isometry3d& X_FP = parent->X_FG(); - EXPECT_TRUE(CompareMatrices((X_FP * pose).matrix(), child->X_FG().matrix())); + EXPECT_TRUE( + CompareMatrices(pose.GetAsMatrix34(), child->X_PG().GetAsMatrix34())); + const RigidTransformd& X_FP = parent->X_FG(); + EXPECT_TRUE(CompareMatrices((X_FP * pose).GetAsMatrix34(), + child->X_FG().GetAsMatrix34())); EXPECT_EQ(InternalFrame::world_frame_id(), parent->frame_id()); } @@ -1258,7 +1265,7 @@ TEST_F(GeometryStateTest, RegisterDuplicateAnchoredGeometry) { // Tests the attempt to register anchored geometry on an invalid source. TEST_F(GeometryStateTest, RegisterAnchoredGeometryInvalidSource) { auto instance = make_unique( - Isometry3d::Identity(), make_unique(1), "sphere"); + RigidTransformd::Identity(), make_unique(1), "sphere"); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.RegisterAnchoredGeometry(SourceId::get_new_id(), move(instance)), @@ -1330,7 +1337,7 @@ TEST_F(GeometryStateTest, RemoveGeometry) { EXPECT_NO_THROW( added_id = geometry_state_.RegisterGeometry( source_id_, frames_[0], - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1), "newest"))); // Adding proximity role to the new geometry brings the total number of @@ -1367,7 +1374,7 @@ TEST_F(GeometryStateTest, RemoveGeometryTree) { // Hang geometry from the first geometry. const GeometryId g_id = geometry_state_.RegisterGeometryWithParent( s_id, root_id, - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), unique_ptr(new Sphere(1)), "leaf")); geometry_state_.AssignRole(s_id, g_id, ProximityProperties()); @@ -1403,7 +1410,7 @@ TEST_F(GeometryStateTest, RemoveChildLeaf) { // Hang geometry from the first geometry. const GeometryId g_id = geometry_state_.RegisterGeometryWithParent( s_id, parent_id, - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), unique_ptr(new Sphere(1)), "leaf")); EXPECT_EQ(geometry_state_.get_num_geometries(), single_tree_total_geometry_count() + 1); @@ -1444,7 +1451,7 @@ TEST_F(GeometryStateTest, RemoveGeometryInvalid) { EXPECT_EQ(geometry_state_.get_num_frames(), single_tree_frame_count() + 1); const GeometryId g_id = geometry_state_.RegisterGeometry( s_id2, frame_id, - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), unique_ptr(new Sphere(1)), "new")); EXPECT_EQ(geometry_state_.get_num_geometries(), single_tree_total_geometry_count() + 1); @@ -1459,11 +1466,11 @@ TEST_F(GeometryStateTest, RemoveGeometryInvalid) { TEST_F(GeometryStateTest, RemoveAnchoredGeometry) { const SourceId s_id = SetUpSingleSourceTree(Assign::kProximity); - const Vector3 normal{0, 1, 0}; - const Vector3 point{1, 1, 1}; + const Vector3 normal_W{0, 1, 0}; + const Vector3 p_WB{1, 1, 1}; const auto anchored_id_1 = geometry_state_.RegisterAnchoredGeometry( s_id, - make_unique(HalfSpace::MakePose(normal, point), + make_unique(HalfSpace::MakePose(normal_W, p_WB), make_unique(), "anchored1")); geometry_state_.AssignRole(s_id, anchored_id_1, ProximityProperties()); // Confirm conditions of having added the anchored geometry. @@ -1554,7 +1561,7 @@ TEST_F(GeometryStateTest, SourceOwnershipInvalidSource) { SetUpSingleSourceTree(); const GeometryId anchored_id = geometry_state_.RegisterAnchoredGeometry( source_id_, - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1), "sphere")); // Valid frame/geometry ids. @@ -1587,7 +1594,7 @@ TEST_F(GeometryStateTest, SourceOwnershipFrameId) { TEST_F(GeometryStateTest, SourceOwnershipGeometryId) { const SourceId s_id = SetUpSingleSourceTree(); const GeometryId anchored_id = geometry_state_.RegisterAnchoredGeometry( - s_id, make_unique(Isometry3d::Identity(), + s_id, make_unique(RigidTransformd::Identity(), make_unique(1), "sphere")); // Test for invalid geometry. @@ -1612,7 +1619,7 @@ TEST_F(GeometryStateTest, ValidateFrameIds) { const SourceId s_id = SetUpSingleSourceTree(); FramePoseVector frame_set; for (auto frame_id : frames_) { - frame_set.set_value(frame_id, Isometry3d::Identity()); + frame_set.set_value(frame_id, RigidTransformd::Identity()); } // Case: frame ids are valid. EXPECT_NO_THROW(gs_tester_.ValidateFrameIds(s_id, frame_set)); @@ -1620,7 +1627,7 @@ TEST_F(GeometryStateTest, ValidateFrameIds) { // Case: Right number, wrong frames. FramePoseVector frame_set_2; for (int i = 0; i < kFrameCount; ++i) { - frame_set_2.set_value(FrameId::get_new_id(), Isometry3d::Identity()); + frame_set_2.set_value(FrameId::get_new_id(), RigidTransformd::Identity()); } DRAKE_EXPECT_THROWS_MESSAGE( gs_tester_.ValidateFrameIds(s_id, frame_set_2), std::runtime_error, @@ -1630,7 +1637,7 @@ TEST_F(GeometryStateTest, ValidateFrameIds) { // Case: Too few frames. FramePoseVector frame_set_3; for (int i = 0; i < kFrameCount - 1; ++i) { - frame_set.set_value(frames_[i], Isometry3d::Identity()); + frame_set.set_value(frames_[i], RigidTransformd::Identity()); } DRAKE_EXPECT_THROWS_MESSAGE( gs_tester_.ValidateFrameIds(s_id, frame_set_3), std::runtime_error, @@ -1648,9 +1655,9 @@ TEST_F(GeometryStateTest, ValidateFrameIds) { TEST_F(GeometryStateTest, SetFramePoses) { const SourceId s_id = SetUpSingleSourceTree(); // A vector of poses we will use to populate FramePoseVectors. - vector frame_poses; + vector frame_poses; for (int i = 0; i < kFrameCount; ++i) { - frame_poses.push_back(Isometry3d::Identity()); + frame_poses.push_back(RigidTransformd::Identity()); } auto make_pose_vector = @@ -1674,23 +1681,22 @@ TEST_F(GeometryStateTest, SetFramePoses) { const auto& world_poses = gs_tester_.get_geometry_world_poses(); for (int i = 0; i < total_geom; ++i) { const GeometryId id = geometries_[i]; - EXPECT_TRUE(CompareMatrices(world_poses.at(id).matrix().block<3, 4>(0, 0), - X_FGs_[i].matrix().block<3, 4>(0, 0))); + EXPECT_TRUE(CompareMatrices(world_poses.at(id).GetAsMatrix34(), + X_FGs_[i].GetAsMatrix34())); } // Case 2: Move the two *root* frames 1 unit in the +y direction. The f2 will // stay at the identity. // The final geometry poses should all be offset by 1 unit in the y. - const Isometry3d offset{Translation3d{0, 1, 0}}; + const RigidTransformd offset{Translation3d{0, 1, 0}}; frame_poses[0] = offset; frame_poses[1] = offset; FramePoseVector poses2 = make_pose_vector(); gs_tester_.SetFramePoses(s_id, poses2); for (int i = 0; i < total_geom; ++i) { const GeometryId id = geometries_[i]; - EXPECT_TRUE( - CompareMatrices(world_poses.at(id).matrix().block<3, 4>(0, 0), - (offset * X_FGs_[i].matrix()).block<3, 4>(0, 0))); + EXPECT_TRUE(CompareMatrices(world_poses.at(id).GetAsMatrix34(), + (offset * X_FGs_[i]).GetAsMatrix34())); } // Case 3: All frames get set to move up one unit. This will leave geometries @@ -1701,13 +1707,12 @@ TEST_F(GeometryStateTest, SetFramePoses) { for (int i = 0; i < total_geom; ++i) { const GeometryId id = geometries_[i]; if (i < (kFrameCount - 1) * kGeometryCount) { - EXPECT_TRUE( - CompareMatrices(world_poses.at(id).matrix().block<3, 4>(0, 0), - (offset * X_FGs_[i].matrix()).block<3, 4>(0, 0))); + EXPECT_TRUE(CompareMatrices(world_poses.at(id).GetAsMatrix34(), + (offset * X_FGs_[i]).GetAsMatrix34())); } else { - EXPECT_TRUE(CompareMatrices( - world_poses.at(id).matrix().block<3, 4>(0, 0), - (offset * offset * X_FGs_[i].matrix()).block<3, 4>(0, 0))); + EXPECT_TRUE( + CompareMatrices(world_poses.at(id).GetAsMatrix34(), + (offset * offset * X_FGs_[i]).GetAsMatrix34())); } } } @@ -1737,35 +1742,35 @@ TEST_F(GeometryStateTest, QueryFrameProperties) { for (int i = 0; i < kFrameCount; ++i) poses.set_value(frames_[i], X_PFs_[i]); gs_tester_.SetFramePoses(s_id, poses); + EXPECT_TRUE(CompareMatrices( + geometry_state_.get_pose_in_world(frames_[0]).GetAsMatrix34(), + X_WFs_[0].GetAsMatrix34())); EXPECT_TRUE( - CompareMatrices(geometry_state_.get_pose_in_world(frames_[0]).matrix(), - X_WFs_[0].matrix())); - EXPECT_TRUE( - CompareMatrices(geometry_state_.get_pose_in_world(world).matrix(), - Isometry3d::Identity().matrix())); + CompareMatrices(geometry_state_.get_pose_in_world(world).GetAsMatrix34(), + RigidTransformd::Identity().GetAsMatrix34())); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.get_pose_in_world(FrameId::get_new_id()), std::logic_error, "No world pose available for invalid frame id: \\d+"); // This assumes that geometry parent belongs to frame 0. - const Isometry3d X_WG = X_WFs_[0] * X_FGs_[0]; + const RigidTransformd X_WG = X_WFs_[0] * X_FGs_[0]; EXPECT_TRUE(CompareMatrices( - geometry_state_.get_pose_in_world(geometries_[0]).matrix(), - X_WG.matrix())); + geometry_state_.get_pose_in_world(geometries_[0]).GetAsMatrix34(), + X_WG.GetAsMatrix34())); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.get_pose_in_world(GeometryId::get_new_id()), std::logic_error, "No world pose available for invalid geometry id: \\d+"); EXPECT_TRUE(CompareMatrices( - geometry_state_.get_pose_in_world(anchored_geometry_).matrix(), - X_WA_.matrix())); + geometry_state_.get_pose_in_world(anchored_geometry_).GetAsMatrix34(), + X_WA_.GetAsMatrix34())); + EXPECT_TRUE(CompareMatrices( + geometry_state_.get_pose_in_parent(frames_[0]).GetAsMatrix34(), + X_PFs_[0].GetAsMatrix34())); EXPECT_TRUE( - CompareMatrices(geometry_state_.get_pose_in_parent(frames_[0]).matrix(), - X_PFs_[0].matrix())); - EXPECT_TRUE( - CompareMatrices(geometry_state_.get_pose_in_parent(world).matrix(), - Isometry3d::Identity().matrix())); + CompareMatrices(geometry_state_.get_pose_in_parent(world).GetAsMatrix34(), + RigidTransformd::Identity().GetAsMatrix34())); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.get_pose_in_parent(FrameId::get_new_id()), std::logic_error, "No pose available for invalid frame id: \\d+"); @@ -1981,7 +1986,7 @@ TEST_F(GeometryStateTest, NonProximityRoleInCollisionFilter) { // Documentation on the single source tree indicates that the previous spheres // are at (5, 0, 0) and (6, 0, 0), respectively. Split the distance to put the // new geometry in a penetrating configuration. - const Isometry3d pose{Translation3d{5.5, 0, 0}}; + const RigidTransformd pose{Translation3d{5.5, 0, 0}}; const string name("added_sphere"); GeometryId added_id = geometry_state_.RegisterGeometry( source_id_, frames_[2], @@ -2183,7 +2188,7 @@ TEST_F(GeometryStateTest, GetGeometryIdFromName) { for (int i = 0; i < 2; ++i) { geometry_state_.RegisterGeometry( source_id_, frames_[0], - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1), dummy_name)); } DRAKE_EXPECT_THROWS_MESSAGE( @@ -2206,7 +2211,7 @@ TEST_F(GeometryStateTest, GeometryNameStorage) { const GeometryId id = geometry_state_.RegisterGeometry( source_id_, frames_[0], make_unique( - Isometry3d::Identity(), make_unique(1), " " + name)); + RigidTransformd::Identity(), make_unique(1), " " + name)); EXPECT_EQ(geometry_state_.GetName(id), name); } @@ -2216,7 +2221,7 @@ TEST_F(GeometryStateTest, GeometryNameStorage) { const GeometryId id = geometry_state_.RegisterGeometry( source_id_, frames_[1], make_unique( - Isometry3d::Identity(), make_unique(1), name)); + RigidTransformd::Identity(), make_unique(1), name)); EXPECT_EQ(geometry_state_.GetName(id), name); } } @@ -2299,7 +2304,7 @@ TEST_F(GeometryStateTest, AssignRolesToGeometry) { // We need at least 8 geometries to run through all role permutations. Add // geometries until we're there. - const Isometry3d pose = Isometry3d::Identity(); + const RigidTransformd pose = RigidTransformd::Identity(); for (int i = 0; i < 8 - single_tree_dynamic_geometry_count(); ++i) { const string name = "new_geom" + std::to_string(i); geometries_.push_back(geometry_state_.RegisterGeometry( @@ -2457,7 +2462,7 @@ TEST_F(GeometryStateTest, RoleLookUp) { Assign::kPerception); GeometryId no_role_id = geometry_state_.RegisterGeometry( source_id_, frames_[0], - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(0.5), "no_roles")); GeometryId invalid_id = GeometryId::get_new_id(); @@ -2688,9 +2693,9 @@ TEST_F(GeometryStateTest, RoleAssignExceptions) { // Addition of geometry with duplicate name -- no problem. Assigning it a // duplicate role -- bad. const GeometryId new_id = geometry_state_.RegisterGeometry( - source_id_, frames_[0], - make_unique( - Isometry3d::Identity(), make_unique(1), geometry_names_[0])); + source_id_, frames_[0], make_unique( + RigidTransformd::Identity(), + make_unique(1), geometry_names_[0])); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.AssignRole(source_id_, new_id, ProximityProperties()), std::logic_error, @@ -2814,7 +2819,7 @@ TEST_F(GeometryStateTest, ProximityRoleOnMesh) { // Add a mesh to a frame. const GeometryId mesh_id = geometry_state_.RegisterGeometry( source_id_, frames_[0], - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), make_unique("path", 1.0), "mesh")); const InternalGeometry* mesh = gs_tester_.GetGeometry(mesh_id); ASSERT_NE(mesh, nullptr); @@ -3030,7 +3035,7 @@ TEST_F(GeometryStateTest, AddRendererAfterGeometry) { // Add one geometry that has no perception properties. const GeometryId id_no_perception = geometry_state_.RegisterGeometry( source_id_, frames_[0], - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(0.5), "shape")); EXPECT_EQ(render_engine_->num_registered(), single_tree_total_geometry_count()); @@ -3113,13 +3118,14 @@ TEST_F(GeometryStateTest, RendererPoseUpdate) { const GeometryId expected_id = expected_id_pose_pair.first; const auto& test_iter = test.find(expected_id); EXPECT_NE(test_iter, test.end()); - EXPECT_TRUE(CompareMatrices(test_iter->second.matrix(), - expected_id_pose_pair.second.matrix())); + EXPECT_TRUE( + CompareMatrices(test_iter->second.GetAsMatrix34(), + expected_id_pose_pair.second.GetAsMatrix34())); } }; auto get_expected_ids = [this]() { - map expected; + map expected; for (int i = 0; i < single_tree_dynamic_geometry_count(); ++i) { const GeometryId id = geometries_[i]; expected.insert({id, gs_tester_.get_geometry_world_poses().at(id)}); @@ -3127,7 +3133,7 @@ TEST_F(GeometryStateTest, RendererPoseUpdate) { return expected; }; - map expected_ids = get_expected_ids(); + map expected_ids = get_expected_ids(); expect_poses(second_engine->updated_ids(), expected_ids); expect_poses(render_engine_->updated_ids(), expected_ids); render_engine_->init_test_data(); @@ -3137,8 +3143,8 @@ TEST_F(GeometryStateTest, RendererPoseUpdate) { // values. const Vector3d offset{1, 2, 3}; for (int f = 0; f < static_cast(frames_.size()); ++f) { - Isometry3d X_PF = X_PFs_[f]; - X_PF.translation() += offset; + RigidTransformd X_PF = X_PFs_[f]; + X_PF.set_translation(X_PF.translation() + offset); poses.set_value(frames_[f], X_PF); } EXPECT_EQ(second_engine->updated_ids().size(), 0u); diff --git a/geometry/test/geometry_visualization_test.cc b/geometry/test/geometry_visualization_test.cc index e38e11d79b80..b89066b59f08 100644 --- a/geometry/test/geometry_visualization_test.cc +++ b/geometry/test/geometry_visualization_test.cc @@ -18,8 +18,8 @@ namespace { using drake::geometry::internal::GeometryVisualizationImpl; using drake::systems::Context; -using Eigen::Isometry3d; using Eigen::Vector4d; +using math::RigidTransformd; using std::make_unique; using std::unique_ptr; @@ -44,7 +44,7 @@ GTEST_TEST(GeometryVisualization, SimpleScene) { const float a = 0.125f; GeometryId sphere_id = scene_graph.RegisterGeometry( source_id, frame_id, - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(radius), "sphere")); Vector4 color{r, g, b, a}; scene_graph.AssignRole(source_id, sphere_id, @@ -57,7 +57,7 @@ GTEST_TEST(GeometryVisualization, SimpleScene) { scene_graph.RegisterFrame(source_id, GeometryFrame(collision_frame_name)); GeometryId collision_id = scene_graph.RegisterGeometry( source_id, collision_frame_id, - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(radius), "sphere_collision")); scene_graph.AssignRole(source_id, collision_id, ProximityProperties()); diff --git a/geometry/test/proximity_engine_test.cc b/geometry/test/proximity_engine_test.cc index 17b9e14cd8f4..43533daa8c54 100644 --- a/geometry/test/proximity_engine_test.cc +++ b/geometry/test/proximity_engine_test.cc @@ -67,10 +67,9 @@ using math::RollPitchYawd; using math::RotationMatrixd; using Eigen::AngleAxisd; -using Eigen::Isometry3d; using Eigen::Translation3d; -using Eigen::Vector3d; using Eigen::Vector2d; +using Eigen::Vector3d; using std::make_shared; using std::move; @@ -94,7 +93,7 @@ GTEST_TEST(ProximityEngineTests, AddDynamicGeometry) { GTEST_TEST(ProximityEngineTests, AddAnchoredGeometry) { ProximityEngine engine; Sphere sphere{0.5}; - Isometry3 pose = Isometry3::Identity(); + RigidTransformd pose = RigidTransformd::Identity(); const GeometryId id = GeometryId::get_new_id(); engine.AddAnchoredGeometry(sphere, pose, id); EXPECT_EQ(engine.num_geometries(), 1); @@ -106,7 +105,7 @@ GTEST_TEST(ProximityEngineTests, AddAnchoredGeometry) { GTEST_TEST(ProximityEngineTests, AddMixedGeometry) { ProximityEngine engine; Sphere sphere{0.5}; - Isometry3 pose = Isometry3::Identity(); + RigidTransformd pose = RigidTransformd::Identity(); const GeometryId id_1 = GeometryId::get_new_id(); engine.AddAnchoredGeometry(sphere, pose, id_1); @@ -133,11 +132,11 @@ GTEST_TEST(ProximityEngineTests, RemoveGeometry) { // x = 0, 2, & 4. With radius of 0.5, they should *not* be colliding. Sphere sphere{0.5}; std::vector ids; - std::unordered_map poses; + std::unordered_map poses; for (int i = 0; i < 3; ++i) { const GeometryId id = GeometryId::get_new_id(); ids.push_back(id); - poses.insert({id, Isometry3d{Translation3d{i * 2.0, 0, 0}}}); + poses.insert({id, RigidTransformd{Translation3d{i * 2.0, 0, 0}}}); if (is_dynamic) { engine.AddDynamicGeometry(sphere, id); } else { @@ -186,7 +185,7 @@ GTEST_TEST(ProximityEngineTests, ExceptionTwoObjectsInObjFileForConvex) { GTEST_TEST(ProximityEngineTests, CopySemantics) { ProximityEngine ref_engine; Sphere sphere{0.5}; - Isometry3 pose = Isometry3d::Identity(); + RigidTransformd pose = RigidTransformd::Identity(); // NOTE: The GeometryId values are all lies; the values are arbitrary but // do not matter in the context of this test. @@ -220,7 +219,7 @@ GTEST_TEST(ProximityEngineTests, CopySemantics) { GTEST_TEST(ProximityEngineTests, MoveSemantics) { ProximityEngine engine; Sphere sphere{0.5}; - Isometry3 pose = Isometry3d::Identity(); + RigidTransformd pose = RigidTransformd::Identity(); engine.AddAnchoredGeometry(sphere, pose, GeometryId::get_new_id()); engine.AddDynamicGeometry(sphere, GeometryId::get_new_id()); @@ -248,7 +247,7 @@ GTEST_TEST(ProximityEngineTests, MoveSemantics) { // A scene with no geometry reports no witness pairs. GTEST_TEST(ProximityEngineTests, SignedDistanceClosestPointsOnEmptyScene) { ProximityEngine engine; - const unordered_map X_WGs; + const unordered_map X_WGs; const auto results = engine.ComputeSignedDistancePairwiseClosestPoints(X_WGs, kInf); @@ -261,8 +260,8 @@ GTEST_TEST(ProximityEngineTests, SignedDistanceClosestPointsSingleAnchored) { Sphere sphere{0.5}; const GeometryId id = GeometryId::get_new_id(); - const unordered_map X_WGs{ - {id, Isometry3d::Identity()}}; + const unordered_map X_WGs{ + {id, RigidTransformd::Identity()}}; engine.AddAnchoredGeometry(sphere, X_WGs.at(id), id); const auto results = @@ -273,16 +272,16 @@ GTEST_TEST(ProximityEngineTests, SignedDistanceClosestPointsSingleAnchored) { // Tests that anchored geometry don't report closest distance with each other. GTEST_TEST(ProximityEngineTests, SignedDistanceClosestPointsMultipleAnchored) { ProximityEngine engine; - unordered_map X_WGs; + unordered_map X_WGs; const double radius = 0.5; Sphere sphere{radius}; const GeometryId id_A = GeometryId::get_new_id(); - X_WGs[id_A] = Isometry3d::Identity(); + X_WGs[id_A] = RigidTransformd::Identity(); engine.AddAnchoredGeometry(sphere, X_WGs.at(id_A), id_A); const GeometryId id_B = GeometryId::get_new_id(); - X_WGs[id_B] = Isometry3d{Translation3d{1.8 * radius, 0, 0}}; + X_WGs[id_B] = RigidTransformd{Translation3d{1.8 * radius, 0, 0}}; engine.AddAnchoredGeometry(sphere, X_WGs.at(id_B), id_B); const auto results = @@ -297,8 +296,8 @@ GTEST_TEST(ProximityEngineTests, SignedDistanceClosestPointsMaxDistance) { ProximityEngine engine; const GeometryId id_A = GeometryId::get_new_id(); const GeometryId id_B = GeometryId::get_new_id(); - unordered_map X_WGs{{id_A, Isometry3d::Identity()}, - {id_B, Isometry3d::Identity()}}; + unordered_map X_WGs{ + {id_A, RigidTransformd::Identity()}, {id_B, RigidTransformd::Identity()}}; const double radius = 0.5; Sphere sphere{radius}; @@ -313,7 +312,7 @@ GTEST_TEST(ProximityEngineTests, SignedDistanceClosestPointsMaxDistance) { { const Vector3d p_WB = Vector3d(2, 3, 4).normalized() * (kCenterDistance - kEps); - X_WGs[id_B].translation() = p_WB; + X_WGs[id_B].set_translation(p_WB); engine.UpdateWorldPoses(X_WGs); const auto results = engine.ComputeSignedDistancePairwiseClosestPoints(X_WGs, kMaxDistance); @@ -324,7 +323,7 @@ GTEST_TEST(ProximityEngineTests, SignedDistanceClosestPointsMaxDistance) { { const Vector3d p_WB = Vector3d(2, 3, 4).normalized() * (kCenterDistance + kEps); - X_WGs[id_B].translation() = p_WB; + X_WGs[id_B].set_translation(p_WB); engine.UpdateWorldPoses(X_WGs); const auto results = engine.ComputeSignedDistancePairwiseClosestPoints(X_WGs, kMaxDistance); @@ -348,9 +347,11 @@ GTEST_TEST(ProximityEngineTests, SignedDistanceToPointNonPositiveThreshold) { const GeometryId id_A = GeometryId::get_new_id(); const GeometryId id_B = GeometryId::get_new_id(); - const unordered_map X_WGs{ - {id_A, Isometry3d{Translation3d{-kRadius + 0.5 * kPenetration, 0, 0}}}, - {id_B, Isometry3d{Translation3d{kRadius - 0.5 * kPenetration, 0, 0}}}}; + const unordered_map X_WGs{ + {id_A, + RigidTransformd{Translation3d{-kRadius + 0.5 * kPenetration, 0, 0}}}, + {id_B, + RigidTransformd{Translation3d{kRadius - 0.5 * kPenetration, 0, 0}}}}; Sphere sphere{kRadius}; engine.AddDynamicGeometry(sphere, id_A); @@ -397,12 +398,12 @@ GTEST_TEST(ProximityEngineTests, SignedDistanceToPointNonPositiveThreshold) { // GTEST_TEST(SignedDistanceToPointBroadphaseTest, MultipleThreshold) { ProximityEngine engine; - unordered_map X_WGs; + unordered_map X_WGs; const double radius = 0.1; const Vector3d center1(1., 1., 1.); const Vector3d center2(-1, -1, -1.); for (const Vector3d& p_WG : {center1, center2}) { - const Isometry3d X_WG(Translation3d{p_WG}); + const RigidTransformd X_WG(Translation3d{p_WG}); const GeometryId id = GeometryId::get_new_id(); X_WGs[id] = X_WG; engine.AddAnchoredGeometry(Sphere(radius), X_WG, id); @@ -1080,7 +1081,7 @@ std::vector GenDistTestDataHalfSpace() { const bool well_defined_grad = true; for (const auto& X_WG : {RigidTransformd(), X_WG1}) { - const auto& R_WG = X_WG.linear(); + const auto& R_WG = X_WG.rotation(); const Vector3d grad_W = R_WG.col(2); const Vector3d p_GN(1.25, 1.5, 0); const Vector3d p_WN = X_WG * p_GN; @@ -1113,7 +1114,7 @@ std::vector GenDistTestDataHalfSpace() { struct SignedDistanceToPointTest : public testing::TestWithParam { ProximityEngine engine; - unordered_map X_WGs; + unordered_map X_WGs; // The tolerance value for determining equivalency between expected and // tested results. The underlying algorithms have an empirically-determined, @@ -1124,8 +1125,8 @@ struct SignedDistanceToPointTest SignedDistanceToPointTest() { const auto& data = GetParam(); const GeometryId id = data.expected_result.id_G; - engine.AddAnchoredGeometry(*data.geometry, data.X_WG.GetAsIsometry3(), id); - X_WGs[id] = data.X_WG.GetAsIsometry3(); + engine.AddAnchoredGeometry(*data.geometry, data.X_WG, id); + X_WGs[id] = data.X_WG; } }; @@ -1234,7 +1235,7 @@ GTEST_TEST(ProximityEngineTests, PenetrationSingleAnchored) { ProximityEngine engine; Sphere sphere{0.5}; - Isometry3 pose = Isometry3::Identity(); + RigidTransformd pose = RigidTransformd::Identity(); const GeometryId id = GeometryId::get_new_id(); engine.AddAnchoredGeometry(sphere, pose, id); auto results = engine.ComputePointPairPenetration(); @@ -1248,9 +1249,9 @@ GTEST_TEST(ProximityEngineTests, PenetrationMultipleAnchored) { const double radius = 0.5; Sphere sphere{radius}; - Isometry3 pose = Isometry3::Identity(); + RigidTransformd pose = RigidTransformd::Identity(); engine.AddAnchoredGeometry(sphere, pose, GeometryId::get_new_id()); - pose.translation() << 1.8 * radius, 0, 0; + pose.set_translation({1.8 * radius, 0, 0}); engine.AddAnchoredGeometry(sphere, pose, GeometryId::get_new_id()); auto results = engine.ComputePointPairPenetration(); EXPECT_EQ(results.size(), 0); @@ -1308,21 +1309,19 @@ class SimplePenetrationTest : public ::testing::Test { DRAKE_DEMAND(engine->num_geometries() == static_cast(X_WGs_.size())); const double x_pos = is_colliding ? colliding_x_ : free_x_; - X_WGs_[id] = Isometry3(Translation3d{x_pos, 0, 0}); + X_WGs_[id].set_translation({x_pos, 0, 0}); engine->UpdateWorldPoses(X_WGs_); } // Poses have been defined as doubles; get them in the required scalar type. template - unordered_map> GetTypedPoses() const { - unordered_map> typed_X_WG; + unordered_map> GetTypedPoses() const { + unordered_map> typed_X_WG; for (const auto& id_pose_pair : X_WGs_) { const GeometryId id = id_pose_pair.first; - const Isometry3d& X_WG = id_pose_pair.second; - Isometry3 X_WG_t; - X_WG_t.matrix() = X_WG.matrix(); - typed_X_WG[id] = X_WG_t; + const RigidTransformd& X_WG = id_pose_pair.second; + typed_X_WG[id] = X_WG.cast(); } return typed_X_WG; } @@ -1454,7 +1453,7 @@ class SimplePenetrationTest : public ::testing::Test { } ProximityEngine engine_; - unordered_map X_WGs_; + unordered_map X_WGs_; const double radius_{0.5}; const Sphere sphere_{radius_}; const double free_x_{2.5 * radius_}; @@ -1465,7 +1464,7 @@ class SimplePenetrationTest : public ::testing::Test { // case *not* colliding. TEST_F(SimplePenetrationTest, PenetrationDynamicAndAnchored) { // Set up anchored geometry - Isometry3 pose = Isometry3::Identity(); + RigidTransformd pose = RigidTransformd::Identity(); const GeometryId anchored_id = GeometryId::get_new_id(); engine_.AddAnchoredGeometry(sphere_, pose, anchored_id); @@ -1475,7 +1474,7 @@ TEST_F(SimplePenetrationTest, PenetrationDynamicAndAnchored) { EXPECT_EQ(engine_.num_geometries(), 2); X_WGs_[anchored_id] = pose; - X_WGs_[dynamic_id] = Isometry3d::Identity(); + X_WGs_[dynamic_id] = RigidTransformd::Identity(); // Non-colliding case MoveDynamicSphere(dynamic_id, false /* not colliding */); @@ -1505,8 +1504,8 @@ TEST_F(SimplePenetrationTest, PenetrationDynamicAndDynamicSingleSource) { engine_.AddDynamicGeometry(sphere_, collide_id); EXPECT_EQ(engine_.num_geometries(), 2); - X_WGs_[origin_id] = Isometry3d::Identity(); - X_WGs_[collide_id] = Isometry3d::Identity(); + X_WGs_[origin_id] = RigidTransformd::Identity(); + X_WGs_[collide_id] = RigidTransformd::Identity(); // Non-colliding case MoveDynamicSphere(collide_id, false /* not colliding */); @@ -1535,7 +1534,7 @@ TEST_F(SimplePenetrationTest, ExcludeCollisionsWithinCliqueGeneration) { const GeometryId id_dynamic2 = GeometryId::get_new_id(); engine_.AddDynamicGeometry(sphere_, id_dynamic2); - Isometry3 pose{Isometry3::Identity()}; + RigidTransformd pose = RigidTransformd::Identity(); const GeometryId id_anchored1 = GeometryId::get_new_id(); engine_.AddAnchoredGeometry(sphere_, pose, id_anchored1); const GeometryId id_anchored2 = GeometryId::get_new_id(); @@ -1592,8 +1591,8 @@ TEST_F(SimplePenetrationTest, ExcludeCollisionsWithin) { engine_.AddDynamicGeometry(sphere_, collide_id); EXPECT_EQ(engine_.num_geometries(), 2); - X_WGs_[origin_id] = Isometry3d::Identity(); - X_WGs_[collide_id] = Isometry3d::Identity(); + X_WGs_[origin_id] = RigidTransformd::Identity(); + X_WGs_[collide_id] = RigidTransformd::Identity(); EXPECT_FALSE(engine_.CollisionFiltered(origin_id, true, collide_id, true)); engine_.ExcludeCollisionsWithin({origin_id, collide_id}, {}); @@ -1628,7 +1627,7 @@ TEST_F(SimplePenetrationTest, ExcludeCollisionsBetweenCliqueGeneration) { GeometryId dynamic3 = GeometryId::get_new_id(); engine_.AddDynamicGeometry(sphere_, dynamic3); - Isometry3 pose{Isometry3::Identity()}; + RigidTransformd pose = RigidTransformd::Identity(); GeometryId anchored1 = GeometryId::get_new_id(); engine_.AddAnchoredGeometry(sphere_, pose, anchored1); GeometryId anchored2 = GeometryId::get_new_id(); @@ -1692,8 +1691,8 @@ TEST_F(SimplePenetrationTest, ExcludeCollisionsBetween) { EXPECT_TRUE(engine_.CollisionFiltered(origin_id, true, collide_id, true)); - X_WGs_[origin_id] = Isometry3d::Identity(); - X_WGs_[collide_id] = Isometry3d::Identity(); + X_WGs_[origin_id] = RigidTransformd::Identity(); + X_WGs_[collide_id] = RigidTransformd::Identity(); // Non-colliding case MoveDynamicSphere(collide_id, false /* not colliding */); @@ -1724,10 +1723,10 @@ GTEST_TEST(ProximityEngineTests, PairwiseSignedDistanceNonPositiveThreshold) { const GeometryId id2 = GeometryId::get_new_id(); const GeometryId id3 = GeometryId::get_new_id(); const double kRadius = 0.5; - const unordered_map X_WGs{ - {id1, Isometry3d{Translation3d{0, 2 * kRadius, 0}}}, - {id2, Isometry3d{Translation3d{-kRadius * 0.9, 0, 0}}}, - {id3, Isometry3d{Translation3d{kRadius * 0.9, 0, 0}}}}; + const unordered_map X_WGs{ + {id1, RigidTransformd{Translation3d{0, 2 * kRadius, 0}}}, + {id2, RigidTransformd{Translation3d{-kRadius * 0.9, 0, 0}}}, + {id3, RigidTransformd{Translation3d{kRadius * 0.9, 0, 0}}}}; Sphere sphere{kRadius}; engine.AddDynamicGeometry(sphere, id1); @@ -2348,7 +2347,7 @@ GenDistPairTestHalfspaceSphere() { const RotationMatrixd R_WS; for (const auto& X_WH : {RigidTransformd(), X_WG1}) { - const auto& R_WH = X_WH.linear(); + const auto& R_WH = X_WH.rotation(); // Half space's outward normal is in the direction of the z-axis of the // half space's canonical frame H. const Vector3d nhat_HS_W = R_WH.col(2); @@ -2403,19 +2402,19 @@ class SignedDistancePairTest public: SignedDistancePairTest() { const auto& data = GetParam(); - engine_.AddAnchoredGeometry(*data.a_, data.X_WA_.GetAsIsometry3(), + engine_.AddAnchoredGeometry(*data.a_, data.X_WA_, data.expected_result_.id_A); engine_.AddDynamicGeometry(*(data.b_), data.expected_result_.id_B); - X_WGs_[data.expected_result_.id_A] = data.X_WA_.GetAsIsometry3(); - X_WGs_[data.expected_result_.id_B] = data.X_WB_.GetAsIsometry3(); + X_WGs_[data.expected_result_.id_A] = data.X_WA_; + X_WGs_[data.expected_result_.id_B] = data.X_WB_; engine_.UpdateWorldPoses(X_WGs_); } protected: ProximityEngine engine_; - unordered_map X_WGs_; + unordered_map X_WGs_; public: // The tolerance value for determining equivalency between expected and @@ -2512,14 +2511,14 @@ GTEST_TEST(SignedDistancePairError, HalfspaceException) { ProximityEngine engine; // NOTE: It's not necessary to put any poses into X_WGs; they never get // evaluated due to the error condition. - unordered_map X_WGs; + unordered_map X_WGs; // We can't use sphere, because sphere-halfspace is supported. Box box{0.5, 0.25, 0.75}; engine.AddDynamicGeometry(box, GeometryId::get_new_id()); HalfSpace halfspace; - engine.AddAnchoredGeometry(halfspace, Isometry3d::Identity(), + engine.AddAnchoredGeometry(halfspace, RigidTransformd::Identity(), GeometryId::get_new_id()); DRAKE_EXPECT_THROWS_MESSAGE( @@ -2773,8 +2772,9 @@ GTEST_TEST(ProximityEngineCollisionTest, SpherePunchThroughBox) { const GeometryId sphere_id = GeometryId::get_new_id(); engine.AddDynamicGeometry(Sphere{radius}, sphere_id); - unordered_map poses{ - {box_id, Isometry3d::Identity()}, {sphere_id, Isometry3d::Identity()}}; + unordered_map poses{ + {box_id, RigidTransformd::Identity()}, + {sphere_id, RigidTransformd::Identity()}}; // clang-format off std::vector test_data{ // In non-penetration, contact_normal and depth values don't matter; they @@ -2791,7 +2791,7 @@ GTEST_TEST(ProximityEngineCollisionTest, SpherePunchThroughBox) { {-eps, 0, 0}, 1, {1, 0, 0}, radius + half_w - eps}}; // clang-format on for (const auto& test : test_data) { - poses[sphere_id].translation() = test.sphere_pose; + poses[sphere_id].set_translation(test.sphere_pose); engine.UpdateWorldPoses(poses); std::vector> results = engine.ComputePointPairPenetration(); @@ -2947,8 +2947,8 @@ class BoxPenetrationTest : public ::testing::Test { ASSERT_EQ(engine_.num_dynamic(), 2); // Update the poses of the geometry. - unordered_map poses{ - {tangent_id, shape_pose(shape_type)}, {box_id, X_WB.GetAsIsometry3()}}; + unordered_map poses{ + {tangent_id, shape_pose(shape_type)}, {box_id, X_WB}}; engine_.UpdateWorldPoses(poses); std::vector> results = engine_.ComputePointPairPenetration(); @@ -3064,24 +3064,24 @@ class BoxPenetrationTest : public ::testing::Test { } // Map enumeration to tangent pose. - Isometry3d shape_pose(TangentShape shape) { - Isometry3d pose = Isometry3d::Identity(); + RigidTransformd shape_pose(TangentShape shape) { + RigidTransformd pose = RigidTransformd::Identity(); switch (shape) { case TangentPlane: break; // leave it at the identity case TangentSphere: - pose.translation() = Vector3d{0, 0, -kRadius}; + pose.set_translation({0, 0, -kRadius}); break; case TangentBox: // The tangent convex is a cube of the same size as the tangent box. // That is why we give them the same pose. case TangentConvex: case TangentStandingCylinder: - pose.translation() = Vector3d{0, 0, -kLength / 2}; + pose.set_translation({0, 0, -kLength / 2}); break; case TangentProneCylinder: - pose = AngleAxisd{M_PI_2, Vector3d::UnitX()}; - pose.translation() = Vector3d{0, 0, -kRadius}; + pose = RigidTransformd(AngleAxisd{M_PI_2, Vector3d::UnitX()}, + Vector3d{0, 0, -kRadius}); break; } return pose; @@ -3188,7 +3188,7 @@ GTEST_TEST(ProximityEngineTests, AddAnchoredMesh) { ProximityEngine engine; Mesh mesh{"invalid/path/thing.obj", 1.0}; DRAKE_EXPECT_THROWS_MESSAGE( - engine.AddAnchoredGeometry(mesh, Isometry3d::Identity(), + engine.AddAnchoredGeometry(mesh, RigidTransformd::Identity(), GeometryId::get_new_id()), std::exception, ".*The proximity engine does not support meshes yet.*"); @@ -3219,22 +3219,13 @@ GTEST_TEST(ProximityEngineTests, Issue10577Regression_Osculation) { engine.AddDynamicGeometry(Box(0.49, 0.63, 0.015), id_A); engine.AddDynamicGeometry(Cylinder(0.08, 0.002), id_B); - Isometry3 X_WA; // Original translation was p_WA = (-0.145, -0.63, 0.2425) and // p_WB = (0, -0.6, 0.251), respectively. - // clang-format off - X_WA.matrix() << 0, -1, 0, -0.25, - 1, 0, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1; - Isometry3 X_WB; - X_WB.matrix() << 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0.0085, - 0, 0, 0, 1; - // clang-format on - const unordered_map> X_WG{{id_A, X_WA}, - {id_B, X_WB}}; + RigidTransformd X_WA(Eigen::AngleAxisd{M_PI_2, Vector3d::UnitZ()}, + Vector3d{-0.25, 0, 0}); + RigidTransformd X_WB(Vector3d{0, 0, 0.0085}); + const unordered_map X_WG{{id_A, X_WA}, + {id_B, X_WB}}; engine.UpdateWorldPoses(X_WG); std::vector geometry_map{id_A, id_B}; auto pairs = engine.ComputePointPairPenetration(); @@ -3253,10 +3244,10 @@ GTEST_TEST(ProximityEngineTests, AnchoredBroadPhaseInitialization) { GeometryId id_A = GeometryId::get_new_id(); engine.AddDynamicGeometry(Sphere(0.5), id_D); - Isometry3d X_WA{Translation3d{-3, 0, 0}}; + RigidTransformd X_WA{Translation3d{-3, 0, 0}}; engine.AddAnchoredGeometry(Sphere(0.5), X_WA, id_A); - Isometry3d X_WD{Translation3d{-3, 0.75, 0}}; + RigidTransformd X_WD{Translation3d{-3, 0.75, 0}}; // NOTE: We only update the dynamic geometries, so we simply provide a map // containing the ids of the dynamic geometries with their pose as a cheat. engine.UpdateWorldPoses({{id_D, X_WD}}); @@ -3289,7 +3280,7 @@ GTEST_TEST(ProximityEngineTests, ComputePointSignedDistanceAutoDiffAnchored) { // An empty world inherently produces no results. { - const unordered_map> X_WGs; + const unordered_map> X_WGs; const auto results = engine.ComputeSignedDistanceToPoint(p_WQ_ad, X_WGs); EXPECT_EQ(results.size(), 0); } @@ -3300,7 +3291,7 @@ GTEST_TEST(ProximityEngineTests, ComputePointSignedDistanceAutoDiffAnchored) { const GeometryId anchored_id = GeometryId::get_new_id(); engine.AddAnchoredGeometry(Sphere(0.7), X_WS, anchored_id); const RigidTransform X_WS_ad = X_WS.cast(); - const unordered_map> X_WGs{ + const unordered_map> X_WGs{ {anchored_id, X_WS_ad}}; // Distance is just beyond the threshold. @@ -3350,9 +3341,9 @@ GTEST_TEST(ProximityEngineTests, ComputePointSignedDistanceAutoDiffDynamic) { // Against a dynamic sphere. const GeometryId id = GeometryId::get_new_id(); engine.AddDynamicGeometry(Sphere(0.7), id); - const auto X_WS_ad = - RigidTransformd(p_WS).cast().GetAsIsometry3(); - const unordered_map> X_WGs{{id, X_WS_ad}}; + const auto X_WS_ad = RigidTransformd(p_WS).cast(); + const unordered_map> X_WGs{ + {id, X_WS_ad}}; engine.UpdateWorldPoses(X_WGs); // Distance is just beyond the threshold. @@ -3405,10 +3396,10 @@ GTEST_TEST(ProximityEngineTests, ComputePairwiseSignedDistanceAutoDiff) { const GeometryId id2 = GeometryId::get_new_id(); engine.AddDynamicGeometry(Sphere(radius), id2); const auto X_WS2_ad = - RigidTransformd(p_WS).cast().GetAsIsometry3(); + RigidTransformd(p_WS).cast(); - const unordered_map> X_WGs{{id1, X_WS1_ad}, - {id2, X_WS2_ad}}; + const unordered_map> X_WGs{ + {id1, X_WS1_ad}, {id2, X_WS2_ad}}; engine.UpdateWorldPoses(X_WGs); std::vector> results = @@ -3437,10 +3428,9 @@ GTEST_TEST(ProximityEngineTests, engine.AddDynamicGeometry(Box(1, 2, 3), id1); engine.AddDynamicGeometry(Box(2, 4, 6), id2); - Eigen::Isometry3d a; - const unordered_map> X_WGs{ - {id1, Isometry3::Identity()}, - {id2, Isometry3::Identity()}}; + const unordered_map> X_WGs{ + {id1, RigidTransform::Identity()}, + {id2, RigidTransform::Identity()}}; DRAKE_EXPECT_THROWS_MESSAGE( engine.ComputeSignedDistancePairwiseClosestPoints(X_WGs, kInf), std::logic_error, diff --git a/geometry/test/query_object_test.cc b/geometry/test/query_object_test.cc index 5da9c0ececf2..6356e752b961 100644 --- a/geometry/test/query_object_test.cc +++ b/geometry/test/query_object_test.cc @@ -9,6 +9,7 @@ #include "drake/geometry/geometry_frame.h" #include "drake/geometry/geometry_instance.h" #include "drake/geometry/scene_graph.h" +#include "drake/math/rigid_transform.h" namespace drake { namespace geometry { @@ -105,8 +106,9 @@ class QueryObjectTester { namespace { -using render::DepthCameraProperties; +using Eigen::Vector3d; using math::RigidTransformd; +using render::DepthCameraProperties; using std::make_unique; using std::unique_ptr; using systems::Context; @@ -213,7 +215,7 @@ TEST_F(QueryObjectTest, DefaultQueryThrows) { GTEST_TEST(QueryObjectInspectTest, CreateValidInspector) { SceneGraph scene_graph; SourceId source_id = scene_graph.RegisterSource("source"); - auto identity = Isometry3::Identity(); + auto identity = RigidTransformd::Identity(); FrameId frame_id = scene_graph.RegisterFrame(source_id, GeometryFrame("frame")); GeometryId geometry_id = scene_graph.RegisterGeometry( @@ -241,7 +243,7 @@ GTEST_TEST(QueryObjectBakeTest, BakedCopyHasFullUpdate) { SourceId s_id = scene_graph.RegisterSource("BakeTest"); FrameId frame_id = scene_graph.RegisterFrame(s_id, GeometryFrame("frame")); unique_ptr> context = scene_graph.AllocateContext(); - Isometry3 X_WF{Translation3{1, 2, 3}}; + RigidTransformd X_WF{Vector3d{1, 2, 3}}; FramePoseVector poses{{frame_id, X_WF}}; scene_graph.get_source_pose_port(s_id).FixValue(context.get(), poses); const auto& query_object = @@ -257,13 +259,15 @@ GTEST_TEST(QueryObjectBakeTest, BakedCopyHasFullUpdate) { const GeometryState& state = QOT::state(query_object); const auto& stale_pose = state.get_pose_in_world(frame_id); // Confirm the live state hasn't been updated yet. - EXPECT_FALSE(CompareMatrices(stale_pose.matrix(), X_WF.matrix())); + EXPECT_FALSE( + CompareMatrices(stale_pose.GetAsMatrix34(), X_WF.GetAsMatrix34())); const QueryObject baked(query_object); const GeometryState& baked_state = QOT::state(query_object); const auto& baked_pose = baked_state.get_pose_in_world(frame_id); - EXPECT_TRUE(CompareMatrices(baked_pose.matrix(), X_WF.matrix())); + EXPECT_TRUE( + CompareMatrices(baked_pose.GetAsMatrix34(), X_WF.GetAsMatrix34())); } } // namespace diff --git a/geometry/test/scene_graph_inspector_test.cc b/geometry/test/scene_graph_inspector_test.cc index 203fbdff6360..fb9df719299d 100644 --- a/geometry/test/scene_graph_inspector_test.cc +++ b/geometry/test/scene_graph_inspector_test.cc @@ -27,6 +27,7 @@ class SceneGraphInspectorTester { namespace { +using math::RigidTransformd; using std::make_unique; // Simply exercises all the methods to confirm there's no build or execution @@ -77,7 +78,7 @@ GTEST_TEST(SceneGraphInspector, ExerciseEverything) { const GeometryId geometry_id = tester.mutable_state().RegisterGeometry( source_id, frame_id, - make_unique(Isometry3::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1.0), "sphere")); inspector.GetGeometryIdByName(frame_id, Role::kUnassigned, "sphere"); @@ -87,8 +88,13 @@ GTEST_TEST(SceneGraphInspector, ExerciseEverything) { inspector.GetFrameId(geometry_id); inspector.GetName(geometry_id); inspector.GetShape(geometry_id); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" inspector.X_PG(geometry_id); inspector.X_FG(geometry_id); +#pragma GCC diagnostic pop + inspector.GetPoseInParent(geometry_id); + inspector.GetPoseInFrame(geometry_id); inspector.GetProximityProperties(geometry_id); inspector.GetIllustrationProperties(geometry_id); inspector.GetPerceptionProperties(geometry_id); @@ -97,7 +103,7 @@ GTEST_TEST(SceneGraphInspector, ExerciseEverything) { const GeometryId geometry_id2 = tester.mutable_state().RegisterGeometry( source_id, frame_id, - make_unique(Isometry3::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1.0), "sphere2")); tester.mutable_state().AssignRole(source_id, geometry_id, ProximityProperties()); diff --git a/geometry/test/scene_graph_test.cc b/geometry/test/scene_graph_test.cc index 857fb5b0efc0..b9f3b46aa409 100644 --- a/geometry/test/scene_graph_test.cc +++ b/geometry/test/scene_graph_test.cc @@ -28,8 +28,8 @@ namespace drake { namespace geometry { -using Eigen::Isometry3d; using internal::DummyRenderEngine; +using math::RigidTransformd; using systems::Context; using systems::rendering::PoseBundle; using systems::System; @@ -90,7 +90,7 @@ namespace { // Convenience function for making a geometry instance. std::unique_ptr make_sphere_instance( double radius = 1.0) { - return make_unique(Isometry3::Identity(), + return make_unique(RigidTransformd::Identity(), make_unique(radius), "sphere"); } @@ -377,15 +377,15 @@ TEST_F(SceneGraphTest, ModelInspector) { // affixed to different nodes, that should be alright. GeometryId anchored_id = scene_graph_.RegisterAnchoredGeometry( source_id, - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1.0), "sphere")); GeometryId sphere_1 = scene_graph_.RegisterGeometry( source_id, frame_1, - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1.0), "sphere")); GeometryId sphere_2 = scene_graph_.RegisterGeometry( source_id, frame_2, - make_unique(Isometry3d::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1.0), "sphere")); const SceneGraphInspector& inspector = scene_graph_.model_inspector(); @@ -425,7 +425,7 @@ TEST_F(SceneGraphTest, RoleManagementSmokeTest) { SourceId s_id = scene_graph_.RegisterSource("test"); FrameId f_id = scene_graph_.RegisterFrame(s_id, GeometryFrame("frame")); auto instance = make_unique( - Isometry3::Identity(), make_unique(1.0), "sphere"); + RigidTransformd::Identity(), make_unique(1.0), "sphere"); instance->set_illustration_properties(IllustrationProperties()); instance->set_proximity_properties(ProximityProperties()); instance->set_perception_properties(PerceptionProperties()); @@ -471,7 +471,7 @@ class GeometrySourceSystem : public systems::LeafSystem { } // Method used to bring frame ids and poses out of sync. Adds a pose in // addition to all of the default poses. - void add_extra_pose() { extra_poses_.push_back(Isometry3()); } + void add_extra_pose() { extra_poses_.push_back(RigidTransformd()); } private: // Populate with the pose data. @@ -481,7 +481,7 @@ class GeometrySourceSystem : public systems::LeafSystem { const int base_count = static_cast(frame_ids_.size()); for (int i = 0; i < base_count; ++i) { - poses->set_value(frame_ids_[i], Isometry3::Identity()); + poses->set_value(frame_ids_[i], RigidTransformd::Identity()); } const int extra_count = static_cast(extra_frame_ids_.size()); @@ -494,7 +494,7 @@ class GeometrySourceSystem : public systems::LeafSystem { SourceId source_id_; std::vector frame_ids_; std::vector extra_frame_ids_; - std::vector> extra_poses_; + std::vector extra_poses_; }; // Simple test case; system registers frames and provides correct connections. @@ -600,7 +600,7 @@ GTEST_TEST(SceneGraphVisualizationTest, NoWorldInPoseVector) { SourceId s_id = scene_graph.RegisterSource("dummy"); scene_graph.RegisterGeometry( s_id, scene_graph.world_frame_id(), - make_unique(Isometry3::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1.0), "sphere")); PoseBundle poses = SceneGraphTester::MakePoseBundle(scene_graph); EXPECT_EQ(0, poses.get_num_poses()); @@ -617,7 +617,7 @@ GTEST_TEST(SceneGraphVisualizationTest, NoWorldInPoseVector) { SourceId s_id = scene_graph.RegisterSource("dummy"); scene_graph.RegisterGeometry( s_id, scene_graph.world_frame_id(), - make_unique(Isometry3::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1.0), "sphere")); FrameId f_id = scene_graph.RegisterFrame(s_id, GeometryFrame("f")); PoseBundle poses = SceneGraphTester::MakePoseBundle(scene_graph); @@ -626,7 +626,7 @@ GTEST_TEST(SceneGraphVisualizationTest, NoWorldInPoseVector) { EXPECT_EQ(0, poses.get_num_poses()); auto context = scene_graph.AllocateContext(); const FramePoseVector pose_vector{{ - f_id, Isometry3::Identity()}}; + f_id, RigidTransformd::Identity()}}; scene_graph.get_source_pose_port(s_id).FixValue(context.get(), pose_vector); EXPECT_NO_THROW( SceneGraphTester::CalcPoseBundle(scene_graph, *context, &poses)); @@ -639,12 +639,12 @@ GTEST_TEST(SceneGraphVisualizationTest, NoWorldInPoseVector) { SourceId s_id = scene_graph.RegisterSource("dummy"); scene_graph.RegisterGeometry( s_id, scene_graph.world_frame_id(), - make_unique(Isometry3::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1.0), "sphere")); FrameId f_id = scene_graph.RegisterFrame(s_id, GeometryFrame("f")); scene_graph.RegisterGeometry( s_id, f_id, - make_unique(Isometry3::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1.0), "sphere")); PoseBundle poses = SceneGraphTester::MakePoseBundle(scene_graph); // The dynamic geometry has no illustration role, so it doesn't lead the @@ -652,7 +652,7 @@ GTEST_TEST(SceneGraphVisualizationTest, NoWorldInPoseVector) { EXPECT_EQ(0, poses.get_num_poses()); auto context = scene_graph.AllocateContext(); const FramePoseVector pose_vector{{ - f_id, Isometry3::Identity()}}; + f_id, RigidTransformd::Identity()}}; scene_graph.get_source_pose_port(s_id).FixValue(context.get(), pose_vector); EXPECT_NO_THROW(SceneGraphTester::CalcPoseBundle(scene_graph, *context, &poses)); @@ -782,7 +782,7 @@ GTEST_TEST(SceneGraphRenderTest, AddRenderer) { SourceId s_id = scene_graph.RegisterSource("dummy"); scene_graph.RegisterGeometry( s_id, scene_graph.world_frame_id(), - make_unique(Isometry3::Identity(), + make_unique(RigidTransformd::Identity(), make_unique(1.0), "sphere")); EXPECT_NO_THROW( diff --git a/geometry/test/shape_specification_test.cc b/geometry/test/shape_specification_test.cc index 4e16a500fa3f..dcefc8e6e783 100644 --- a/geometry/test/shape_specification_test.cc +++ b/geometry/test/shape_specification_test.cc @@ -11,6 +11,7 @@ namespace drake { namespace geometry { namespace { +using math::RigidTransformd; using std::unique_ptr; // Confirm correct interactions with the Reifier. @@ -152,61 +153,64 @@ TEST_F(ReifierTest, CloningShapes) { // that the pose conforms to expectations. Also confirms that the rotational // component is orthonormal. ::testing::AssertionResult ValidatePlanePose( - const Eigen::Isometry3d& pose, const Vector3& expected_z, + const RigidTransformd& pose, const Vector3& expected_z, const Vector3& expected_translation, double tolerance = 1e-14) { using std::abs; // Test expected z-axis value. - const Vector3& z_axis = pose.linear().col(2); + const Vector3& z_axis = pose.rotation().col(2); if (!CompareMatrices(z_axis, expected_z, tolerance, MatrixCompareType::absolute)) { return ::testing::AssertionFailure() - << "pose =\n" - << pose.matrix() << "\nExpected z-axis " << expected_z.transpose() - << " does not match pose's z-axis " << z_axis.transpose(); + << "pose =\n" + << pose.GetAsMatrix34() << "\nExpected z-axis " + << expected_z.transpose() << " does not match pose's z-axis " + << z_axis.transpose(); } // Test expected translation. if (!CompareMatrices(pose.translation(), expected_translation, tolerance, MatrixCompareType::absolute)) { return ::testing::AssertionFailure() - << "pose =\n" - << pose.matrix() << "\nExpected translation " - << expected_translation.transpose() - << " does not match pose's translation " - << pose.translation().transpose(); + << "pose =\n" + << pose.GetAsMatrix34() << "\nExpected translation " + << expected_translation.transpose() + << " does not match pose's translation " + << pose.translation().transpose(); } // Test unit-length rotation. char axis_labels[] = {'x', 'y', 'z'}; for (int i = 0; i < 3; ++i) { - if (abs(pose.linear().col(i).norm() - 1) > tolerance) { + if (abs(pose.rotation().col(i).norm() - 1) > tolerance) { return ::testing::AssertionFailure() - << "pose =\n" - << pose.matrix() << "\ndoes not have unit length " << axis_labels[i] - << "-axis " << pose.linear().col(i).transpose(); + << "pose =\n" + << pose.GetAsMatrix34() << "\ndoes not have unit length " + << axis_labels[i] << "-axis " + << pose.rotation().col(i).transpose(); } } // Test orthogonality. for (int i = 0; i < 2; ++i) { for (int j = i + 1; j < 3; ++j) { - double dot_product = pose.linear().col(i).dot(pose.linear().col(j)); + double dot_product = pose.rotation().col(i).dot(pose.rotation().col(j)); if (abs(dot_product) > tolerance) { return ::testing::AssertionFailure() - << "For pose =\n" - << pose.matrix() << "\nThe " << axis_labels[i] << "-axis and " - << axis_labels[j] << "-axis are not orthogonal"; + << "For pose =\n" + << pose.GetAsMatrix34() << "\nThe " << axis_labels[i] + << "-axis and " << axis_labels[j] << "-axis are not orthogonal"; } } } return ::testing::AssertionSuccess() - << "pose =\n" - << pose.matrix() << "\nhas expected z-axis = " << expected_z.transpose() - << "\nand expected translation = " << expected_translation.transpose(); + << "pose =\n" + << pose.GetAsMatrix34() + << "\nhas expected z-axis = " << expected_z.transpose() + << "\nand expected translation = " << expected_translation.transpose(); } -// Confirms that the pose computed by HalfSpace::MakePose is consistent with +// Confirms that the pose computed by HalfSpace::X_FC() is consistent with // the normal and point provided. GTEST_TEST(HalfSpaceTest, MakePose) { Vector3 n; @@ -219,7 +223,7 @@ GTEST_TEST(HalfSpaceTest, MakePose) { { n << 0, 0, 1; p << 0, 0, 0; - Isometry3 pose = HalfSpace::MakePose(n, p); + RigidTransformd pose = HalfSpace::MakePose(n, p); EXPECT_TRUE(ValidatePlanePose(pose, n, p)); } @@ -227,7 +231,7 @@ GTEST_TEST(HalfSpaceTest, MakePose) { { n << 0, 0, 1; p << 0, 0, 1; - Isometry3 pose = HalfSpace::MakePose(n, p); + RigidTransformd pose = HalfSpace::MakePose(n, p); EXPECT_TRUE(ValidatePlanePose(pose, n, p)); } @@ -236,7 +240,7 @@ GTEST_TEST(HalfSpaceTest, MakePose) { { n << 0, 0, 1; p << 1, 0, 0; - Isometry3 pose = HalfSpace::MakePose(n, p); + RigidTransformd pose = HalfSpace::MakePose(n, p); EXPECT_TRUE(ValidatePlanePose(pose, n, Vector3::Zero())); } @@ -245,7 +249,7 @@ GTEST_TEST(HalfSpaceTest, MakePose) { { n << 0, 0, 1; p << 1, 1, 1; - Isometry3 pose = HalfSpace::MakePose(n, p); + RigidTransformd pose = HalfSpace::MakePose(n, p); p << 0, 0, 1; EXPECT_TRUE(ValidatePlanePose(pose, n, p)); } @@ -255,7 +259,7 @@ GTEST_TEST(HalfSpaceTest, MakePose) { { n << 1, 1, 1; p << 0, 0, 0; - Isometry3 pose = HalfSpace::MakePose(n, p); + RigidTransformd pose = HalfSpace::MakePose(n, p); EXPECT_TRUE(ValidatePlanePose(pose, n.normalized(), p)); } @@ -264,7 +268,7 @@ GTEST_TEST(HalfSpaceTest, MakePose) { { n << 1, 1, 1; p << 1, 1, 1; - Isometry3 pose = HalfSpace::MakePose(n, p); + RigidTransformd pose = HalfSpace::MakePose(n, p); EXPECT_TRUE(ValidatePlanePose(pose, n.normalized(), p)); } @@ -274,7 +278,7 @@ GTEST_TEST(HalfSpaceTest, MakePose) { { n << 1, 1, 1; p << 1, 0, 0; - Isometry3 pose = HalfSpace::MakePose(n, p); + RigidTransformd pose = HalfSpace::MakePose(n, p); p = n / 3.0; EXPECT_TRUE(ValidatePlanePose(pose, n.normalized(), p)); } diff --git a/geometry/test/utilities_test.cc b/geometry/test/utilities_test.cc index da673d5a2967..8b44615e08b9 100644 --- a/geometry/test/utilities_test.cc +++ b/geometry/test/utilities_test.cc @@ -5,12 +5,17 @@ #include #include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/math/rigid_transform.h" +#include "drake/math/roll_pitch_yaw.h" namespace drake { namespace geometry { namespace internal { namespace { +using math::RigidTransform; +using math::RollPitchYaw; + GTEST_TEST(GeometryUtilities, CanonicalizeGeometryName) { // Confirms that the canonical version of the given name is unchanged. auto expect_unchanged = [](const std::string& name) { @@ -54,29 +59,26 @@ 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(); +GTEST_TEST(GeometryUtilities, RigidTransformConversion) { + RigidTransform X_AB{ + RollPitchYaw{M_PI / 3, M_PI / 6, M_PI / 7}, + Vector3{1, 2, 3}}; - Isometry3 X_AB_converted = convert_to_double(X_AB); - EXPECT_TRUE(CompareMatrices(X_AB.matrix(), X_AB_converted.matrix())); // Double to double conversion is just a pass through without copying. - const Isometry3& X_AB_converted_ref = convert_to_double(X_AB); + const RigidTransform& X_AB_converted_ref = convert_to_double(X_AB); EXPECT_EQ(&X_AB, &X_AB_converted_ref); - Isometry3 X_AB_ad(X_AB); - Isometry3 X_AB_ad_converted = convert_to_double(X_AB_ad); - EXPECT_TRUE(CompareMatrices(X_AB.matrix(), X_AB_ad_converted.matrix())); + RigidTransform X_AB_ad(X_AB.cast()); + RigidTransform X_AB_ad_converted = convert_to_double(X_AB_ad); + EXPECT_TRUE( + CompareMatrices(X_AB.GetAsMatrix34(), X_AB_ad_converted.GetAsMatrix34())); } GTEST_TEST(GeometryUtilities, Vector3Conversion) { Vector3 p_AB{1, 2, 3}; Vector3 p_AB_converted = convert_to_double(p_AB); - EXPECT_TRUE(CompareMatrices(p_AB.matrix(), p_AB_converted.matrix())); + EXPECT_TRUE(CompareMatrices(p_AB, p_AB_converted)); // Double to double conversion is just a pass through without copying, so // we'll compare addresses. const Vector3& p_AB_converted_ref = convert_to_double(p_AB); @@ -84,7 +86,7 @@ GTEST_TEST(GeometryUtilities, Vector3Conversion) { Vector3 p_AB_ad(p_AB); Vector3 X_AB_ad_converted = convert_to_double(p_AB_ad); - EXPECT_TRUE(CompareMatrices(p_AB.matrix(), X_AB_ad_converted.matrix())); + EXPECT_TRUE(CompareMatrices(p_AB, X_AB_ad_converted)); } } // namespace diff --git a/geometry/utilities.h b/geometry/utilities.h index 58b17df1855a..6c0b35d23b1d 100644 --- a/geometry/utilities.h +++ b/geometry/utilities.h @@ -79,33 +79,14 @@ Vector3 convert_to_double( return result; } -// TODO(SeanCurtis-TRI): Get rid of these when I finally swap for -// RigidTransforms. - -inline const Isometry3& convert_to_double( - const Isometry3& transform) { - return transform; -} - -template -Isometry3 convert_to_double( - 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; -} - // Don't needlessly copy transforms that are already scalar-valued. -inline const math::RigidTransformd& convert(const math::RigidTransformd& X_AB) { +inline const math::RigidTransformd& convert_to_double( + const math::RigidTransformd& X_AB) { return X_AB; } template -math::RigidTransformd convert( +math::RigidTransformd convert_to_double( const math::RigidTransform>& X_AB) { Matrix3 R_converted; Vector3 p_converted; diff --git a/multibody/inverse_kinematics/distance_constraint.cc b/multibody/inverse_kinematics/distance_constraint.cc index 76338f60e977..ca5a4c278502 100644 --- a/multibody/inverse_kinematics/distance_constraint.cc +++ b/multibody/inverse_kinematics/distance_constraint.cc @@ -77,7 +77,11 @@ void EvalDistance(const MultibodyPlant& plant, plant.GetBodyFromFrameId(frame_B_id)->body_frame(); internal::CalcDistanceDerivatives( plant, *context, frameA, frameB, - inspector.X_FG(signed_distance_pair.id_A) * + // GetPoseInFrame() returns RigidTransform -- we can't + // multiply across heterogeneous scalar types; so we cast the double + // to T. + inspector.GetPoseInFrame(signed_distance_pair.id_A) + .template cast() * signed_distance_pair.p_ACa, signed_distance_pair.distance, signed_distance_pair.nhat_BA_W, x, y->data()); diff --git a/multibody/inverse_kinematics/minimum_distance_constraint.cc b/multibody/inverse_kinematics/minimum_distance_constraint.cc index 47a645a4a255..6296d1a2139b 100644 --- a/multibody/inverse_kinematics/minimum_distance_constraint.cc +++ b/multibody/inverse_kinematics/minimum_distance_constraint.cc @@ -49,7 +49,11 @@ VectorX Distances(const MultibodyPlant& plant, plant.GetBodyFromFrameId(frame_B_id)->body_frame(); internal::CalcDistanceDerivatives( plant, *context, frameA, frameB, - inspector.X_FG(signed_distance_pair.id_A) * + // GetPoseInFrame() returns RigidTransform -- we can't + // multiply across heterogeneous scalar types; so we cast the double + // to T. + inspector.GetPoseInFrame(signed_distance_pair.id_A) + .template cast() * signed_distance_pair.p_ACa, signed_distance_pair.distance, signed_distance_pair.nhat_BA_W, q, &distances(distance_count++)); diff --git a/multibody/optimization/manipulator_equation_constraint.cc b/multibody/optimization/manipulator_equation_constraint.cc index b266836bab11..bf1fa6141d81 100644 --- a/multibody/optimization/manipulator_equation_constraint.cc +++ b/multibody/optimization/manipulator_equation_constraint.cc @@ -113,12 +113,12 @@ void ManipulatorEquationConstraint::DoEval( // Compute the Jacobian. // Define Body A's frame as A, the geometry attached to body A as frame Ga, // and the witness point on geometry Ga as Ca. - const auto& X_AGa = inspector.X_FG(signed_distance_pair.id_A); + const auto& X_AGa = inspector.GetPoseInFrame(signed_distance_pair.id_A); const auto& p_GaCa = signed_distance_pair.p_ACa; const Vector3 p_ACa = X_AGa.cast() * p_GaCa; // Define Body B's frame as B, the geometry attached to body B as frame Gb, // and the witness point on geometry Gb as Cb. - const auto& X_BGb = inspector.X_FG(signed_distance_pair.id_B); + const auto& X_BGb = inspector.GetPoseInFrame(signed_distance_pair.id_B); const auto& p_GbCb = signed_distance_pair.p_BCb; const Vector3 p_BCb = X_BGb.cast() * p_GbCb; Eigen::Matrix Jv_V_WCa( diff --git a/multibody/optimization/sliding_friction_complementarity_constraint.cc b/multibody/optimization/sliding_friction_complementarity_constraint.cc index 441b4634ae4c..193b638b873c 100644 --- a/multibody/optimization/sliding_friction_complementarity_constraint.cc +++ b/multibody/optimization/sliding_friction_complementarity_constraint.cc @@ -198,7 +198,9 @@ void SlidingFrictionComplementarityNonlinearConstraint::DoEval( // the body frame of body B. const Vector3& p_BgCb = signed_distance_pair.p_BCb; const Vector3 p_BbCb = - inspector.X_FG(signed_distance_pair.id_B) * p_BgCb; + inspector.GetPoseInFrame(signed_distance_pair.id_B) + .template cast() * + p_BgCb; const Vector3 p_BbCb_W = plant.CalcRelativeTransform(context, plant.world_frame(), frameB) .rotation() * diff --git a/multibody/optimization/static_equilibrium_constraint.cc b/multibody/optimization/static_equilibrium_constraint.cc index 28fa79c1565c..6b6409ad829e 100644 --- a/multibody/optimization/static_equilibrium_constraint.cc +++ b/multibody/optimization/static_equilibrium_constraint.cc @@ -82,12 +82,12 @@ void StaticEquilibriumConstraint::DoEval( // Compute the Jacobian. // Define Body A's frame as A, the geometry attached to body A has frame Ga, // and the witness point on geometry Ga is Ca. - const auto& X_AGa = inspector.X_FG(signed_distance_pair.id_A); + const auto& X_AGa = inspector.GetPoseInFrame(signed_distance_pair.id_A); const auto& p_GaCa = signed_distance_pair.p_ACa; const Vector3 p_ACa = X_AGa.cast() * p_GaCa; // Define Body B's frame as B, the geometry attached to body B has frame Gb, // and the witness point on geometry Gb is Cb. - const auto& X_BGb = inspector.X_FG(signed_distance_pair.id_B); + const auto& X_BGb = inspector.GetPoseInFrame(signed_distance_pair.id_B); const auto& p_GbCb = signed_distance_pair.p_BCb; const Vector3 p_BCb = X_BGb.cast() * p_GbCb; Eigen::Matrix Jv_V_WCa( diff --git a/multibody/optimization/static_equilibrium_problem.cc b/multibody/optimization/static_equilibrium_problem.cc index 24fa938fb04c..6e13a612a74f 100644 --- a/multibody/optimization/static_equilibrium_problem.cc +++ b/multibody/optimization/static_equilibrium_problem.cc @@ -122,7 +122,8 @@ std::vector StaticEquilibriumProblem::GetContactWrenchSolution( body_B_index = frame_Fb.body().index(); // Define Body B's frame as Fb, the geometry attached to body B has // frame Gb, and the witness point on geometry Gb is Cb. - const auto& X_FbGb = inspector.X_FG(signed_distance_pair.id_B); + const auto& X_FbGb = inspector.GetPoseInFrame(signed_distance_pair.id_B) + .template cast(); const auto& p_GbCb = signed_distance_pair.p_BCb; const Vector3 p_FbCb = X_FbGb * p_GbCb; plant_.CalcPointsPositions(*context_, frame_Fb, p_FbCb, diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index 5ba91c2370f9..4a8ddb366eab 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -436,8 +436,8 @@ void AddLinksFromSpecification( geometry_instance->illustration_properties() != nullptr); plant->RegisterVisualGeometry( - body, RigidTransformd(geometry_instance->pose()), - geometry_instance->shape(), geometry_instance->name(), + body, geometry_instance->pose(), geometry_instance->shape(), + geometry_instance->name(), *geometry_instance->illustration_properties()); } } diff --git a/multibody/parsing/detail_urdf_parser.cc b/multibody/parsing/detail_urdf_parser.cc index 611950aebc04..14fe1bffae5a 100644 --- a/multibody/parsing/detail_urdf_parser.cc +++ b/multibody/parsing/detail_urdf_parser.cc @@ -126,8 +126,8 @@ void ParseBody(const multibody::PackageMap& package_map, // instance, even if it is empty. DRAKE_DEMAND(geometry_instance.illustration_properties() != nullptr); plant->RegisterVisualGeometry( - body, RigidTransformd(geometry_instance.pose()), - geometry_instance.shape(), geometry_instance.name(), + body, geometry_instance.pose(), geometry_instance.shape(), + geometry_instance.name(), *geometry_instance.illustration_properties()); } @@ -138,9 +138,9 @@ void ParseBody(const multibody::PackageMap& package_map, geometry::GeometryInstance geometry_instance = ParseCollision(body_name, package_map, root_dir, collision_node, &friction); - plant->RegisterCollisionGeometry( - body, RigidTransformd(geometry_instance.pose()), - geometry_instance.shape(), geometry_instance.name(), friction); + plant->RegisterCollisionGeometry(body, geometry_instance.pose(), + geometry_instance.shape(), + geometry_instance.name(), friction); } } } diff --git a/multibody/parsing/test/detail_scene_graph_test.cc b/multibody/parsing/test/detail_scene_graph_test.cc index 19acf2bd2b61..c73df697a9b7 100644 --- a/multibody/parsing/test/detail_scene_graph_test.cc +++ b/multibody/parsing/test/detail_scene_graph_test.cc @@ -410,11 +410,11 @@ GTEST_TEST(SceneGraphParserDetail, MakeHalfSpaceGeometryInstanceFromSdfVisual) { // The expected orientation of the canonical frame C (in which the plane's // normal aligns with Cz) in the link frame L. - const RotationMatrix R_LC_expected( - HalfSpace::MakePose(normal_L_expected, Vector3d::Zero()).linear()); + const RotationMatrix R_LC_expected = + HalfSpace::MakePose(normal_L_expected, Vector3d::Zero()).rotation(); // Retrieve the GeometryInstance pose as parsed from the sdf::Visual. - const RotationMatrix R_LC(geometry_instance->pose().linear()); + const RotationMatrix R_LC = geometry_instance->pose().rotation(); const Vector3d normal_L = R_LC.col(2); // Verify results to precision given by kTolerance. @@ -735,8 +735,8 @@ GTEST_TEST(SceneGraphParserDetail, // The expected orientation of the canonical frame C (in which the plane's // normal aligns with Cz) in the link frame L. - const RotationMatrix R_LG_expected( - HalfSpace::MakePose(normal_L_expected, Vector3d::Zero()).linear()); + const RotationMatrix& R_LG_expected = + HalfSpace::MakePose(normal_L_expected, Vector3d::Zero()).rotation(); // Verify results to precision given by kTolerance. const double kTolerance = 10 * std::numeric_limits::epsilon(); diff --git a/multibody/parsing/test/detail_urdf_geometry_test.cc b/multibody/parsing/test/detail_urdf_geometry_test.cc index cfa4b9fc035d..e952b95773c4 100644 --- a/multibody/parsing/test/detail_urdf_geometry_test.cc +++ b/multibody/parsing/test/detail_urdf_geometry_test.cc @@ -119,7 +119,7 @@ TEST_F(UrdfGeometryTests, TestParseMaterial1) { EXPECT_EQ(visual.name().substr(0, name_base.size()), name_base); EXPECT_TRUE(CompareMatrices( - visual.pose().matrix(), RigidTransformd().matrix())); + visual.pose().GetAsMatrix34(), RigidTransformd().GetAsMatrix34())); const geometry::Box* box = dynamic_cast(&visual.shape()); @@ -149,7 +149,7 @@ TEST_F(UrdfGeometryTests, TestParseMaterial2) { const RigidTransformd expected_pose(Eigen::Vector3d(0, 0, 0.3)); EXPECT_TRUE(CompareMatrices( - visual.pose().matrix(), expected_pose.matrix())); + visual.pose().GetAsMatrix34(), expected_pose.GetAsMatrix34())); const geometry::Cylinder* cylinder = dynamic_cast(&visual.shape()); diff --git a/multibody/plant/test/multibody_plant_test.cc b/multibody/plant/test/multibody_plant_test.cc index 778c0453d943..593cbf3c4077 100644 --- a/multibody/plant/test/multibody_plant_test.cc +++ b/multibody/plant/test/multibody_plant_test.cc @@ -903,8 +903,7 @@ class SphereChainScenario { ground_id_ = plant_->RegisterCollisionGeometry( plant_->world_body(), // A half-space passing through the origin in the x-z plane. - RigidTransformd( - geometry::HalfSpace::MakePose(Vector3d::UnitY(), Vector3d::Zero())), + geometry::HalfSpace::MakePose(Vector3d::UnitY(), Vector3d::Zero()), geometry::HalfSpace(), "ground", CoulombFriction()); auto make_sphere = [this](int i) { @@ -1172,8 +1171,7 @@ GTEST_TEST(MultibodyPlantTest, CollisionGeometryRegistration) { GeometryId ground_id = plant.RegisterCollisionGeometry( plant.world_body(), // A half-space passing through the origin in the x-z plane. - RigidTransformd( - geometry::HalfSpace::MakePose(Vector3d::UnitY(), Vector3d::Zero())), + geometry::HalfSpace::MakePose(Vector3d::UnitY(), Vector3d::Zero()), geometry::HalfSpace(), "ground", ground_friction); // Add two spherical bodies. @@ -1275,8 +1273,7 @@ GTEST_TEST(MultibodyPlantTest, VisualGeometryRegistration) { GeometryId ground_id = plant.RegisterVisualGeometry( plant.world_body(), // A half-space passing through the origin in the x-z plane. - RigidTransformd( - geometry::HalfSpace::MakePose(Vector3d::UnitY(), Vector3d::Zero())), + geometry::HalfSpace::MakePose(Vector3d::UnitY(), Vector3d::Zero()), geometry::HalfSpace(), "ground"); EXPECT_EQ(render_engine.num_registered(), 1); diff --git a/systems/rendering/render_pose_to_geometry_pose.cc b/systems/rendering/render_pose_to_geometry_pose.cc index 32cb2fb4b7b3..7e7d16d86120 100644 --- a/systems/rendering/render_pose_to_geometry_pose.cc +++ b/systems/rendering/render_pose_to_geometry_pose.cc @@ -7,6 +7,7 @@ using drake::geometry::FrameId; using drake::geometry::SourceId; +using drake::math::RigidTransform; namespace drake { namespace systems { @@ -28,7 +29,7 @@ RenderPoseToGeometryPose::RenderPoseToGeometryPose( [this, frame_id](const Context& context, AbstractValue* calculated) { const Input& input = get_input_port().template Eval(context); calculated->get_mutable_value() = - {{frame_id, input.get_isometry()}}; + {{frame_id, RigidTransform(input.get_isometry())}}; }); } diff --git a/systems/sensors/test/optitrack_sender_test.cc b/systems/sensors/test/optitrack_sender_test.cc index b7438b5e822e..353b530ef6f4 100644 --- a/systems/sensors/test/optitrack_sender_test.cc +++ b/systems/sensors/test/optitrack_sender_test.cc @@ -4,12 +4,14 @@ #include "optitrack/optitrack_frame_t.hpp" #include "drake/geometry/frame_kinematics_vector.h" +#include "drake/math/rigid_transform.h" #include "drake/systems/framework/context.h" namespace drake { namespace systems { namespace sensors { +using math::RigidTransformd; using optitrack::optitrack_frame_t; // This test sets up a systems::sensors::TrackedBody structure to be passed @@ -33,10 +35,9 @@ GTEST_TEST(OptitrackSenderTest, OptitrackLcmSenderTest) { // Sets up a test body with an arbitrarily chosen pose. Eigen::Vector3d axis(1 / sqrt(3), 1 / sqrt(3), 1 / sqrt(3)); - const geometry::FramePoseVector pose_vector{{ - frame_id, - Eigen::Isometry3d(Eigen::AngleAxis(0.2, axis)). - pretranslate(Eigen::Vector3d(tx, ty, tz))}}; + const geometry::FramePoseVector pose_vector{ + {frame_id, RigidTransformd(Eigen::AngleAxis(0.2, axis), + Eigen::Vector3d(tx, ty, tz))}}; EXPECT_EQ(pose_vector.value(frame_id).translation()[0], tx); EXPECT_EQ(pose_vector.value(frame_id).translation()[1], ty);