diff --git a/attic/manipulation/util/frame_pose_tracker.cc b/attic/manipulation/util/frame_pose_tracker.cc index 97a90c0e2ea9..74e499e50a5d 100644 --- a/attic/manipulation/util/frame_pose_tracker.cc +++ b/attic/manipulation/util/frame_pose_tracker.cc @@ -100,7 +100,6 @@ void FramePoseTracker::Init() { pose_vector_output_port_index_ = this->DeclareAbstractOutputPort( - geometry::FramePoseVector(source_id_, frame_ids), &FramePoseTracker::OutputStatus).get_index(); } 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 3eb214bf8a8a..b21adf3a7c93 100644 --- a/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc +++ b/attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc @@ -97,9 +97,7 @@ RigidBodyPlantBridge::RigidBodyPlantBridge(const RigidBodyTree* tree, // second body id. std::vector dynamic_frames(body_ids_.begin() + 1, body_ids_.end()); geometry_pose_port_ = this->DeclareAbstractOutputPort( - FramePoseVector(source_id_, dynamic_frames), - &RigidBodyPlantBridge::CalcFramePoseOutput) - .get_index(); + &RigidBodyPlantBridge::CalcFramePoseOutput).get_index(); } template @@ -188,9 +186,6 @@ void RigidBodyPlantBridge::RegisterTree(SceneGraph* scene_graph) { template void RigidBodyPlantBridge::CalcFramePoseOutput( const MyContext& context, FramePoseVector* poses) const { - DRAKE_DEMAND(source_id_.is_valid()); - DRAKE_DEMAND(poses->size() == static_cast(body_ids_.size() - 1)); - const BasicVector& input_vector = *this->EvalVectorInput(context, 0); // Obtains the generalized positions from vector_base. const VectorX q = input_vector.CopyToVector().head( diff --git a/bindings/pydrake/BUILD.bazel b/bindings/pydrake/BUILD.bazel index 53d78c97e404..d7f6404ffa8e 100644 --- a/bindings/pydrake/BUILD.bazel +++ b/bindings/pydrake/BUILD.bazel @@ -148,6 +148,7 @@ drake_pybind_library( package_info = PACKAGE_INFO, py_deps = [ ":module_py", + "//bindings/pydrake/common/test_utilities", "//bindings/pydrake/systems:framework_py", "//bindings/pydrake/systems:lcm_py", ], diff --git a/bindings/pydrake/geometry_py.cc b/bindings/pydrake/geometry_py.cc index f457c94d6814..d16668f1a927 100644 --- a/bindings/pydrake/geometry_py.cc +++ b/bindings/pydrake/geometry_py.cc @@ -77,9 +77,12 @@ PYBIND11_MODULE(geometry, m) { py::class_>( m, "FramePoseVector", doc.FrameKinematicsVector.doc) .def(py::init<>(), doc.FrameKinematicsVector.ctor.doc_0args) - .def(py::init&>(), + .def(py::init([](SourceId source_id, const std::vector& ids) { + WarnDeprecated("See API docs for deprecation notice."); + return std::make_unique>(source_id, ids); + }), py::arg("source_id"), py::arg("ids"), - doc.FrameKinematicsVector.ctor.doc_2args); + doc.FrameKinematicsVector.ctor.doc_deprecated_2args); pysystems::AddValueInstantiation>(m); py::class_>(m, "QueryObject", doc.QueryObject.doc) diff --git a/bindings/pydrake/test/geometry_test.py b/bindings/pydrake/test/geometry_test.py index 4567d19dd9f7..0d9527267230 100644 --- a/bindings/pydrake/test/geometry_test.py +++ b/bindings/pydrake/test/geometry_test.py @@ -4,6 +4,7 @@ import warnings from pydrake.common import FindResourceOrThrow +from pydrake.common.test_utilities.deprecation import catch_drake_warnings from pydrake.lcm import DrakeMockLcm from pydrake.systems.framework import DiagramBuilder, InputPort, OutputPort from pydrake.common.deprecation import DrakeDeprecationWarning @@ -41,8 +42,9 @@ def test_scene_graph_api(self): def test_frame_pose_vector_api(self): mut.FramePoseVector() - mut.FramePoseVector(source_id=mut.SourceId.get_new_id(), - ids=[mut.FrameId.get_new_id()]) + with catch_drake_warnings(expected_count=1): + mut.FramePoseVector(source_id=mut.SourceId.get_new_id(), + ids=[mut.FrameId.get_new_id()]) def test_query_object_api(self): # TODO(eric.cousineau): Create self-contained unittests (#9899). diff --git a/examples/pendulum/pendulum_geometry.cc b/examples/pendulum/pendulum_geometry.cc index 2726cc62b9e0..7604af0f72cd 100644 --- a/examples/pendulum/pendulum_geometry.cc +++ b/examples/pendulum/pendulum_geometry.cc @@ -95,16 +95,13 @@ PendulumGeometry::PendulumGeometry(geometry::SceneGraph* scene_graph) { void PendulumGeometry::OutputGeometryPose( const systems::Context& context, geometry::FramePoseVector* poses) const { - DRAKE_DEMAND(source_id_.is_valid()); DRAKE_DEMAND(frame_id_.is_valid()); const auto& input = get_input_port(0).Eval>(context); const double theta = input.theta(); const math::RigidTransformd pose(math::RotationMatrixd::MakeYRotation(theta)); - *poses = geometry::FramePoseVector(source_id_, {frame_id_}); - poses->clear(); - poses->set_value(frame_id_, pose.GetAsIsometry3()); + *poses = {{frame_id_, pose.GetAsIsometry3()}}; } } // namespace pendulum diff --git a/examples/quadrotor/quadrotor_plant.cc b/examples/quadrotor/quadrotor_plant.cc index 75697be939d9..4d0d00dad96b 100644 --- a/examples/quadrotor/quadrotor_plant.cc +++ b/examples/quadrotor/quadrotor_plant.cc @@ -151,9 +151,7 @@ systems::OutputPortIndex QuadrotorPlant::AllocateGeometryPoseOutputPort() { DRAKE_DEMAND(source_id_.is_valid() && frame_id_.is_valid()); return this ->DeclareAbstractOutputPort( - "geometry_pose", - geometry::FramePoseVector(source_id_, {frame_id_}), - &QuadrotorPlant::CopyPoseOut) + "geometry_pose", &QuadrotorPlant::CopyPoseOut) .get_index(); } @@ -184,16 +182,11 @@ void QuadrotorPlant::RegisterGeometry( template void QuadrotorPlant::CopyPoseOut(const systems::Context& context, geometry::FramePoseVector* poses) const { - DRAKE_DEMAND(poses->size() == 1); - DRAKE_DEMAND(poses->source_id() == source_id_); - VectorX state = context.get_continuous_state_vector().CopyToVector(); - - poses->clear(); math::RigidTransform pose( math::RollPitchYaw(state.template segment<3>(3)), state.template head<3>()); - poses->set_value(frame_id_, pose.GetAsIsometry3()); + *poses = {{frame_id_, pose.GetAsIsometry3()}}; } std::unique_ptr> StabilizingLQRController( diff --git a/examples/scene_graph/bouncing_ball_plant.cc b/examples/scene_graph/bouncing_ball_plant.cc index b997df634e6c..054c9495e7c0 100644 --- a/examples/scene_graph/bouncing_ball_plant.cc +++ b/examples/scene_graph/bouncing_ball_plant.cc @@ -31,9 +31,9 @@ template BouncingBallPlant::BouncingBallPlant(SourceId source_id, SceneGraph* scene_graph, const Vector2& p_WB) - : source_id_(source_id), p_WB_(p_WB) { + : p_WB_(p_WB) { DRAKE_DEMAND(scene_graph != nullptr); - DRAKE_DEMAND(source_id_.is_valid()); + DRAKE_DEMAND(source_id.is_valid()); geometry_query_port_ = this->DeclareAbstractInputPort( systems::kUseDefaultName, Value>{}) @@ -61,7 +61,6 @@ BouncingBallPlant::BouncingBallPlant(SourceId source_id, // Allocate the output port now that the frame has been registered. geometry_pose_port_ = this->DeclareAbstractOutputPort( - FramePoseVector(source_id_, {ball_frame_id_}), &BouncingBallPlant::CalcFramePoseOutput, {this->configuration_ticket()}) .get_index(); @@ -99,14 +98,10 @@ void BouncingBallPlant::CopyStateToOutput( template void BouncingBallPlant::CalcFramePoseOutput( const Context& context, FramePoseVector* poses) const { - DRAKE_DEMAND(poses->source_id() == source_id_); - DRAKE_DEMAND(poses->size() == 1); - Isometry3 pose = Isometry3::Identity(); const BouncingBallVector& state = get_state(context); pose.translation() << p_WB_.x(), p_WB_.y(), state.z(); - poses->clear(); - poses->set_value(ball_frame_id_, pose); + *poses = {{ball_frame_id_, pose}}; } // Compute the actual physics. diff --git a/examples/scene_graph/bouncing_ball_plant.h b/examples/scene_graph/bouncing_ball_plant.h index 697afe57055b..dcf42cc2d7c0 100644 --- a/examples/scene_graph/bouncing_ball_plant.h +++ b/examples/scene_graph/bouncing_ball_plant.h @@ -106,8 +106,6 @@ class BouncingBallPlant : public systems::LeafSystem { return get_mutable_state(&context->get_mutable_continuous_state()); } - // This plant's source id in SceneGraph. - geometry::SourceId source_id_; // The projected position of the ball onto the ground plane. I.e., it's // "where the ball bounces". const Vector2 p_WB_; diff --git a/examples/scene_graph/dev/bouncing_ball_plant.cc b/examples/scene_graph/dev/bouncing_ball_plant.cc index a0471d094383..118eee939db7 100644 --- a/examples/scene_graph/dev/bouncing_ball_plant.cc +++ b/examples/scene_graph/dev/bouncing_ball_plant.cc @@ -31,9 +31,9 @@ template BouncingBallPlant::BouncingBallPlant(SourceId source_id, SceneGraph* scene_graph, const Vector2& p_WB) - : source_id_(source_id), p_WB_(p_WB) { + : p_WB_(p_WB) { DRAKE_DEMAND(scene_graph != nullptr); - DRAKE_DEMAND(source_id_.is_valid()); + DRAKE_DEMAND(source_id.is_valid()); geometry_query_port_ = this->DeclareAbstractInputPort( systems::kUseDefaultName, Value>{}) @@ -61,7 +61,6 @@ BouncingBallPlant::BouncingBallPlant(SourceId source_id, // Allocate the output port now that the frame has been registered. geometry_pose_port_ = this->DeclareAbstractOutputPort( - FramePoseVector(source_id_, {ball_frame_id_}), &BouncingBallPlant::CalcFramePoseOutput, {this->configuration_ticket()}) .get_index(); @@ -99,14 +98,10 @@ void BouncingBallPlant::CopyStateToOutput( template void BouncingBallPlant::CalcFramePoseOutput( const Context& context, FramePoseVector* poses) const { - DRAKE_DEMAND(poses->source_id() == source_id_); - DRAKE_DEMAND(poses->size() == 1); - Isometry3 pose = Isometry3::Identity(); const BouncingBallVector& state = get_state(context); pose.translation() << p_WB_.x(), p_WB_.y(), state.z(); - poses->clear(); - poses->set_value(ball_frame_id_, pose); + *poses = {{ball_frame_id_, pose}}; } // Compute the actual physics. diff --git a/examples/scene_graph/dev/bouncing_ball_plant.h b/examples/scene_graph/dev/bouncing_ball_plant.h index e8bec89df2f8..c05c0cf621d0 100644 --- a/examples/scene_graph/dev/bouncing_ball_plant.h +++ b/examples/scene_graph/dev/bouncing_ball_plant.h @@ -106,8 +106,6 @@ class BouncingBallPlant : public systems::LeafSystem { return get_mutable_state(&context->get_mutable_continuous_state()); } - // This plant's source id in SceneGraph. - geometry::SourceId source_id_; // The projected position of the ball onto the ground plane. I.e., it's // "where the ball bounces". const Vector2 p_WB_; diff --git a/examples/scene_graph/dev/solar_system.cc b/examples/scene_graph/dev/solar_system.cc index 32e72d00ee85..b394c39149cb 100644 --- a/examples/scene_graph/dev/solar_system.cc +++ b/examples/scene_graph/dev/solar_system.cc @@ -61,7 +61,6 @@ SolarSystem::SolarSystem(SceneGraph* scene_graph) { // Now that frames have been registered, allocate the output port. geometry_pose_port_ = this->DeclareAbstractOutputPort( - FramePoseVector(source_id_, body_ids_), &SolarSystem::CalcFramePoseOutput, {this->configuration_ticket()}) .get_index(); @@ -262,11 +261,7 @@ void SolarSystem::AllocateGeometry(SceneGraph* scene_graph) { template void SolarSystem::CalcFramePoseOutput(const Context& context, FramePoseVector* poses) const { - DRAKE_DEMAND(poses->size() == kBodyCount); - DRAKE_DEMAND(poses->source_id() == source_id_); - const BasicVector& state = get_state(context); - poses->clear(); for (int i = 0; i < kBodyCount; ++i) { Isometry3 pose = body_offset_[i]; diff --git a/examples/scene_graph/solar_system.cc b/examples/scene_graph/solar_system.cc index c7a08a916ad3..da821b45523a 100644 --- a/examples/scene_graph/solar_system.cc +++ b/examples/scene_graph/solar_system.cc @@ -50,7 +50,6 @@ SolarSystem::SolarSystem(SceneGraph* scene_graph) { // Now that frames have been registered, allocate the output port. geometry_pose_port_ = this->DeclareAbstractOutputPort( - FramePoseVector(source_id_, body_ids_), &SolarSystem::CalcFramePoseOutput, {this->configuration_ticket()}) .get_index(); @@ -319,11 +318,7 @@ void SolarSystem::AllocateGeometry(SceneGraph* scene_graph) { template void SolarSystem::CalcFramePoseOutput(const Context& context, FramePoseVector* poses) const { - DRAKE_DEMAND(poses->size() == kBodyCount); - DRAKE_DEMAND(poses->source_id() == source_id_); - const BasicVector& state = get_state(context); - poses->clear(); for (int i = 0; i < kBodyCount; ++i) { Isometry3 pose = body_offset_[i]; diff --git a/geometry/BUILD.bazel b/geometry/BUILD.bazel index 985e3223dca3..b9f6c60a4600 100644 --- a/geometry/BUILD.bazel +++ b/geometry/BUILD.bazel @@ -282,6 +282,8 @@ drake_cc_googletest( deps = [ ":frame_kinematics", "//common/test_utilities", + "//common/test_utilities:limit_malloc", + "//math:geometric_transform", ], ) diff --git a/geometry/dev/geometry_state.cc b/geometry/dev/geometry_state.cc index da090d45741e..83ac2d4ab9e8 100644 --- a/geometry/dev/geometry_state.cc +++ b/geometry/dev/geometry_state.cc @@ -1025,12 +1025,13 @@ void GeometryState::CollectIndices( } template -void GeometryState::SetFramePoses(const FramePoseVector& poses) { +void GeometryState::SetFramePoses( + const SourceId source_id, const FramePoseVector& poses) { // TODO(SeanCurtis-TRI): Down the road, make this validation depend on // ASSERT_ARMED. - ValidateFrameIds(poses); + ValidateFrameIds(source_id, poses); const Isometry3 world_pose = Isometry3::Identity(); - for (auto frame_id : source_root_frame_map_[poses.source_id()]) { + for (auto frame_id : source_root_frame_map_[source_id]) { UpdatePosesRecursively(frames_[frame_id], world_pose, poses); } } @@ -1038,8 +1039,8 @@ void GeometryState::SetFramePoses(const FramePoseVector& poses) { template template void GeometryState::ValidateFrameIds( + const SourceId source_id, const FrameKinematicsVector& kinematics_data) const { - SourceId source_id = kinematics_data.source_id(); auto& frames = GetFramesForSource(source_id); const int ref_frame_count = static_cast(frames.size()); if (ref_frame_count != kinematics_data.size()) { diff --git a/geometry/dev/geometry_state.h b/geometry/dev/geometry_state.h index 42a0529e427a..bbb34b8d55e1 100644 --- a/geometry/dev/geometry_state.h +++ b/geometry/dev/geometry_state.h @@ -782,14 +782,15 @@ class GeometryState { // @param poses The frame id and pose values. // @throws std::logic_error If the ids are invalid as defined by // ValidateFrameIds(). - void SetFramePoses(const FramePoseVector& poses); + void SetFramePoses(SourceId source_id, const FramePoseVector& poses); // Confirms that the set of ids provided include _all_ of the frames // registered to the set's source id and that no extra frames are included. // @param values The kinematics values (ids and values) to validate. // @throws std::runtime_error if the set is inconsistent with known topology. template - void ValidateFrameIds(const FrameKinematicsVector& values) const; + void ValidateFrameIds(SourceId source_id, + const FrameKinematicsVector& values) const; // Method that performs any final book-keeping/updating on the state after // _all_ of the state's frames have had their poses updated. diff --git a/geometry/dev/scene_graph.cc b/geometry/dev/scene_graph.cc index 5d0777064b6b..8d5f588d4eca 100644 --- a/geometry/dev/scene_graph.cc +++ b/geometry/dev/scene_graph.cc @@ -456,7 +456,7 @@ void SceneGraph::CalcPoseUpdate(const GeometryContext& context, } const auto& poses = pose_port.template Eval>(context); - mutable_state.SetFramePoses(poses); + mutable_state.SetFramePoses(source_id, poses); } } } diff --git a/geometry/dev/test/geometry_state_test.cc b/geometry/dev/test/geometry_state_test.cc index 76342510ab79..bb2bb678ea82 100644 --- a/geometry/dev/test/geometry_state_test.cc +++ b/geometry/dev/test/geometry_state_test.cc @@ -85,8 +85,8 @@ class GeometryStateTester { return state_->X_PF_; } - void SetFramePoses(const FramePoseVector& poses) { - state_->SetFramePoses(poses); + void SetFramePoses(SourceId source_id, const FramePoseVector& poses) { + state_->SetFramePoses(source_id, poses); } // Returns the internal index for the geometry with the given index; if there @@ -119,8 +119,9 @@ class GeometryStateTester { } template - void ValidateFrameIds(const FrameKinematicsVector& data) const { - state_->ValidateFrameIds(data); + void ValidateFrameIds(SourceId source_id, + const FrameKinematicsVector& data) const { + state_->ValidateFrameIds(source_id, data); } int peek_next_clique() const { @@ -627,12 +628,11 @@ TEST_F(GeometryStateTest, ValidateSingleSourceTree) { } // Confirm posing positions the frames properly. - FramePoseVector poses(source_id_, frames_); - poses.clear(); + FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { poses.set_value(frames_[f], X_PF_[f]); } - gs_tester_.SetFramePoses(poses); + gs_tester_.SetFramePoses(source_id_, poses); gs_tester_.FinalizePoseUpdate(); test_frame(0, gs_tester_.get_world_frame(), 0); @@ -1122,33 +1122,30 @@ TEST_F(GeometryStateTest, GetFrameIdFromBadId) { // Tests the validation of the ids provided in a frame kinematics vector. TEST_F(GeometryStateTest, ValidateFrameIds) { SourceId s_id = SetUpSingleSourceTree(); - FramePoseVector frame_set(s_id, frames_); - frame_set.clear(); + FramePoseVector frame_set; for (auto frame_id : frames_) { frame_set.set_value(frame_id, Isometry3::Identity()); } // Case: frame ids are valid. - EXPECT_NO_THROW(gs_tester_.ValidateFrameIds(frame_set)); + EXPECT_NO_THROW(gs_tester_.ValidateFrameIds(s_id, frame_set)); // Case: Right number, wrong frames. - vector bad_frames; + FramePoseVector frame_set_2; for (int i = 0; i < kFrameCount; ++i) { - bad_frames.push_back(FrameId::get_new_id()); + frame_set_2.set_value(FrameId::get_new_id(), Isometry3d::Identity()); } - FramePoseVector frame_set_2(s_id, bad_frames); DRAKE_EXPECT_THROWS_MESSAGE( - gs_tester_.ValidateFrameIds(frame_set_2), std::runtime_error, + gs_tester_.ValidateFrameIds(s_id, frame_set_2), std::runtime_error, "Registered frame id \\(\\d+\\) belonging to source \\d+ was not found " "in the provided kinematics data."); // Case: Too few frames. - vector missing_frames; + FramePoseVector frame_set_3; for (int i = 0; i < kFrameCount - 1; ++i) { - missing_frames.push_back(frames_[i]); + frame_set.set_value(frames_[i], Isometry3::Identity()); } - FramePoseVector frame_set_3(s_id, missing_frames); DRAKE_EXPECT_THROWS_MESSAGE( - gs_tester_.ValidateFrameIds(frame_set_3), std::runtime_error, + gs_tester_.ValidateFrameIds(s_id, frame_set_3), std::runtime_error, "Disagreement in expected number of frames \\(\\d+\\)" " and the given number of frames \\(\\d+\\)."); } @@ -1169,10 +1166,9 @@ TEST_F(GeometryStateTest, SetFramePoses) { } auto make_pose_vector = - [&s_id, &frame_poses, this]() -> FramePoseVector { + [&frame_poses, this]() -> FramePoseVector { const int count = static_cast(this->frames_.size()); - FramePoseVector poses(s_id, this->frames_); - poses.clear(); + FramePoseVector poses; for (int i = 0; i < count; ++i) { poses.set_value(this->frames_[i], frame_poses[i]); } @@ -1185,7 +1181,7 @@ TEST_F(GeometryStateTest, SetFramePoses) { // Case 1: Set all frames to identity poses. The world pose of all the // geometry should be that of the geometry in its frame. FramePoseVector poses1 = make_pose_vector(); - gs_tester_.SetFramePoses(poses1); + gs_tester_.SetFramePoses(s_id, poses1); const auto& world_poses = gs_tester_.get_geometry_world_poses(); for (int i = 0; i < kFrameCount * kGeometryCount; ++i) { EXPECT_TRUE(CompareMatrices(world_poses[i].matrix().block<3, 4>(0, 0), @@ -1200,7 +1196,7 @@ TEST_F(GeometryStateTest, SetFramePoses) { frame_poses[0] = offset; frame_poses[1] = offset; FramePoseVector poses2 = make_pose_vector(); - gs_tester_.SetFramePoses(poses2); + gs_tester_.SetFramePoses(s_id, poses2); for (int i = 0; i < kFrameCount * kGeometryCount; ++i) { EXPECT_TRUE( CompareMatrices(world_poses[i].matrix().block<3, 4>(0, 0), @@ -1211,7 +1207,7 @@ TEST_F(GeometryStateTest, SetFramePoses) { // 0, 1, 2, & 3 moved up 1, and geometries 4 & 5 moved up two. frame_poses[2] = offset; FramePoseVector poses3 = make_pose_vector(); - gs_tester_.SetFramePoses(poses3); + gs_tester_.SetFramePoses(s_id, poses3); for (int i = 0; i < (kFrameCount - 1) * kGeometryCount; ++i) { EXPECT_TRUE( CompareMatrices(world_poses[i].matrix().block<3, 4>(0, 0), @@ -1245,10 +1241,9 @@ TEST_F(GeometryStateTest, QueryFrameProperties) { "No frame name available for invalid frame id: \\d+"); // Set the frame poses to query geometry and frame poses. - FramePoseVector poses(s_id, frames_); - poses.clear(); + FramePoseVector poses; for (int i = 0; i < kFrameCount; ++i) poses.set_value(frames_[i], X_PF_[i]); - gs_tester_.SetFramePoses(poses); + gs_tester_.SetFramePoses(s_id, poses); EXPECT_TRUE( CompareMatrices(geometry_state_.get_pose_in_world(frames_[0]).matrix(), @@ -1286,12 +1281,11 @@ TEST_F(GeometryStateTest, ExcludeCollisionsWithin) { SetUpSingleSourceTree(true /* assign proximity roles */); // Pose all of the frames to the specified poses in their parent frame. - FramePoseVector poses(source_id_, frames_); - poses.clear(); + FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { poses.set_value(frames_[f], X_PF_[f]); } - gs_tester_.SetFramePoses(poses); + gs_tester_.SetFramePoses(source_id_, poses); gs_tester_.FinalizePoseUpdate(); // This is *non* const; we'll decrement it as we filter more and more @@ -1342,12 +1336,11 @@ TEST_F(GeometryStateTest, ExcludeCollisionsBetween) { SetUpSingleSourceTree(true /* add proximity roles */); // Pose all of the frames to the specified poses in their parent frame. - FramePoseVector poses(source_id_, frames_); - poses.clear(); + FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { poses.set_value(frames_[f], X_PF_[f]); } - gs_tester_.SetFramePoses(poses); + gs_tester_.SetFramePoses(source_id_, poses); gs_tester_.FinalizePoseUpdate(); // This is *non* const; we'll decrement it as we filter more and more @@ -1383,12 +1376,11 @@ TEST_F(GeometryStateTest, NonProximityRoleInCollisionFilter) { SetUpSingleSourceTree(true /* add proximity roles */); // Pose all of the frames to the specified poses in their parent frame. - FramePoseVector poses(source_id_, frames_); - poses.clear(); + FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { poses.set_value(frames_[f], X_PF_[f]); } - gs_tester_.SetFramePoses(poses); + gs_tester_.SetFramePoses(source_id_, poses); gs_tester_.FinalizePoseUpdate(); // This is *non* const; we'll decrement it as we filter more and more diff --git a/geometry/dev/test/scene_graph_test.cc b/geometry/dev/test/scene_graph_test.cc index 9cb3fc7d2faf..7d509c8ae735 100644 --- a/geometry/dev/test/scene_graph_test.cc +++ b/geometry/dev/test/scene_graph_test.cc @@ -415,11 +415,7 @@ class GeometrySourceSystem : public systems::LeafSystem { frame_ids_.push_back(f_id); // Set up output port now that the frame is registered. - std::vector all_ids{f_id}; - all_ids.insert(all_ids.end(), extra_frame_ids_.begin(), - extra_frame_ids_.end()); this->DeclareAbstractOutputPort( - FramePoseVector(source_id_, all_ids), &GeometrySourceSystem::CalcFramePoseOutput); } SourceId get_source_id() const { return source_id_; } @@ -442,11 +438,6 @@ class GeometrySourceSystem : public systems::LeafSystem { // Populate with the pose data. void CalcFramePoseOutput(const Context& context, FramePoseVector* poses) const { - const int frame_count = - static_cast(frame_ids_.size() + extra_frame_ids_.size()); - DRAKE_DEMAND(poses->size() == frame_count); - DRAKE_DEMAND(poses->source_id() == source_id_); - poses->clear(); const int base_count = static_cast(frame_ids_.size()); @@ -600,9 +591,9 @@ GTEST_TEST(SceneGraphVisualizationTest, NoWorldInPoseVector) { // bundle. EXPECT_EQ(0, poses.get_num_poses()); auto context = scene_graph.AllocateContext(); - FramePoseVector pose_vector(s_id, {f_id}); - context->FixInputPort(scene_graph.get_source_pose_port(s_id).get_index(), - Value>(pose_vector)); + const FramePoseVector pose_vector{{ + f_id, Isometry3::Identity()}}; + scene_graph.get_source_pose_port(s_id).FixValue(context.get(), pose_vector); EXPECT_NO_THROW( SceneGraphTester::CalcPoseBundle(scene_graph, *context, &poses)); } diff --git a/geometry/frame_kinematics_vector.cc b/geometry/frame_kinematics_vector.cc index 2ec22dbcb38e..e0e6fa1e97dd 100644 --- a/geometry/frame_kinematics_vector.cc +++ b/geometry/frame_kinematics_vector.cc @@ -1,6 +1,6 @@ #include "drake/geometry/frame_kinematics_vector.h" -#include +#include #include #include @@ -11,59 +11,119 @@ namespace drake { namespace geometry { -using std::make_pair; -using std::move; +namespace { +template +void InitializeKinematicsValue(Isometry3* value) { + value->setIdentity(); +} +} // namespace +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" template FrameKinematicsVector::FrameKinematicsVector() : FrameKinematicsVector({}, {}) {} +#pragma GCC diagnostic pop template FrameKinematicsVector::FrameKinematicsVector( SourceId source_id, const std::vector& ids) : source_id_(source_id), values_(0) { + optional default_value{KinematicsValue{}}; + InitializeKinematicsValue(&default_value.value()); for (FrameId id : ids) { - bool is_unique = values_.insert(make_pair(id, FlaggedValue())).second; + bool is_unique = values_.emplace(id, default_value).second; if (!is_unique) { throw std::runtime_error( fmt::format("At least one frame id appears multiple times: {}", id)); } + ++size_; + } + DRAKE_ASSERT_VOID(CheckInvariants()); +} + +template +FrameKinematicsVector::FrameKinematicsVector( + std::initializer_list> init) { + values_.insert(init.begin(), init.end()); + size_ = init.size(); + DRAKE_ASSERT_VOID(CheckInvariants()); +} + +template +FrameKinematicsVector& +FrameKinematicsVector::operator=( + std::initializer_list> init) { + // N.B. We can't use unordered_map::insert in our operator= implementation + // because it does not overwrite pre-existing keys. (Our clear() doesn't + // remove the keys, it only nulls the values.) + clear(); + for (const auto& item : init) { + set_value(item.first, item.second); } + DRAKE_ASSERT_VOID(CheckInvariants()); + return *this; } template void FrameKinematicsVector::clear() { - ++version_; + for (auto& item : values_) { + item.second = nullopt; + } + size_ = 0; } template void FrameKinematicsVector::set_value( FrameId id, const KinematicsValue& value) { - using std::to_string; - - if (values_.count(id) != 1) { - throw std::runtime_error( - "Trying to set a kinematics value for a frame id that does not belong " - "to the kinematics vector: " + to_string(id)); - } - if (values_[id].version == version_) { - throw std::runtime_error( - "Trying to set kinematics value for the same id (" + to_string(id) - + ") multiple times. Did you forget to call clear()?"); - } - values_[id].version = version_; - values_[id].value = value; + auto& map_value = values_[id]; + if (!map_value.has_value()) { ++size_; } + map_value = value; } template const KinematicsValue& FrameKinematicsVector::value( FrameId id) const { using std::to_string; - if (values_.count(id) != 1) { - throw std::runtime_error("Can't acquire value for id " + to_string(id) + - ". It is not part of the kinematics data id set."); + auto iter = values_.find(id); + if (iter != values_.end()) { + const optional& map_value = iter->second; + if (map_value.has_value()) { + return *map_value; + } + } + throw std::runtime_error("No such FrameId " + to_string(id) + "."); +} + +template +bool FrameKinematicsVector::has_id(FrameId id) const { + auto iter = values_.find(id); + return (iter != values_.end()) && iter->second.has_value(); +} + +template +std::vector +FrameKinematicsVector::frame_ids() const { + std::vector result; + result.reserve(size_); + for (const auto& item : values_) { + if (item.second.has_value()) { + result.emplace_back(item.first); + } + } + DRAKE_ASSERT(static_cast(result.size()) == size_); + return result; +} + +template +void FrameKinematicsVector::CheckInvariants() const { + int num_nonnull = 0; + for (const auto& item : values_) { + if (item.second.has_value()) { + ++num_nonnull; + } } - return values_.at(id).value; + DRAKE_DEMAND(num_nonnull == size_); } // Explicitly instantiates on the most common scalar types. diff --git a/geometry/frame_kinematics_vector.h b/geometry/frame_kinematics_vector.h index ac7047508119..6a38f010193d 100644 --- a/geometry/frame_kinematics_vector.h +++ b/geometry/frame_kinematics_vector.h @@ -1,10 +1,14 @@ #pragma once +#include #include #include #include +#include "drake/common/drake_assert.h" #include "drake/common/drake_copyable.h" +#include "drake/common/drake_deprecated.h" +#include "drake/common/drake_optional.h" #include "drake/common/eigen_types.h" #include "drake/geometry/geometry_ids.h" #include "drake/geometry/utilities.h" @@ -12,140 +16,57 @@ namespace drake { namespace geometry { -#ifndef DRAKE_DOXYGEN_CXX -namespace detail { - -// A type-traits-style initializer for making sure that quantities in the frame -// kinematics vector are initialized. This is because Eigen actively does *not* -// initialize its data types and there is a code path by which uninitialized -// values in the vector can be accessed. - -template -struct KinematicsValueInitializer { - static void Initialize(Value* value) { - std::logic_error("Unsupported kinematics value"); - } -}; - -template -struct KinematicsValueInitializer> { - static void Initialize(Isometry3* value) { - value->setIdentity(); - } -}; - -// TODO(SeanCurtis-TRI): Add specializations for SpatialVelocity and -// SpatialAcceleration (setting them to zero vectors) as those quantities are -// added to SceneGraph. - -} // namespace detail -#endif // DRAKE_DOXYGEN_CXX - /** A %FrameKinematicsVector is used to report kinematics data for registered frames (identified by unique FrameId values) to SceneGraph. It serves as the basis of FramePoseVector, FrameVelocityVector, and FrameAccelerationVector. - The %FrameKinematicsVector must be constructed with the source's SourceId and - the ids of the frames *owned* by the source. Once constructed, it cannot be - resized. Typically, this will be done in the allocation method of the - LeafSystem which serves as a geometry source. - - Populating the vector with values is a two-step process: clear and set. Before - writing any kinematics data the vector should be _cleared_ (see clear()). After - clearing, each registered frame will have a kinematics value assigned to it - by calling set_value(). - - Only those frame ids provided in the constructor can be set. Attempting to - set the value for any other frame id is considered an error. - Attempting to write more frames than the vector was constructed for is - considered an error and will throw an exception. Failing to set data for every - registered frame will be considered an error when the %FrameKinematicsVector - is consumed by SceneGraph. - - The usage of this method would be in the allocation and calculation - of a LeafSystem's output port. The nature of the allocation depends on whether - the source id and number of frames are available at construction or not. The - first example shows the case where source id and frame count are known. The - second shows the alternate, deferred case. - ``` template - class AllocInConstructorSystem : public LeafSystem { + class MySystem : public LeafSystem { public: - explicit AllocInConstructorSystem(SourceId source_id) - : source_id_(source_id) { + MySystem() { ... - // Register frames, storing ids in frame_ids_ this->DeclareAbstractOutputPort( - FramePoseVector(source_id, frame_ids_), &AllocInConstructorSystem::CalcFramePoseOutput); ... } private: - void CalcFramePoseOutput(const MyContext& context, + void CalcFramePoseOutput(const Context& context, geometry::FramePoseVector* poses) const { - DRAKE_DEMAND(poses->source_id() == source_id_); - DRAKE_DEMAND(poses->size() == static_cast(frame_ids_.size())); - poses->clear(); for (int i = 0; i < static_cast(frame_ids_.size()); ++i) { poses->set_value(frame_ids_[i], poses_[i]); } } - SourceId source_id_; std::vector frame_ids_; std::vector> poses_; }; ``` - __Example 1: Known source id and frame count in constructor.__ + If a System only ever emits a single frame (or small-constant-number of + frames), then there's a shorter alternative way to write a Calc method, using + an initializer_list: ``` - /// Definition of FramePoseVector deferred to define number of frames. However, - /// it must be defined prior to call to `AllocateContext()`. - template - class DeferredAllocationSystem : public LeafSystem { - public: - DeferredAllocationSystem() { - ... - this->DeclareAbstractOutputPort( - &DeferredAllocationSystem::AllocateFramePoseOutput, - &DeferredAllocationSystem::CalcFramePoseOutput); - } - - private: - geometry::FramePoseVector AllocateFramePoseOutput() const { - // Assume that source_id_ has been assigned and the frames have been - // registered. - DRAKE_DEMAND(source_id_.is_valid()); - - return geometry::FramePoseVector(source_id_, frame_ids_); - } - - void CalcFramePoseOutput(const MyContext& context, + void CalcFramePoseOutput(const Context& context, geometry::FramePoseVector* poses) const { - DRAKE_DEMAND(poses->source_id() == source_id_); - DRAKE_DEMAND(poses->size() == static_cast(frame_ids_.size())); - - poses->clear(); - for (int i = 0; i < static_cast(frame_ids_.size()); ++i) { - poses->set_value(frame_ids_[i], poses_[i]); - } + const Isometry3& pose = ...; + *poses = {{frame_id_, pose}}; } - - SourceId source_id_; - std::vector frame_ids_; - std::vector> poses_; - }; ``` - __Example 2: Deferred pose vector allocation.__ + + N.B. When the systems framework calls the `Calc` method, the value pointed to + by `poses` is in an unspecified state. The implementation of `Calc` must + always ensure that `poses` contains the correct value upon return, no matter + what value it started with. The easy ways to do this are to call either + `poses->clear()` or the assignment operator `*poses = ...`. @tparam KinematicsValue The underlying data type of for the order of kinematics data (e.g., pose, velocity, or @@ -170,59 +91,49 @@ struct KinematicsValueInitializer> { */ template class FrameKinematicsVector { - // Forward declaration. - struct FlaggedValue; - public: DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(FrameKinematicsVector) - /** An object that represents the range of FrameId values in this class. It - is used in range-based for loops to iterate through registered frames. */ - using FrameIdRange = internal::MapKeyRange; - - // TODO(SeanCurtis-TRI) Find some API language that cautions users that this - // result is not terribly useful on its own, but instead it will usually be - // assigned into (using operator=) from another FrameKinematicsVector. /** Initializes the vector using an invalid SourceId with no frames .*/ FrameKinematicsVector(); - /** Initializes the vector on the owned ids. - @param source_id The source id of the owning geometry source. - @param ids The set of *all* frames owned by this geometry source. All - of these ids must have values provided in the output port - calculation and _only_ these ids. SceneGraph will - validate the ids to confirm that they are all owned by - the source with the given `source_id`. */ + /** Initializes the vector to the given source ID and frame IDs, using a + default value for each frame. */ + DRAKE_DEPRECATED("2019-08-01", "Simply use the default constructor.") FrameKinematicsVector(SourceId source_id, const std::vector& ids); - /** Initializes the vector to start setting kinematics values. */ - void clear(); + /** Initializes the vector using an invalid SourceId and the given frames and + kinematics values. */ + FrameKinematicsVector( + std::initializer_list> init); - /** Sets the kinematics `value` for the frame indicated by the given `id`. - There are various error conditions which will lead to an exception being - thrown: + /** Resets the vector to the given frames and kinematics values .*/ + FrameKinematicsVector& operator=( + std::initializer_list> init); - 1. the id provided is not one of the frame ids provided in the constructor. - 2. clear() hasn't been called. - 3. the value for a particular id is set multiple times between clear() - invocations. + /** Clears all values, resetting the size to zero. */ + void clear(); - If this isn't invoked for _every_ frame id provided at construction, it will - lead to a subsequent exception when SceneGraph consumes the data. */ + /** Sets the kinematics `value` for the frame indicated by the given `id`. */ void set_value(FrameId id, const KinematicsValue& value); + DRAKE_DEPRECATED("2019-08-01", + "The source_id is being removed from FrameKinematicsVector; " + "the SceneGraph no longer requires that it be set.") SourceId source_id() const { return source_id_; } - /** Returns the constructed size of this vector -- the number of FrameId - values it was constructed with. */ - int size() const { return static_cast(values_.size()); } + /** Returns number of frame_ids(). */ + int size() const { + DRAKE_ASSERT_VOID(CheckInvariants()); + return size_; + } /** Returns the value associated with the given `id`. @throws std::runtime_error if `id` is not in the specified set of ids. */ const KinematicsValue& value(FrameId id) const; /** Reports true if the given id is a member of this data. */ - bool has_id(FrameId id) const { return values_.count(id) > 0; } + bool has_id(FrameId id) const; /** Provides a range object for all of the frame ids in the vector. This is intended to be used as: @@ -234,28 +145,29 @@ class FrameKinematicsVector { } @endcode */ - FrameIdRange frame_ids() const { return FrameIdRange(&values_); } + std::vector frame_ids() const; private: - // Utility function to help catch misuse. - struct FlaggedValue { - FlaggedValue() { - detail::KinematicsValueInitializer::Initialize(&value); - } - int64_t version{0}; - KinematicsValue value; - }; - - // The source id for the geometry source reporting data in this vector - SourceId source_id_; + void CheckInvariants() const; - // Mapping from frame id to its current pose (with a flag indicating - // successful update). - std::unordered_map values_; + // TODO(jwnimmer-tri) This field is only here to support the deprecated + // source_id() method; when that method is removed, this field should be + // removed also. + SourceId source_id_; - // The current version tag -- used to detect if values have been properly - // updated. - int64_t version_{0}; + // Mapping from frame id to its current pose. If the map's optional value is + // nullopt, we treat it as if the map key were absent instead. We do this in + // order to avoid reallocating map nodes as we repeatedly clear() and then + // re-set_value() the same IDs over and over again. + // TODO(jwnimmer-tri) A better way to avoid map node allocations would be to + // replace this unordered_map with a flat_hash_map (where the entire storage + // is a single heap slab); in that case, the complicated implementation in + // the cc file would become simplified. + std::unordered_map> values_; + + // The count of non-nullopt items in values_. We could recompute this from + // values_, but we store it separately so that size() is still constant-time. + int size_{0}; }; /** Class for communicating _pose_ information to SceneGraph for registered @@ -267,6 +179,7 @@ class FrameKinematicsVector { - double - AutoDiffXd + - Expression They are already available to link against in the containing library. No other values for T are currently supported. diff --git a/geometry/geometry_state.cc b/geometry/geometry_state.cc index 52490b35496c..947143fb520b 100644 --- a/geometry/geometry_state.cc +++ b/geometry/geometry_state.cc @@ -830,12 +830,13 @@ void GeometryState::CollectIndices( } template -void GeometryState::SetFramePoses(const FramePoseVector& poses) { +void GeometryState::SetFramePoses( + const SourceId source_id, const FramePoseVector& poses) { // TODO(SeanCurtis-TRI): Down the road, make this validation depend on // ASSERT_ARMED. - ValidateFrameIds(poses); + ValidateFrameIds(source_id, poses); const Isometry3 world_pose = Isometry3::Identity(); - for (auto frame_id : source_root_frame_map_[poses.source_id()]) { + for (auto frame_id : source_root_frame_map_[source_id]) { UpdatePosesRecursively(frames_[frame_id], world_pose, poses); } } @@ -843,8 +844,8 @@ void GeometryState::SetFramePoses(const FramePoseVector& poses) { template template void GeometryState::ValidateFrameIds( + const SourceId source_id, const FrameKinematicsVector& kinematics_data) const { - SourceId source_id = kinematics_data.source_id(); auto& frames = GetFramesForSource(source_id); const int ref_frame_count = static_cast(frames.size()); if (ref_frame_count != kinematics_data.size()) { diff --git a/geometry/geometry_state.h b/geometry/geometry_state.h index e57d93a3df53..7a7ce7a382d9 100644 --- a/geometry/geometry_state.h +++ b/geometry/geometry_state.h @@ -543,16 +543,19 @@ class GeometryState { // Sets the kinematic poses for the frames indicated by the given ids. // @param poses The frame id and pose values. + // @pre source_id is a registered source. // @throws std::logic_error If the ids are invalid as defined by // ValidateFrameIds(). - void SetFramePoses(const FramePoseVector& poses); + void SetFramePoses(SourceId source_id, const FramePoseVector& poses); // Confirms that the set of ids provided include _all_ of the frames // registered to the set's source id and that no extra frames are included. // @param values The kinematics values (ids and values) to validate. + // @pre source_id is a registered source. // @throws std::runtime_error if the set is inconsistent with known topology. template - void ValidateFrameIds(const FrameKinematicsVector& values) const; + void ValidateFrameIds(SourceId source_id, + const FrameKinematicsVector& values) const; // Method that performs any final book-keeping/updating on the state after // _all_ of the state's frames have had their poses updated. diff --git a/geometry/scene_graph.cc b/geometry/scene_graph.cc index 04d41b608516..e419e1c696c6 100644 --- a/geometry/scene_graph.cc +++ b/geometry/scene_graph.cc @@ -384,7 +384,7 @@ void SceneGraph::CalcPoseUpdate(const GeometryContext& context, } const auto& poses = pose_port.template Eval>(context); - mutable_state.SetFramePoses(poses); + mutable_state.SetFramePoses(source_id, poses); } } } diff --git a/geometry/scene_graph.h b/geometry/scene_graph.h index de33fbbc52f2..5ec9080d5a2e 100644 --- a/geometry/scene_graph.h +++ b/geometry/scene_graph.h @@ -74,8 +74,8 @@ class QueryObject; FramePoseVector. For each registered frame, this "pose vector" maps the registered FrameId to a pose value. All registered frames must be accounted for and only frames registered by a source can be included in its output port. - See the details in FrameKinematicsVector for details on how to allocate and - calculate this port. + See the details in FrameKinematicsVector for details on how to provide values + for this port. @section geom_sys_outputs Outputs diff --git a/geometry/test/frame_kinematics_vector_test.cc b/geometry/test/frame_kinematics_vector_test.cc index 2652f1400043..896daae8a94a 100644 --- a/geometry/test/frame_kinematics_vector_test.cc +++ b/geometry/test/frame_kinematics_vector_test.cc @@ -1,5 +1,6 @@ #include "drake/geometry/frame_kinematics_vector.h" +#include #include #include @@ -9,6 +10,8 @@ #include "drake/common/symbolic.h" #include "drake/common/test_utilities/eigen_matrix_compare.h" #include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/common/test_utilities/limit_malloc.h" +#include "drake/math/rigid_transform.h" namespace drake { namespace geometry { @@ -22,10 +25,15 @@ ::testing::AssertionResult ExpectExactIdentity(const Isometry3& pose) { GTEST_TEST(FrameKinematicsVector, DefaultConstructor) { const FramePoseVector dut; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" EXPECT_FALSE(dut.source_id().is_valid()); +#pragma GCC diagnostic pop EXPECT_EQ(dut.size(), 0); } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" GTEST_TEST(FrameKinematicsVector, Constructor) { SourceId source_id = SourceId::get_new_id(); @@ -54,22 +62,59 @@ GTEST_TEST(FrameKinematicsVector, Constructor) { FramePoseVector(source_id, duplicate_ids), std::runtime_error, "At least one frame id appears multiple times: \\d+"); } +#pragma GCC diagnostic pop + +GTEST_TEST(FrameKinematicsVector, InitializerListCtor) { + const auto& id_0 = FrameId::get_new_id(); + const Isometry3 pose_0 = Isometry3::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 FramePoseVector 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))); +} + +GTEST_TEST(FrameKinematicsVector, InitializerListAssign) { + const auto& id_0 = FrameId::get_new_id(); + const Isometry3 pose_0 = Isometry3::Identity(); + const auto& id_1 = FrameId::get_new_id(); + const Isometry3 pose_1 = + math::RigidTransformd{Eigen::Translation3d(0.1, 0.2, 0.3)}. + GetAsIsometry3(); + + // 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()}}; + 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))); + + dut = {}; + ASSERT_EQ(dut.size(), 0); +} GTEST_TEST(FrameKinematicsVector, WorkingWithValues) { - SourceId source_id = SourceId::get_new_id(); int kPoseCount = 3; std::vector ids; for (int i = 0; i < kPoseCount; ++i) ids.push_back(FrameId::get_new_id()); - FramePoseVector poses(source_id, ids); + FramePoseVector poses; - // Forgot to call clear. - DRAKE_EXPECT_THROWS_MESSAGE( - poses.set_value(ids[0], Isometry3::Identity()), - std::runtime_error, - "Trying to set kinematics value for the same id .* multiple " - "times. Did you forget to call clear.*?"); - - poses.clear(); std::vector> recorded_poses; for (int i = 0; i < kPoseCount; ++i) { Isometry3 pose = Isometry3::Identity(); @@ -78,22 +123,25 @@ GTEST_TEST(FrameKinematicsVector, WorkingWithValues) { EXPECT_NO_THROW(poses.set_value(ids[i], pose)); } - // Set valid id multiple times. - DRAKE_EXPECT_THROWS_MESSAGE( - poses.set_value(ids[0], Isometry3::Identity()), - std::runtime_error, - "Trying to set kinematics value for the same id .* multiple " - "times. Did you forget to call clear.*?"); + // 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())); + } - // Set invalid id. - DRAKE_EXPECT_THROWS_MESSAGE( - poses.set_value(FrameId::get_new_id(), Isometry3::Identity()), - std::runtime_error, - "Trying to set a kinematics value for a frame id that does not belong " - "to the kinematics vector: \\d+"); + // Confirm that poses get cleared properly. + poses.clear(); + EXPECT_EQ(poses.size(), 0); + EXPECT_FALSE(poses.has_id(ids[0])); - // Confirm that poses get recorded properly. + // Confirm that poses get re-established properly. + std::reverse(recorded_poses.begin(), recorded_poses.end()); + for (int i = 0; i < kPoseCount; ++i) { + EXPECT_NO_THROW(poses.set_value(ids[i], recorded_poses[i])); + } 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())); } @@ -101,35 +149,52 @@ GTEST_TEST(FrameKinematicsVector, WorkingWithValues) { // Ask for the pose of an id that does not belong to the set. DRAKE_EXPECT_THROWS_MESSAGE(poses.value(FrameId::get_new_id()), std::runtime_error, - "Can't acquire value for id \\d+. It is not part " - "of the kinematics data id set."); + "No such FrameId \\d+."); } -GTEST_TEST(FrameKinematicsVector, AutoDiffInstantiation) { - SourceId source_id = SourceId::get_new_id(); - std::vector ids{FrameId::get_new_id(), FrameId::get_new_id()}; - const int kCount = static_cast(ids.size()); - FramePoseVector poses(source_id, ids); +GTEST_TEST(FrameKinematicsVector, SetWithoutAllocations) { + const int kPoseCount = 3; + const std::vector ids{ + FrameId::get_new_id(), + FrameId::get_new_id(), + FrameId::get_new_id(), + }; + const std::vector> poses{ + Isometry3::Identity(), + Isometry3::Identity(), + Isometry3::Identity(), + }; + + // For the initial setting, we'd expect to see allocations. + FramePoseVector dut; + for (int i = 0; i < kPoseCount; ++i) { + dut.set_value(ids[i], poses[i]); + } - EXPECT_EQ(poses.source_id(), source_id); - EXPECT_EQ(poses.size(), kCount); + // Subsequent clear + set should not touch the heap. + for (int j = 0; j < 5; ++j) { + drake::test::LimitMalloc guard; + dut.clear(); + for (int i = 0; i < kPoseCount; ++i) { + dut.set_value(ids[i], poses[i]); + } + } +} + +GTEST_TEST(FrameKinematicsVector, AutoDiffInstantiation) { + FramePoseVector poses; + poses.set_value(FrameId::get_new_id(), Isometry3::Identity()); + EXPECT_EQ(poses.size(), 1); } GTEST_TEST(FrameKinematicsVector, SymbolicInstantiation) { using symbolic::Expression; using symbolic::Variable; - SourceId source_id = SourceId::get_new_id(); std::vector ids{FrameId::get_new_id(), FrameId::get_new_id()}; - const int kCount = static_cast(ids.size()); - FramePoseVector poses(source_id, ids); - - EXPECT_EQ(poses.source_id(), source_id); - EXPECT_EQ(poses.size(), kCount); + FramePoseVector poses; // Set and retrieve a simple symbolic::Expression. - poses.clear(); - poses.set_value(ids[0], Isometry3::Identity()); const Variable var_x_{"x"}; @@ -149,11 +214,12 @@ GTEST_TEST(FrameKinematicsVector, SymbolicInstantiation) { } GTEST_TEST(FrameKinematicsVector, FrameIdRange) { - SourceId source_id = SourceId::get_new_id(); - int kPoseCount = 3; + FramePoseVector poses; std::vector ids; - for (int i = 0; i < kPoseCount; ++i) ids.push_back(FrameId::get_new_id()); - FramePoseVector poses(source_id, ids); + for (int i = 0; i < 3; ++i) { + ids.push_back(FrameId::get_new_id()); + poses.set_value(ids.back(), Isometry3::Identity()); + } std::set actual_ids; for (FrameId id : poses.frame_ids()) actual_ids.insert(id); diff --git a/geometry/test/geometry_state_test.cc b/geometry/test/geometry_state_test.cc index 1ff14dff93e5..ff0d8b89a69c 100644 --- a/geometry/test/geometry_state_test.cc +++ b/geometry/test/geometry_state_test.cc @@ -94,8 +94,8 @@ class GeometryStateTester { return state_->X_PF_; } - void SetFramePoses(const FramePoseVector& poses) { - state_->SetFramePoses(poses); + void SetFramePoses(SourceId source_id, const FramePoseVector& poses) { + state_->SetFramePoses(source_id, poses); } void FinalizePoseUpdate() { @@ -103,8 +103,9 @@ class GeometryStateTester { } template - void ValidateFrameIds(const FrameKinematicsVector& data) const { - state_->ValidateFrameIds(data); + void ValidateFrameIds(SourceId source_id, + const FrameKinematicsVector& data) const { + state_->ValidateFrameIds(source_id, data); } int peek_next_clique() const { @@ -789,12 +790,11 @@ TEST_F(GeometryStateTest, ValidateSingleSourceTree) { } // Confirm posing positions the frames properly. - FramePoseVector poses(source_id_, frames_); - poses.clear(); + FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { poses.set_value(frames_[f], X_PF_[f]); } - gs_tester_.SetFramePoses(poses); + gs_tester_.SetFramePoses(s_id, poses); gs_tester_.FinalizePoseUpdate(); test_frame(FrameIndex(0), gs_tester_.get_world_frame(), 0); @@ -1201,12 +1201,11 @@ TEST_F(GeometryStateTest, RegisterAnchoredNullGeometry) { TEST_F(GeometryStateTest, RemoveGeometry) { SourceId s_id = SetUpSingleSourceTree(true); // Pose all of the frames to the specified poses in their parent frame. - FramePoseVector poses(source_id_, frames_); - poses.clear(); + FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { poses.set_value(frames_[f], X_PF_[f]); } - gs_tester_.SetFramePoses(poses); + gs_tester_.SetFramePoses(s_id, poses); gs_tester_.FinalizePoseUpdate(); // The geometry to remove, its parent frame, and its engine index. @@ -1300,12 +1299,11 @@ TEST_F(GeometryStateTest, RemoveGeometry) { TEST_F(GeometryStateTest, RemoveGeometryTree) { SourceId s_id = SetUpSingleSourceTree(true); // Pose all of the frames to the specified poses in their parent frame. - FramePoseVector poses(source_id_, frames_); - poses.clear(); + FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { poses.set_value(frames_[f], X_PF_[f]); } - gs_tester_.SetFramePoses(poses); + gs_tester_.SetFramePoses(s_id, poses); gs_tester_.FinalizePoseUpdate(); // The geometry to remove, its parent frame, and its proximity index. @@ -1558,33 +1556,30 @@ TEST_F(GeometryStateTest, GetFrameIdFromBadId) { // Tests the validation of the ids provided in a frame kinematics vector. TEST_F(GeometryStateTest, ValidateFrameIds) { SourceId s_id = SetUpSingleSourceTree(); - FramePoseVector frame_set(s_id, frames_); - frame_set.clear(); + FramePoseVector frame_set; for (auto frame_id : frames_) { frame_set.set_value(frame_id, Isometry3::Identity()); } // Case: frame ids are valid. - EXPECT_NO_THROW(gs_tester_.ValidateFrameIds(frame_set)); + EXPECT_NO_THROW(gs_tester_.ValidateFrameIds(s_id, frame_set)); // Case: Right number, wrong frames. - vector bad_frames; + FramePoseVector frame_set_2; for (int i = 0; i < kFrameCount; ++i) { - bad_frames.push_back(FrameId::get_new_id()); + frame_set_2.set_value(FrameId::get_new_id(), Isometry3d::Identity()); } - FramePoseVector frame_set_2(s_id, bad_frames); DRAKE_EXPECT_THROWS_MESSAGE( - gs_tester_.ValidateFrameIds(frame_set_2), std::runtime_error, + gs_tester_.ValidateFrameIds(s_id, frame_set_2), std::runtime_error, "Registered frame id \\(\\d+\\) belonging to source \\d+ was not found " "in the provided kinematics data."); // Case: Too few frames. - vector missing_frames; + FramePoseVector frame_set_3; for (int i = 0; i < kFrameCount - 1; ++i) { - missing_frames.push_back(frames_[i]); + frame_set.set_value(frames_[i], Isometry3::Identity()); } - FramePoseVector frame_set_3(s_id, missing_frames); DRAKE_EXPECT_THROWS_MESSAGE( - gs_tester_.ValidateFrameIds(frame_set_3), std::runtime_error, + gs_tester_.ValidateFrameIds(s_id, frame_set_3), std::runtime_error, "Disagreement in expected number of frames \\(\\d+\\)" " and the given number of frames \\(\\d+\\)."); } @@ -1605,10 +1600,9 @@ TEST_F(GeometryStateTest, SetFramePoses) { } auto make_pose_vector = - [&s_id, &frame_poses, this]() -> FramePoseVector { + [&frame_poses, this]() -> FramePoseVector { const int count = static_cast(this->frames_.size()); - FramePoseVector poses(s_id, this->frames_); - poses.clear(); + FramePoseVector poses; for (int i = 0; i < count; ++i) { poses.set_value(this->frames_[i], frame_poses[i]); } @@ -1621,7 +1615,7 @@ TEST_F(GeometryStateTest, SetFramePoses) { // Case 1: Set all frames to identity poses. The world pose of all the // geometry should be that of the geometry in its frame. FramePoseVector poses1 = make_pose_vector(); - gs_tester_.SetFramePoses(poses1); + gs_tester_.SetFramePoses(s_id, poses1); const auto& world_poses = gs_tester_.get_geometry_world_poses(); for (int i = 0; i < kFrameCount * kGeometryCount; ++i) { EXPECT_TRUE(CompareMatrices(world_poses[i].matrix().block<3, 4>(0, 0), @@ -1636,7 +1630,7 @@ TEST_F(GeometryStateTest, SetFramePoses) { frame_poses[0] = offset; frame_poses[1] = offset; FramePoseVector poses2 = make_pose_vector(); - gs_tester_.SetFramePoses(poses2); + gs_tester_.SetFramePoses(s_id, poses2); for (int i = 0; i < kFrameCount * kGeometryCount; ++i) { EXPECT_TRUE( CompareMatrices(world_poses[i].matrix().block<3, 4>(0, 0), @@ -1647,7 +1641,7 @@ TEST_F(GeometryStateTest, SetFramePoses) { // 0, 1, 2, & 3 moved up 1, and geometries 4 & 5 moved up two. frame_poses[2] = offset; FramePoseVector poses3 = make_pose_vector(); - gs_tester_.SetFramePoses(poses3); + gs_tester_.SetFramePoses(s_id, poses3); for (int i = 0; i < (kFrameCount - 1) * kGeometryCount; ++i) { EXPECT_TRUE( CompareMatrices(world_poses[i].matrix().block<3, 4>(0, 0), @@ -1682,10 +1676,9 @@ TEST_F(GeometryStateTest, QueryFrameProperties) { "No frame name available for invalid frame id: \\d+"); // Set the frame poses to query geometry and frame poses. - FramePoseVector poses(s_id, frames_); - poses.clear(); + FramePoseVector poses; for (int i = 0; i < kFrameCount; ++i) poses.set_value(frames_[i], X_PF_[i]); - gs_tester_.SetFramePoses(poses); + gs_tester_.SetFramePoses(s_id, poses); EXPECT_TRUE( CompareMatrices(geometry_state_.get_pose_in_world(frames_[0]).matrix(), @@ -1791,12 +1784,11 @@ TEST_F(GeometryStateTest, ExcludeCollisionsWithin) { SetUpSingleSourceTree(true /* assign proximity roles */); // Pose all of the frames to the specified poses in their parent frame. - FramePoseVector poses(source_id_, frames_); - poses.clear(); + FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { poses.set_value(frames_[f], X_PF_[f]); } - gs_tester_.SetFramePoses(poses); + gs_tester_.SetFramePoses(source_id_, poses); gs_tester_.FinalizePoseUpdate(); // This is *non* const; we'll decrement it as we filter more and more @@ -1864,12 +1856,11 @@ TEST_F(GeometryStateTest, ExcludeCollisionsBetween) { SetUpSingleSourceTree(true /* add proximity roles */); // Pose all of the frames to the specified poses in their parent frame. - FramePoseVector poses(source_id_, frames_); - poses.clear(); + FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { poses.set_value(frames_[f], X_PF_[f]); } - gs_tester_.SetFramePoses(poses); + gs_tester_.SetFramePoses(source_id_, poses); gs_tester_.FinalizePoseUpdate(); // This is *non* const; we'll decrement it as we filter more and more @@ -1904,12 +1895,11 @@ TEST_F(GeometryStateTest, NonProximityRoleInCollisionFilter) { SetUpSingleSourceTree(true /* add proximity roles */); // Pose all of the frames to the specified poses in their parent frame. - FramePoseVector poses(source_id_, frames_); - poses.clear(); + FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { poses.set_value(frames_[f], X_PF_[f]); } - gs_tester_.SetFramePoses(poses); + gs_tester_.SetFramePoses(source_id_, poses); gs_tester_.FinalizePoseUpdate(); // This is *non* const; we'll decrement it as we filter more and more diff --git a/geometry/test/scene_graph_test.cc b/geometry/test/scene_graph_test.cc index 3b1e4abe7718..254e74a37afb 100644 --- a/geometry/test/scene_graph_test.cc +++ b/geometry/test/scene_graph_test.cc @@ -413,11 +413,7 @@ class GeometrySourceSystem : public systems::LeafSystem { frame_ids_.push_back(f_id); // Set up output port now that the frame is registered. - std::vector all_ids{f_id}; - all_ids.insert(all_ids.end(), extra_frame_ids_.begin(), - extra_frame_ids_.end()); this->DeclareAbstractOutputPort( - FramePoseVector(source_id_, all_ids), &GeometrySourceSystem::CalcFramePoseOutput); } SourceId get_source_id() const { return source_id_; } @@ -440,11 +436,6 @@ class GeometrySourceSystem : public systems::LeafSystem { // Populate with the pose data. void CalcFramePoseOutput(const Context& context, FramePoseVector* poses) const { - const int frame_count = - static_cast(frame_ids_.size() + extra_frame_ids_.size()); - DRAKE_DEMAND(poses->size() == frame_count); - DRAKE_DEMAND(poses->source_id() == source_id_); - poses->clear(); const int base_count = static_cast(frame_ids_.size()); @@ -597,9 +588,9 @@ GTEST_TEST(SceneGraphVisualizationTest, NoWorldInPoseVector) { // bundle. EXPECT_EQ(0, poses.get_num_poses()); auto context = scene_graph.AllocateContext(); - FramePoseVector pose_vector(s_id, {f_id}); - context->FixInputPort(scene_graph.get_source_pose_port(s_id).get_index(), - Value>(pose_vector)); + const FramePoseVector pose_vector{{ + f_id, Isometry3::Identity()}}; + scene_graph.get_source_pose_port(s_id).FixValue(context.get(), pose_vector); EXPECT_NO_THROW( SceneGraphTester::CalcPoseBundle(scene_graph, *context, &poses)); } @@ -623,9 +614,9 @@ GTEST_TEST(SceneGraphVisualizationTest, NoWorldInPoseVector) { // frame to be included in the bundle. EXPECT_EQ(0, poses.get_num_poses()); auto context = scene_graph.AllocateContext(); - FramePoseVector pose_vector(s_id, {f_id}); - context->FixInputPort(scene_graph.get_source_pose_port(s_id).get_index(), - Value>(pose_vector)); + const FramePoseVector pose_vector{{ + f_id, Isometry3::Identity()}}; + scene_graph.get_source_pose_port(s_id).FixValue(context.get(), pose_vector); EXPECT_NO_THROW(SceneGraphTester::CalcPoseBundle(scene_graph, *context, &poses)); } diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index 41a3d56a2db8..144920e94107 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -1976,26 +1976,8 @@ template void MultibodyPlant::DeclareSceneGraphPorts() { geometry_query_port_ = this->DeclareAbstractInputPort( "geometry_query", Value>{}).get_index(); - // Allocate pose port. - // TODO(eric.cousineau): Simplify this logic. - typename systems::LeafOutputPort::AllocCallback pose_alloc = [this]() { - // This presupposes that the source id has been assigned and _all_ frames - // have been registered. - std::vector ids; - for (auto it : this->body_index_to_frame_id_) { - if (it.first == world_index()) continue; - ids.push_back(it.second); - } - return AbstractValue::Make( - FramePoseVector(*this->source_id_, ids)); - }; - typename systems::LeafOutputPort::CalcCallback pose_callback = [this]( - const Context& context, AbstractValue* value) { - this->CalcFramePoseOutput( - context, &value->get_mutable_value>()); - }; geometry_pose_port_ = this->DeclareAbstractOutputPort( - "geometry_pose", pose_alloc, pose_callback, + "geometry_pose", &MultibodyPlant::CalcFramePoseOutput, {this->configuration_ticket()}).get_index(); } @@ -2004,14 +1986,12 @@ void MultibodyPlant::CalcFramePoseOutput( const Context& context, FramePoseVector* poses) const { DRAKE_MBP_THROW_IF_NOT_FINALIZED(); DRAKE_ASSERT(source_id_ != nullopt); - // NOTE: The body index to frame id map *always* includes the world body but - // the world body does *not* get reported in the frame poses; only dynamic - // frames do. - DRAKE_ASSERT( - poses->size() == static_cast(body_index_to_frame_id_.size() - 1)); const internal::PositionKinematicsCache& pc = EvalPositionKinematics(context); + // NOTE: The body index to frame id map *always* includes the world body but + // the world body does *not* get reported in the frame poses; only dynamic + // frames do. // TODO(amcastro-tri): Make use of Body::EvalPoseInWorld(context) once caching // lands. poses->clear(); diff --git a/multibody/plant/test/multibody_plant_test.cc b/multibody/plant/test/multibody_plant_test.cc index d6b5ba013cc4..155a271e2edd 100644 --- a/multibody/plant/test/multibody_plant_test.cc +++ b/multibody/plant/test/multibody_plant_test.cc @@ -686,11 +686,10 @@ TEST_F(AcrobotPlantTests, VisualGeometryRegistration) { EXPECT_NO_THROW(poses_value->get_value>()); const FramePoseVector& poses = poses_value->get_value>(); - EXPECT_EQ(poses.source_id(), plant_->get_source_id()); - EXPECT_EQ(poses.size(), 2); // Only two frames move. // Compute the poses for each geometry in the model. plant_->get_geometry_poses_output_port().Calc(*context, poses_value.get()); + EXPECT_EQ(poses.size(), 2); // Only two frames move. const FrameId world_frame_id = plant_->GetBodyFrameIdOrThrow(plant_->world_body().index()); @@ -1170,11 +1169,10 @@ GTEST_TEST(MultibodyPlantTest, CollisionGeometryRegistration) { EXPECT_NO_THROW(poses_value->get_value>()); const FramePoseVector& pose_data = poses_value->get_value>(); - EXPECT_EQ(pose_data.source_id(), plant.get_source_id()); - EXPECT_EQ(pose_data.size(), 2); // Only two frames move. // Compute the poses for each geometry in the model. plant.get_geometry_poses_output_port().Calc(*context, poses_value.get()); + EXPECT_EQ(pose_data.size(), 2); // Only two frames move. const double kTolerance = 5 * std::numeric_limits::epsilon(); for (BodyIndex body_index(1); diff --git a/systems/rendering/render_pose_to_geometry_pose.cc b/systems/rendering/render_pose_to_geometry_pose.cc index 4b3c0e9a7367..32cb2fb4b7b3 100644 --- a/systems/rendering/render_pose_to_geometry_pose.cc +++ b/systems/rendering/render_pose_to_geometry_pose.cc @@ -22,16 +22,13 @@ RenderPoseToGeometryPose::RenderPoseToGeometryPose( using Output = geometry::FramePoseVector; this->DeclareVectorInputPort(Input{}); this->DeclareAbstractOutputPort( - [source_id, frame_id]() { - const std::vector frame_ids{frame_id}; - const Output result(source_id, frame_ids); - return Value::Make(result); + []() { + return Value{}.Clone(); }, [this, frame_id](const Context& context, AbstractValue* calculated) { const Input& input = get_input_port().template Eval(context); - Output& output = calculated->get_mutable_value(); - output.clear(); - output.set_value(frame_id, input.get_isometry()); + calculated->get_mutable_value() = + {{frame_id, input.get_isometry()}}; }); } diff --git a/systems/rendering/test/multibody_position_to_geometry_pose_test.cc b/systems/rendering/test/multibody_position_to_geometry_pose_test.cc index 1e48ba528be7..743308e841fd 100644 --- a/systems/rendering/test/multibody_position_to_geometry_pose_test.cc +++ b/systems/rendering/test/multibody_position_to_geometry_pose_test.cc @@ -35,7 +35,6 @@ GTEST_TEST(MultibodyPositionToGeometryPoseTest, InputOutput) { const auto& output = dut.get_output_port().Eval>(*context); - EXPECT_EQ(output.source_id(), mbp.get_source_id()); for (multibody::BodyIndex i(0); i < mbp.num_bodies(); i++) { if (i == mbp.world_body().index()) { // The world geometry will not appear in the poses. diff --git a/systems/rendering/test/render_pose_to_geometry_pose_test.cc b/systems/rendering/test/render_pose_to_geometry_pose_test.cc index 7a94ff62e151..400c045f1f61 100644 --- a/systems/rendering/test/render_pose_to_geometry_pose_test.cc +++ b/systems/rendering/test/render_pose_to_geometry_pose_test.cc @@ -26,7 +26,6 @@ GTEST_TEST(RenderPoseToGeometryPoseTest, InputOutput) { const auto& output = dut.get_output_port().Eval>( *context); - EXPECT_EQ(output.source_id(), source_id); EXPECT_EQ(output.size(), 1); ASSERT_TRUE(output.has_id(frame_id)); EXPECT_TRUE(CompareMatrices( diff --git a/systems/sensors/test/optitrack_sender_test.cc b/systems/sensors/test/optitrack_sender_test.cc index 8367dcd0b0e9..b7438b5e822e 100644 --- a/systems/sensors/test/optitrack_sender_test.cc +++ b/systems/sensors/test/optitrack_sender_test.cc @@ -18,7 +18,6 @@ using optitrack::optitrack_frame_t; // ensures that the TrackedBody input to the OptitrackLcmFrameSender system // matches the output `optitrack_frame_t` object. GTEST_TEST(OptitrackSenderTest, OptitrackLcmSenderTest) { - geometry::SourceId source_id = geometry::SourceId::get_new_id(); geometry::FrameId frame_id = geometry::FrameId::get_new_id(); std::vector frame_ids{frame_id}; int optitrack_id = 11; @@ -28,18 +27,16 @@ GTEST_TEST(OptitrackSenderTest, OptitrackLcmSenderTest) { "test_body", optitrack_id); OptitrackLcmFrameSender dut(frame_map); - geometry::FramePoseVector pose_vector(source_id, frame_ids); - pose_vector.clear(); - constexpr double tx = 0.2; // x-translation for the test object constexpr double ty = 0.4; // y-translation for the test object constexpr double tz = 0.7; // z-translation for the test object // Sets up a test body with an arbitrarily chosen pose. Eigen::Vector3d axis(1 / sqrt(3), 1 / sqrt(3), 1 / sqrt(3)); - pose_vector.set_value(frame_id, - Eigen::Isometry3d(Eigen::AngleAxis(0.2, axis)). - pretranslate(Eigen::Vector3d(tx, ty, tz))); + const geometry::FramePoseVector pose_vector{{ + frame_id, + Eigen::Isometry3d(Eigen::AngleAxis(0.2, axis)). + pretranslate(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);