multibody: Properly validate State at public APIs

Closes: #11252
RobotLocomotion · Aug 20, 2021 · 7fc37d7 · 7fc37d7
1 parent febbb91
commit 7fc37d7
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 18 deletions.
diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc
@@ -845,13 +845,6 @@ MultibodyPlant<symbolic::Expression>::member_scene_graph() {
   return stub_.access();
 }
 
-template <typename T>
-void MultibodyPlant<T>::CheckValidState(const systems::State<T>* state) const {
-  DRAKE_THROW_UNLESS(state != nullptr);
-  DRAKE_THROW_UNLESS(
-      is_discrete() == (state->get_discrete_state().num_groups() > 0));
-}
-
 template <typename T>
 void MultibodyPlant<T>::FilterAdjacentBodies() {
   DRAKE_DEMAND(geometry_source_is_registered());

diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h
@@ -1905,7 +1905,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   Eigen::VectorBlock<VectorX<T>> GetMutablePositions(
       const systems::Context<T>& context, systems::State<T>* state) const {
     this->ValidateContext(context);
-    DRAKE_ASSERT_VOID(CheckValidState(state));
+    this->ValidateCreatedForThisSystem(state);
     return internal_tree().get_mutable_positions(state);
   }
 
@@ -1947,8 +1947,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
                     systems::State<T>* state, ModelInstanceIndex model_instance,
                     const VectorX<T>& q_instance) const {
     this->ValidateContext(context);
+    this->ValidateCreatedForThisSystem(state);
     DRAKE_THROW_UNLESS(q_instance.size() == num_positions(model_instance));
-    CheckValidState(state);
     Eigen::VectorBlock<VectorX<T>> q = GetMutablePositions(context, state);
     internal_tree().SetPositionsInArray(model_instance, q_instance, &q);
   }
@@ -2005,7 +2005,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   Eigen::VectorBlock<VectorX<T>> GetMutableVelocities(
       const systems::Context<T>& context, systems::State<T>* state) const {
     this->ValidateContext(context);
-    DRAKE_ASSERT_VOID(CheckValidState(state));
+    this->ValidateCreatedForThisSystem(state);
     return internal_tree().get_mutable_velocities(state);
   }
 
@@ -2047,8 +2047,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
       const systems::Context<T>& context, systems::State<T>* state,
       ModelInstanceIndex model_instance, const VectorX<T>& v_instance) const {
     this->ValidateContext(context);
+    this->ValidateCreatedForThisSystem(state);
     DRAKE_THROW_UNLESS(v_instance.size() == num_velocities(model_instance));
-    CheckValidState(state);
     Eigen::VectorBlock<VectorX<T>> v = GetMutableVelocities(context, state);
     internal_tree().SetVelocitiesInArray(model_instance, v_instance, &v);
   }
@@ -2062,7 +2062,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
                        systems::State<T>* state) const override {
     DRAKE_MBP_THROW_IF_NOT_FINALIZED();
     this->ValidateContext(context);
-    CheckValidState(state);
+    this->ValidateCreatedForThisSystem(state);
     internal_tree().SetDefaultState(context, state);
     for (const BodyIndex& index : GetFloatingBaseBodies()) {
       SetFreeBodyPose(
@@ -2082,7 +2082,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
                       RandomGenerator* generator) const override {
     DRAKE_MBP_THROW_IF_NOT_FINALIZED();
     this->ValidateContext(context);
-    CheckValidState(state);
+    this->ValidateCreatedForThisSystem(state);
     internal_tree().SetRandomState(context, state, generator);
   }
 
@@ -2224,7 +2224,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
       const systems::Context<T>& context, systems::State<T>* state,
       const Body<T>& body, const math::RigidTransform<T>& X_WB) const {
     this->ValidateContext(context);
-    CheckValidState(state);
+    this->ValidateCreatedForThisSystem(state);
     internal_tree().SetFreeBodyPoseOrThrow(body, X_WB, context, state);
   }
 
@@ -2274,7 +2274,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
       const systems::Context<T>& context, systems::State<T>* state,
       const Body<T>& body, const SpatialVelocity<T>& V_WB) const {
     this->ValidateContext(context);
-    CheckValidState(state);
+    this->ValidateCreatedForThisSystem(state);
     internal_tree().SetFreeBodySpatialVelocityOrThrow(
         body, V_WB, context, state);
   }
@@ -4114,9 +4114,6 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
         geometry::internal::kMaterialGroup, geometry::internal::kFriction);
   }
 
-  // Checks that the provided State is consistent with this plant.
-  void CheckValidState(const systems::State<T>*) const;
-
   // Helper method to apply collision filters based on body-adjacency. By
   // default, we don't consider collisions between geometries affixed to
   // bodies connected by a joint.