Exclude z from MBP::get_state_output_port(). (RobotLocomotion#12619)

YinjieLin · Jan 24, 2020 · 696be03 · 696be03
1 parent 5ec4568
commit 696be03
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 31 deletions.
diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc
@@ -2197,7 +2197,7 @@ void MultibodyPlant<T>::DeclareStateCacheAndPorts() {
   state_output_port_ =
       this->DeclareVectorOutputPort("continuous_state",
                                     BasicVector<T>(num_multibody_states()),
-                                    &MultibodyPlant::CopyContinuousStateOut,
+                                    &MultibodyPlant::CopyMultibodyStateOut,
                                     {this->all_state_ticket()})
           .get_index();
 
@@ -2236,7 +2236,7 @@ void MultibodyPlant<T>::DeclareStateCacheAndPorts() {
                 [this, model_instance_index](
                     const systems::Context<T>& context,
                     systems::BasicVector<T>* result) {
-                  this->CopyContinuousStateOut(model_instance_index, context,
+                  this->CopyMultibodyStateOut(model_instance_index, context,
                                                result);
                 },
                 {this->all_state_ticket()})
@@ -2547,35 +2547,22 @@ void MultibodyPlant<T>::DeclareCacheEntries() {
               cache_indexes_.spatial_contact_forces_continuous)});
   cache_indexes_.generalized_contact_forces_continuous =
       generalized_contact_forces_continuous_cache_entry.cache_index();
-      }
-
-      template <typename T>
-      const systems::BasicVector<T> &
-      MultibodyPlant<T>::GetStateVector(const Context<T>& context) const {
-    if (is_discrete()) {
-      return context.get_discrete_state(0);
-    } else {
-      return dynamic_cast<const systems::BasicVector<T>&>(
-          context.get_continuous_state_vector());
-    }
 }
 
 template <typename T>
-void MultibodyPlant<T>::CopyContinuousStateOut(
+void MultibodyPlant<T>::CopyMultibodyStateOut(
     const Context<T>& context, BasicVector<T>* state_vector) const {
   DRAKE_MBP_THROW_IF_NOT_FINALIZED();
-  state_vector->SetFrom(GetStateVector(context));
+  state_vector->SetFromVector(GetPositionsAndVelocities(context));
 }
 
 template <typename T>
-void MultibodyPlant<T>::CopyContinuousStateOut(
+void MultibodyPlant<T>::CopyMultibodyStateOut(
     ModelInstanceIndex model_instance,
     const Context<T>& context, BasicVector<T>* state_vector) const {
   DRAKE_MBP_THROW_IF_NOT_FINALIZED();
-
-  VectorX<T> instance_state_vector =
-      GetPositionsAndVelocities(context, model_instance);
-  state_vector->SetFromVector(instance_state_vector);
+  state_vector->SetFromVector(
+      GetPositionsAndVelocities(context, model_instance));
 }
 
 template <typename T>

diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h
@@ -412,8 +412,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   /// SceneGraph.
   const systems::InputPort<T>& get_geometry_query_input_port() const;
 
-  /// Returns a constant reference to the output port for the full state
-  /// x = [q v] of the model.
+  /// Returns a constant reference to the output port for the multibody state
+  /// x = [q, v] of the model.
   /// @pre Finalize() was already called on `this` plant.
   /// @throws std::exception if called before Finalize().
   const systems::OutputPort<T>& get_state_output_port() const;
@@ -3664,17 +3664,15 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
         body_index_to_frame_id_.end();
   }
 
-  // Helper to retrieve a constant reference to the state vector from context.
-  const systems::BasicVector<T>& GetStateVector(
-      const systems::Context<T>& context) const;
-
-  // Calc method for the continuous state vector output port.
-  void CopyContinuousStateOut(
+  // Calc method for the multibody state vector output port. It only copies the
+  // multibody state [q, v], ignoring any miscellaneous state z if present.
+  void CopyMultibodyStateOut(
       const systems::Context<T>& context, systems::BasicVector<T>* state) const;
 
-  // Calc method for the per-model-instance continuous state vector output
-  // port.
-  void CopyContinuousStateOut(
+  // Calc method for the per-model-instance multibody state vector output port.
+  // It only copies the per-model-instance multibody state [q, v], ignoring any
+  // miscellaneous state z if present.
+  void CopyMultibodyStateOut(
       ModelInstanceIndex model_instance,
       const systems::Context<T>& context, systems::BasicVector<T>* state) const;