Skip to content

Commit

Permalink
multibody: Properly validate State at public APIs (#15646)
Browse files Browse the repository at this point in the history
Closes: #11252
  • Loading branch information
rpoyner-tri committed Aug 24, 2021
1 parent 0eb9f7e commit 0317359
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 18 deletions.
7 changes: 0 additions & 7 deletions multibody/plant/multibody_plant.cc
Original file line number Diff line number Diff line change
Expand Up @@ -844,13 +844,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());
Expand Down
19 changes: 8 additions & 11 deletions multibody/plant/multibody_plant.h
Original file line number Diff line number Diff line change
Expand Up @@ -1906,7 +1906,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);
}

Expand Down Expand Up @@ -1949,8 +1949,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
systems::State<T>* state, ModelInstanceIndex model_instance,
const Eigen::Ref<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);
}
Expand Down Expand Up @@ -2007,7 +2007,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);
}

Expand Down Expand Up @@ -2051,8 +2051,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
ModelInstanceIndex model_instance,
const Eigen::Ref<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);
}
Expand All @@ -2066,7 +2066,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(
Expand All @@ -2086,7 +2086,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);
}

Expand Down Expand Up @@ -2228,7 +2228,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);
}

Expand Down Expand Up @@ -2278,7 +2278,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);
}
Expand Down Expand Up @@ -4117,9 +4117,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.
Expand Down

0 comments on commit 0317359

Please sign in to comment.