geometry: Make FrameKinematicsVector less ceremonial (RobotLocomotion…

…#11302)

The required call pattern ceremony is difficult to get right when
writing systems that produce frames:

(1) The FlaggedValue (serial numbering) is awkward for output ports to
provide, and unlikely to catch real bugs.

(2) The IDs can be checked by the SceneGraph based solely on the FrameId.
Requiring that callers copy the SourceId into the output port's value is
useless and painful.

attic/manipulation/util/frame_pose_tracker.cc

@@ -100,7 +100,6 @@ void FramePoseTracker::Init() {
 
   pose_vector_output_port_index_ =
       this->DeclareAbstractOutputPort(
-          geometry::FramePoseVector<double>(source_id_, frame_ids),
           &FramePoseTracker::OutputStatus).get_index();
 }
 

attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc

@@ -97,9 +97,7 @@ RigidBodyPlantBridge<T>::RigidBodyPlantBridge(const RigidBodyTree<T>* tree,
   // second body id.
   std::vector<FrameId> dynamic_frames(body_ids_.begin() + 1, body_ids_.end());
   geometry_pose_port_ = this->DeclareAbstractOutputPort(
-          FramePoseVector<T>(source_id_, dynamic_frames),
-          &RigidBodyPlantBridge::CalcFramePoseOutput)
-      .get_index();
+          &RigidBodyPlantBridge::CalcFramePoseOutput).get_index();
 }
 
 template <typename T>
