diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index bff7519006e6..9bb82aeed2a1 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -1717,6 +1717,7 @@ void MultibodyPlant::SetDefaultPositions( const Eigen::Ref& q) { DRAKE_MBP_THROW_IF_NOT_FINALIZED(); DRAKE_THROW_UNLESS(q.size() == num_positions()); + DRAKE_THROW_UNLESS(q.allFinite()); for (JointIndex i : GetJointIndices()) { Joint& joint = get_mutable_joint(i); joint.set_default_positions( @@ -1730,6 +1731,7 @@ void MultibodyPlant::SetDefaultPositions( const Eigen::Ref& q_instance) { DRAKE_MBP_THROW_IF_NOT_FINALIZED(); DRAKE_THROW_UNLESS(q_instance.size() == num_positions(model_instance)); + DRAKE_THROW_UNLESS(q_instance.allFinite()); VectorX q_T(num_positions()); internal_tree().SetPositionsInArray(model_instance, q_instance.cast(), &q_T); diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h index a1b2ceaffc5e..b08f649599a9 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -2664,13 +2664,18 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// Context from a given vector [q; v]. Prefer this method over /// GetMutablePositionsAndVelocities(). /// @throws std::exception if `context` is nullptr, if `context` does - /// not correspond to the context for a multibody model, or if the length of - /// `q_v` is not equal to `num_positions() + num_velocities()`. + /// not correspond to the context for a multibody model, if the length of + /// `q_v` is not equal to `num_positions() + num_velocities()`, or if `q_v` + /// contains non-finite values. void SetPositionsAndVelocities( systems::Context* context, const Eigen::Ref>& q_v) const { this->ValidateContext(context); DRAKE_THROW_UNLESS(q_v.size() == (num_positions() + num_velocities())); + DRAKE_THROW_UNLESS(all_of(q_v, [](const T& t) { + using std::isfinite; + return isfinite(t); + })); internal_tree().GetMutablePositionsAndVelocities(context) = q_v; } @@ -2678,14 +2683,19 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// vector [q; v] for a specified model instance in a given Context. /// @throws std::exception if `context` is nullptr, if `context` does /// not correspond to the Context for a multibody model, if the model instance - /// index is invalid, or if the length of `q_v` is not equal to - /// `num_positions(model_instance) + num_velocities(model_instance)`. + /// index is invalid, if the length of `q_v` is not equal to + /// `num_positions(model_instance) + num_velocities(model_instance)`, or if + /// `q_v` contains non-finite values. void SetPositionsAndVelocities( systems::Context* context, ModelInstanceIndex model_instance, const Eigen::Ref>& q_v) const { this->ValidateContext(context); DRAKE_THROW_UNLESS(q_v.size() == (num_positions(model_instance) + num_velocities(model_instance))); + DRAKE_THROW_UNLESS(all_of(q_v, [](const T& t) { + using std::isfinite; + return isfinite(t); + })); internal_tree().SetPositionsAndVelocities(model_instance, q_v, context); } @@ -2733,12 +2743,16 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// Sets the generalized positions q in a given Context from a given vector. /// Prefer this method over GetMutablePositions(). /// @throws std::exception if `context` is nullptr, if `context` does not - /// correspond to the Context for a multibody model, or if the length of `q` - /// is not equal to `num_positions()`. + /// correspond to the Context for a multibody model, if the length of `q` + /// is not equal to `num_positions()`, or if `q` contains non-finite values. void SetPositions(systems::Context* context, const Eigen::Ref>& q) const { this->ValidateContext(context); DRAKE_THROW_UNLESS(q.size() == num_positions()); + DRAKE_THROW_UNLESS(all_of(q, [](const T& t) { + using std::isfinite; + return isfinite(t); + })); internal_tree().GetMutablePositions(context) = q; } @@ -2746,13 +2760,18 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// given Context from a given vector. /// @throws std::exception if the `context` is nullptr, if `context` does /// not correspond to the Context for a multibody model, if the model instance - /// index is invalid, or if the length of `q_instance` is not equal to - /// `num_positions(model_instance)`. + /// index is invalid, if the length of `q_instance` is not equal to + /// `num_positions(model_instance)`, or if `q_instance` contains non-finite + /// values. void SetPositions(systems::Context* context, ModelInstanceIndex model_instance, const Eigen::Ref>& q_instance) const { this->ValidateContext(context); DRAKE_THROW_UNLESS(q_instance.size() == num_positions(model_instance)); + DRAKE_THROW_UNLESS(all_of(q_instance, [](const T& t) { + using std::isfinite; + return isfinite(t); + })); Eigen::VectorBlock> q = internal_tree().GetMutablePositions(context); internal_tree().SetPositionsInArray(model_instance, q_instance, &q); @@ -2763,8 +2782,9 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// @note No cache invalidation occurs. /// @throws std::exception if the `context` is nullptr, if `context` does /// not correspond to the Context for a multibody model, if the model instance - /// index is invalid, or if the length of `q_instance` is not equal to - /// `num_positions(model_instance)`. + /// index is invalid, if the length of `q_instance` is not equal to + /// `num_positions(model_instance)`, or if `q_instance` contains non-finite + /// values. /// @pre `state` comes from this MultibodyPlant. void SetPositions(const systems::Context& context, systems::State* state, ModelInstanceIndex model_instance, @@ -2772,6 +2792,10 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { this->ValidateContext(context); this->ValidateCreatedForThisSystem(state); DRAKE_THROW_UNLESS(q_instance.size() == num_positions(model_instance)); + DRAKE_THROW_UNLESS(all_of(q_instance, [](const T& t) { + using std::isfinite; + return isfinite(t); + })); Eigen::VectorBlock> q = internal_tree().get_mutable_positions(state); internal_tree().SetPositionsInArray(model_instance, q_instance, &q); @@ -2792,8 +2816,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// or SetDefaultContext/SetDefaultState will return a Context populated with /// these position values. They have no other effects on the dynamics of the /// system. - /// @throws std::exception if the plant is not finalized or if q is - /// not of size num_positions(). + /// @throws std::exception if the plant is not finalized, if q is not of size + /// num_positions(), or `q` contains non-finite values. void SetDefaultPositions(const Eigen::Ref& q); /// Sets the default positions for the model instance. Calls to @@ -2801,8 +2825,9 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// Context populated with these position values. They have no other effects /// on the dynamics of the system. /// @throws std::exception if the plant is not - /// finalized, if the model_instance is invalid, or if the length of - /// `q_instance` is not equal to `num_positions(model_instance)`. + /// finalized, if the model_instance is invalid, if the length of `q_instance` + /// is not equal to `num_positions(model_instance)`, or if `q_instance` + /// contains non-finite values. void SetDefaultPositions(ModelInstanceIndex model_instance, const Eigen::Ref& q_instance); @@ -2850,12 +2875,17 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// Sets the generalized velocities v in a given Context from a given /// vector. Prefer this method over GetMutableVelocities(). /// @throws std::exception if the `context` is nullptr, if the context does - /// not correspond to the context for a multibody model, or if the length of - /// `v` is not equal to `num_velocities()`. + /// not correspond to the context for a multibody model, if the length of + /// `v` is not equal to `num_velocities()`, or if `v` contains non-finite + /// values. void SetVelocities(systems::Context* context, const Eigen::Ref>& v) const { this->ValidateContext(context); DRAKE_THROW_UNLESS(v.size() == num_velocities()); + DRAKE_THROW_UNLESS(all_of(v, [](const T& t) { + using std::isfinite; + return isfinite(t); + })); internal_tree().GetMutableVelocities(context) = v; } @@ -2863,13 +2893,18 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// given Context from a given vector. /// @throws std::exception if the `context` is nullptr, if `context` does /// not correspond to the Context for a multibody model, if the model instance - /// index is invalid, or if the length of `v_instance` is not equal to - /// `num_velocities(model_instance)`. + /// index is invalid, if the length of `v_instance` is not equal to + /// `num_velocities(model_instance)`, or if `v_instance` contains non-finite + /// values. void SetVelocities(systems::Context* context, ModelInstanceIndex model_instance, const Eigen::Ref>& v_instance) const { this->ValidateContext(context); DRAKE_THROW_UNLESS(v_instance.size() == num_velocities(model_instance)); + DRAKE_THROW_UNLESS(all_of(v_instance, [](const T& t) { + using std::isfinite; + return isfinite(t); + })); Eigen::VectorBlock> v = internal_tree().GetMutableVelocities(context); internal_tree().SetVelocitiesInArray(model_instance, v_instance, &v); @@ -2880,8 +2915,9 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// @note No cache invalidation occurs. /// @throws std::exception if the `context` is nullptr, if `context` does /// not correspond to the Context for a multibody model, if the model instance - /// index is invalid, or if the length of `v_instance` is not equal to - /// `num_velocities(model_instance)`. + /// index is invalid, if the length of `v_instance` is not equal to + /// `num_velocities(model_instance)`, or if `v_instance` contains non-finite + /// values. /// @pre `state` comes from this MultibodyPlant. void SetVelocities(const systems::Context& context, systems::State* state, @@ -2890,6 +2926,10 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { this->ValidateContext(context); this->ValidateCreatedForThisSystem(state); DRAKE_THROW_UNLESS(v_instance.size() == num_velocities(model_instance)); + DRAKE_THROW_UNLESS(all_of(v_instance, [](const T& t) { + using std::isfinite; + return isfinite(t); + })); Eigen::VectorBlock> v = internal_tree().get_mutable_velocities(state); internal_tree().SetVelocitiesInArray(model_instance, v_instance, &v); @@ -3139,12 +3179,17 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// Sets the vector of generalized velocities for `model_instance` in /// `v` using `v_instance`, leaving all other elements in the array /// untouched. This method throws an exception if `v` is not of size - /// MultibodyPlant::num_velocities() or `v_instance` is not of size - /// `MultibodyPlant::num_positions(model_instance)`. + /// MultibodyPlant::num_velocities(), `v_instance` is not of size + /// `MultibodyPlant::num_positions(model_instance)`, or `v_instance` contains + /// non-finite values. void SetVelocitiesInArray(ModelInstanceIndex model_instance, const Eigen::Ref>& v_instance, EigenPtr> v) const { DRAKE_DEMAND(v != nullptr); + DRAKE_THROW_UNLESS(all_of(v_instance, [](const T& t) { + using std::isfinite; + return isfinite(t); + })); internal_tree().SetVelocitiesInArray(model_instance, v_instance, v); } /// @} diff --git a/multibody/plant/test/multibody_plant_test.cc b/multibody/plant/test/multibody_plant_test.cc index cb31a183b07f..3cf7aa16c8f7 100644 --- a/multibody/plant/test/multibody_plant_test.cc +++ b/multibody/plant/test/multibody_plant_test.cc @@ -671,7 +671,11 @@ class AcrobotPlantTests : public ::testing::Test { const std::string url = "package://drake/multibody/benchmarks/acrobot/acrobot.sdf"; std::tie(plant_, scene_graph_) = AddMultibodyPlantSceneGraph(&builder, 0.0); - Parser(plant_).AddModelsFromUrl(url); + const std::vector instances = + Parser(plant_).AddModelsFromUrl(url); + DRAKE_DEMAND(instances.size() == 1); + model_instance_ = instances[0]; + // Sanity check on the availability of the optional source id before using // it. DRAKE_DEMAND(plant_->get_source_id() != std::nullopt); @@ -971,6 +975,8 @@ class AcrobotPlantTests : public ::testing::Test { RevoluteJoint* elbow_{nullptr}; // Input port for the actuation: systems::FixedInputPortValue* input_port_{nullptr}; + // The model instance of the acrobot. + ModelInstanceIndex model_instance_{}; // Reference benchmark for verification. Acrobot acrobot_benchmark_{Vector3d::UnitZ() /* Plane normal */, @@ -1135,6 +1141,117 @@ TEST_F(AcrobotPlantTests, SetDefaultState) { EXPECT_EQ(shoulder_->get_angle(*plant_context_), 4.2); } +TEST_F(AcrobotPlantTests, SetPositionWithNonFinites) { + VectorX p = VectorX::Zero(plant_->num_positions()); + ASSERT_GT(p.rows(), 0); + + // First confirm we've got the right context and right size of things. + EXPECT_NO_THROW(plant_->SetPositions(plant_context_, p)); + + p[0] = std::numeric_limits::quiet_NaN(); + EXPECT_THROW(plant_->SetPositions(plant_context_, p), std::exception); + EXPECT_THROW(plant_->SetPositions(plant_context_, model_instance_, p), + std::exception); + EXPECT_THROW(plant_->SetPositions(*plant_context_, + &plant_context_->get_mutable_state(), + model_instance_, p), + std::exception); + + p[0] = std::numeric_limits::infinity(); + EXPECT_THROW(plant_->SetPositions(plant_context_, p), std::exception); + EXPECT_THROW(plant_->SetPositions(plant_context_, model_instance_, p), + std::exception); + EXPECT_THROW(plant_->SetPositions(*plant_context_, + &plant_context_->get_mutable_state(), + model_instance_, p), + std::exception); +} + +TEST_F(AcrobotPlantTests, SetDefaultPositionWithNonFinites) { + VectorX p = VectorX::Zero(plant_->num_positions()); + ASSERT_GT(p.rows(), 0); + + // First confirm we've got the right size of things. + EXPECT_NO_THROW(plant_->SetDefaultPositions(p)); + + p[0] = std::numeric_limits::quiet_NaN(); + EXPECT_THROW(plant_->SetDefaultPositions(p), std::exception); + EXPECT_THROW(plant_->SetDefaultPositions(model_instance_, p), std::exception); + + p[0] = std::numeric_limits::infinity(); + EXPECT_THROW(plant_->SetDefaultPositions(p), std::exception); + EXPECT_THROW(plant_->SetDefaultPositions(model_instance_, p), std::exception); +} + +TEST_F(AcrobotPlantTests, SetVelocitiesWithNonFinites) { + VectorX v = VectorX::Zero(plant_->num_velocities()); + ASSERT_GT(v.rows(), 0); + + // First confirm we've got the right context and right size of things. + EXPECT_NO_THROW(plant_->SetVelocities(plant_context_, v)); + + v[0] = std::numeric_limits::quiet_NaN(); + EXPECT_THROW(plant_->SetVelocities(plant_context_, v), std::exception); + EXPECT_THROW(plant_->SetVelocities(plant_context_, model_instance_, v), + std::exception); + EXPECT_THROW(plant_->SetVelocities(*plant_context_, + &plant_context_->get_mutable_state(), + model_instance_, v), + std::exception); + + v[0] = std::numeric_limits::infinity(); + EXPECT_THROW(plant_->SetVelocities(plant_context_, v), std::exception); + EXPECT_THROW(plant_->SetVelocities(plant_context_, model_instance_, v), + std::exception); + EXPECT_THROW(plant_->SetVelocities(*plant_context_, + &plant_context_->get_mutable_state(), + model_instance_, v), + std::exception); +} + +TEST_F(AcrobotPlantTests, SetVelocitiesInArrayWithNonFinites) { + VectorX v_all = VectorX::Zero(plant_->num_velocities()); + ASSERT_GT(v_all.rows(), 0); + VectorX v_instance = + VectorX::Zero(plant_->num_velocities(model_instance_)); + + // First confirm we've got the right context and right size of things. + EXPECT_NO_THROW( + plant_->SetVelocitiesInArray(model_instance_, v_instance, &v_all)); + + v_instance[0] = std::numeric_limits::quiet_NaN(); + EXPECT_THROW( + plant_->SetVelocitiesInArray(model_instance_, v_instance, &v_all), + std::exception); + + v_instance[0] = std::numeric_limits::infinity(); + EXPECT_THROW( + plant_->SetVelocitiesInArray(model_instance_, v_instance, &v_all), + std::exception); +} + +TEST_F(AcrobotPlantTests, SetPositionAndVelocitiesWithNonFinites) { + VectorX q = VectorX::Zero(plant_->num_multibody_states()); + ASSERT_GT(q.rows(), 0); + + // First confirm we've got the right context and right size of things. + EXPECT_NO_THROW(plant_->SetPositionsAndVelocities(plant_context_, q)); + + q[0] = std::numeric_limits::quiet_NaN(); + EXPECT_THROW(plant_->SetPositionsAndVelocities(plant_context_, q), + std::exception); + EXPECT_THROW( + plant_->SetPositionsAndVelocities(plant_context_, model_instance_, q), + std::exception); + + q[0] = std::numeric_limits::infinity(); + EXPECT_THROW(plant_->SetPositionsAndVelocities(plant_context_, q), + std::exception); + EXPECT_THROW( + plant_->SetPositionsAndVelocities(plant_context_, model_instance_, q), + std::exception); +} + GTEST_TEST(MultibodyPlantTest, SetDefaultFreeBodyPose) { // We cannot use Acrobot for testing `SetDefaultFreeBodyPose` since it has no // free bodies.