@@ -188,9 +186,6 @@ void RigidBodyPlantBridge<T>::RegisterTree(SceneGraph<T>* scene_graph) {
 template <typename T>
 void RigidBodyPlantBridge<T>::CalcFramePoseOutput(
     const MyContext& context, FramePoseVector<T>* poses) const {
-  DRAKE_DEMAND(source_id_.is_valid());
-  DRAKE_DEMAND(poses->size() == static_cast<int>(body_ids_.size() - 1));
-
   const BasicVector<T>& input_vector = *this->EvalVectorInput(context, 0);
   // Obtains the generalized positions from vector_base.
   const VectorX<T> q = input_vector.CopyToVector().head(

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",
     ],

bindings/pydrake/geometry_py.cc

@@ -77,9 +77,12 @@ PYBIND11_MODULE(geometry, m) {
   py::class_<FramePoseVector<T>>(
       m, "FramePoseVector", doc.FrameKinematicsVector.doc)
       .def(py::init<>(), doc.FrameKinematicsVector.ctor.doc_0args)
-      .def(py::init<SourceId, const std::vector<FrameId>&>(),
+      .def(py::init([](SourceId source_id, const std::vector<FrameId>& ids) {
+        WarnDeprecated("See API docs for deprecation notice.");
+        return std::make_unique<FramePoseVector<T>>(source_id, ids);
+      }),
           py::arg("source_id"), py::arg("ids"),
-          doc.FrameKinematicsVector.ctor.doc_2args);
+          doc.FrameKinematicsVector.ctor.doc_deprecated_2args);
   pysystems::AddValueInstantiation<FramePoseVector<T>>(m);
 
   py::class_<QueryObject<T>>(m, "QueryObject", doc.QueryObject.doc)

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).

examples/pendulum/pendulum_geometry.cc

@@ -95,16 +95,13 @@ PendulumGeometry::PendulumGeometry(geometry::SceneGraph<double>* scene_graph) {
 void PendulumGeometry::OutputGeometryPose(
     const systems::Context<double>& context,
     geometry::FramePoseVector<double>* poses) const {
-  DRAKE_DEMAND(source_id_.is_valid());
   DRAKE_DEMAND(frame_id_.is_valid());
 
   const auto& input = get_input_port(0).Eval<PendulumState<double>>(context);
   const double theta = input.theta();
   const math::RigidTransformd pose(math::RotationMatrixd::MakeYRotation(theta));
 
-  *poses = geometry::FramePoseVector<double>(source_id_, {frame_id_});
-  poses->clear();
-  poses->set_value(frame_id_, pose.GetAsIsometry3());
+  *poses = {{frame_id_, pose.GetAsIsometry3()}};
 }
 
 }  // namespace pendulum

examples/quadrotor/quadrotor_plant.cc

@@ -151,9 +151,7 @@ systems::OutputPortIndex QuadrotorPlant<T>::AllocateGeometryPoseOutputPort() {
   DRAKE_DEMAND(source_id_.is_valid() && frame_id_.is_valid());
   return this
       ->DeclareAbstractOutputPort(
-          "geometry_pose",
-          geometry::FramePoseVector<T>(source_id_, {frame_id_}),
-          &QuadrotorPlant<T>::CopyPoseOut)
+          "geometry_pose", &QuadrotorPlant<T>::CopyPoseOut)
       .get_index();
 }
 
@@ -184,16 +182,11 @@ void QuadrotorPlant<T>::RegisterGeometry(
 template <typename T>
 void QuadrotorPlant<T>::CopyPoseOut(const systems::Context<T>& context,
                                    geometry::FramePoseVector<T>* poses) const {
-  DRAKE_DEMAND(poses->size() == 1);
-  DRAKE_DEMAND(poses->source_id() == source_id_);
-
   VectorX<T> state = context.get_continuous_state_vector().CopyToVector();
-
-  poses->clear();
   math::RigidTransform<T> pose(
       math::RollPitchYaw<T>(state.template segment<3>(3)),
       state.template head<3>());
-  poses->set_value(frame_id_, pose.GetAsIsometry3());
+  *poses = {{frame_id_, pose.GetAsIsometry3()}};
 }
 
 std::unique_ptr<systems::AffineSystem<double>> StabilizingLQRController(

examples/scene_graph/bouncing_ball_plant.cc

@@ -31,9 +31,9 @@ template <typename T>
 BouncingBallPlant<T>::BouncingBallPlant(SourceId source_id,
                                         SceneGraph<T>* scene_graph,
                                         const Vector2<double>& 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<geometry::QueryObject<T>>{})
@@ -61,7 +61,6 @@ BouncingBallPlant<T>::BouncingBallPlant(SourceId source_id,
 
   // Allocate the output port now that the frame has been registered.
   geometry_pose_port_ = this->DeclareAbstractOutputPort(
-          FramePoseVector<double>(source_id_, {ball_frame_id_}),
           &BouncingBallPlant::CalcFramePoseOutput,
           {this->configuration_ticket()})
       .get_index();
@@ -99,14 +98,10 @@ void BouncingBallPlant<T>::CopyStateToOutput(
 template <typename T>
 void BouncingBallPlant<T>::CalcFramePoseOutput(
     const Context<T>& context, FramePoseVector<T>* poses) const {
-  DRAKE_DEMAND(poses->source_id() == source_id_);
-  DRAKE_DEMAND(poses->size() == 1);
-
   Isometry3<T> pose = Isometry3<T>::Identity();
   const BouncingBallVector<T>& 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.

examples/scene_graph/bouncing_ball_plant.h

@@ -106,8 +106,6 @@ class BouncingBallPlant : public systems::LeafSystem<T> {
     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<double> p_WB_;

examples/scene_graph/dev/bouncing_ball_plant.cc

@@ -31,9 +31,9 @@ template <typename T>
 BouncingBallPlant<T>::BouncingBallPlant(SourceId source_id,
                                         SceneGraph<T>* scene_graph,
                                         const Vector2<double>& 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<geometry::QueryObject<T>>{})
@@ -61,7 +61,6 @@ BouncingBallPlant<T>::BouncingBallPlant(SourceId source_id,
 
   // Allocate the output port now that the frame has been registered.
   geometry_pose_port_ = this->DeclareAbstractOutputPort(
-          FramePoseVector<double>(source_id_, {ball_frame_id_}),
           &BouncingBallPlant::CalcFramePoseOutput,
           {this->configuration_ticket()})
       .get_index();
@@ -99,14 +98,10 @@ void BouncingBallPlant<T>::CopyStateToOutput(
 template <typename T>
 void BouncingBallPlant<T>::CalcFramePoseOutput(
     const Context<T>& context, FramePoseVector<T>* poses) const {
-  DRAKE_DEMAND(poses->source_id() == source_id_);
-  DRAKE_DEMAND(poses->size() == 1);
-
   Isometry3<T> pose = Isometry3<T>::Identity();
   const BouncingBallVector<T>& 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.

examples/scene_graph/dev/bouncing_ball_plant.h

@@ -106,8 +106,6 @@ class BouncingBallPlant : public systems::LeafSystem<T> {
     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<double> p_WB_;

examples/scene_graph/dev/solar_system.cc

@@ -61,7 +61,6 @@ SolarSystem<T>::SolarSystem(SceneGraph<T>* scene_graph) {
 
   // Now that frames have been registered, allocate the output port.
   geometry_pose_port_ = this->DeclareAbstractOutputPort(
-          FramePoseVector<T>(source_id_, body_ids_),
           &SolarSystem::CalcFramePoseOutput,
           {this->configuration_ticket()})
       .get_index();
@@ -262,11 +261,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
 template <typename T>
 void SolarSystem<T>::CalcFramePoseOutput(const Context<T>& context,
                                          FramePoseVector<T>* poses) const {
-  DRAKE_DEMAND(poses->size() == kBodyCount);
-  DRAKE_DEMAND(poses->source_id() == source_id_);
-
   const BasicVector<T>& state = get_state(context);
-
   poses->clear();
   for (int i = 0; i < kBodyCount; ++i) {
     Isometry3<T> pose = body_offset_[i];

examples/scene_graph/solar_system.cc

@@ -50,7 +50,6 @@ SolarSystem<T>::SolarSystem(SceneGraph<T>* scene_graph) {
 
   // Now that frames have been registered, allocate the output port.
   geometry_pose_port_ = this->DeclareAbstractOutputPort(
-          FramePoseVector<T>(source_id_, body_ids_),
           &SolarSystem::CalcFramePoseOutput,
           {this->configuration_ticket()})
       .get_index();
@@ -319,11 +318,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
 template <typename T>
 void SolarSystem<T>::CalcFramePoseOutput(const Context<T>& context,
                                          FramePoseVector<T>* poses) const {
-  DRAKE_DEMAND(poses->size() == kBodyCount);
-  DRAKE_DEMAND(poses->source_id() == source_id_);
-
   const BasicVector<T>& state = get_state(context);
-
   poses->clear();
   for (int i = 0; i < kBodyCount; ++i) {
     Isometry3<T> pose = body_offset_[i];

geometry/BUILD.bazel

@@ -282,6 +282,8 @@ drake_cc_googletest(
     deps = [
         ":frame_kinematics",
         "//common/test_utilities",
+        "//common/test_utilities:limit_malloc",
+        "//math:geometric_transform",
     ],
 )
 

geometry/dev/geometry_state.cc

@@ -1025,21 +1025,22 @@ void GeometryState<T>::CollectIndices(
 }
 
 template <typename T>
-void GeometryState<T>::SetFramePoses(const FramePoseVector<T>& poses) {
+void GeometryState<T>::SetFramePoses(
+    const SourceId source_id, const FramePoseVector<T>& poses) {
   // TODO(SeanCurtis-TRI): Down the road, make this validation depend on
   // ASSERT_ARMED.
-  ValidateFrameIds(poses);
+  ValidateFrameIds(source_id, poses);
   const Isometry3<T> world_pose = Isometry3<T>::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);
   }
 }
 
 template <typename T>
 template <typename ValueType>
 void GeometryState<T>::ValidateFrameIds(
+    const SourceId source_id,
     const FrameKinematicsVector<ValueType>& kinematics_data) const {
-  SourceId source_id = kinematics_data.source_id();
   auto& frames = GetFramesForSource(source_id);
   const int ref_frame_count = static_cast<int>(frames.size());
   if (ref_frame_count != kinematics_data.size()) {

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<T>& poses);
+  void SetFramePoses(SourceId source_id, const FramePoseVector<T>& 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 <typename ValueType>
-  void ValidateFrameIds(const FrameKinematicsVector<ValueType>& values) const;
+  void ValidateFrameIds(SourceId source_id,
+                        const FrameKinematicsVector<ValueType>& 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.

geometry/dev/scene_graph.cc

@@ -456,7 +456,7 @@ void SceneGraph<T>::CalcPoseUpdate(const GeometryContext<T>& context,
         }
         const auto& poses =
             pose_port.template Eval<FramePoseVector<T>>(context);
-        mutable_state.SetFramePoses(poses);
+        mutable_state.SetFramePoses(source_id, poses);
       }
     }
   }