Skip to content

Commit

Permalink
MultibodyPlant hinders writing user non-finite values to context
Browse files Browse the repository at this point in the history 
MultibodyPlant::SetFoo(context, vector): when writing user-provided
configuration values (qs and vs) into the context, we throw if the input
has non-finite values.

Includes the following APIs:
  - SetPositions() - 3 overloads
  - SetPositionsAndVelocities() - 2 overloads
  - SetDefaultPositions() - 2 overloads
  - SetVelocities() - 3 overloads
  - SetVelocitiesInArray()
  • Loading branch information
@SeanCurtis-TRI
SeanCurtis-TRI committed Feb 7, 2025
1 parent 9f26738 commit 4b6b862
Show file tree
Hide file tree
Showing 3 changed files with 215 additions and 23 deletions.
12 changes: 12 additions & 0 deletions multibody/plant/multibody_plant.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1717,6 +1717,12 @@ void MultibodyPlant<T>::SetDefaultPositions(
const Eigen::Ref<const Eigen::VectorXd>& q) {
DRAKE_MBP_THROW_IF_NOT_FINALIZED();
DRAKE_THROW_UNLESS(q.size() == num_positions());
if constexpr (!std::is_same_v<T, symbolic::Expression>) {
using std::isfinite;
DRAKE_THROW_UNLESS(all_of(q, [](const T& t) {
return isfinite(t);
}));
}
for (JointIndex i : GetJointIndices()) {
Joint<T>& joint = get_mutable_joint(i);
joint.set_default_positions(
Expand All @@ -1730,6 +1736,12 @@ void MultibodyPlant<T>::SetDefaultPositions(
const Eigen::Ref<const Eigen::VectorXd>& q_instance) {
DRAKE_MBP_THROW_IF_NOT_FINALIZED();
DRAKE_THROW_UNLESS(q_instance.size() == num_positions(model_instance));
if constexpr (!std::is_same_v<T, symbolic::Expression>) {
using std::isfinite;
DRAKE_THROW_UNLESS(all_of(q_instance, [](const T& t) {
return isfinite(t);
}));
}
VectorX<T> q_T(num_positions());
internal_tree().SetPositionsInArray(model_instance, q_instance.cast<T>(),
&q_T);
Expand Down
107 changes: 85 additions & 22 deletions multibody/plant/multibody_plant.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2664,28 +2664,42 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// 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<T>* context,
const Eigen::Ref<const VectorX<T>>& q_v) const {
this->ValidateContext(context);
DRAKE_THROW_UNLESS(q_v.size() == (num_positions() + num_velocities()));
if constexpr (!std::is_same_v<T, symbolic::Expression>) {
using std::isfinite;
DRAKE_THROW_UNLESS(all_of(q_v, [](const T& t) {
return isfinite(t);
}));
}
internal_tree().GetMutablePositionsAndVelocities(context) = q_v;
}

/// Sets generalized positions q and generalized velocities v from a given
/// 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<T>* context, ModelInstanceIndex model_instance,
const Eigen::Ref<const VectorX<T>>& q_v) const {
this->ValidateContext(context);
DRAKE_THROW_UNLESS(q_v.size() == (num_positions(model_instance) +
num_velocities(model_instance)));
if constexpr (!std::is_same_v<T, symbolic::Expression>) {
using std::isfinite;
DRAKE_THROW_UNLESS(all_of(q_v, [](const T& t) {
return isfinite(t);
}));
}
internal_tree().SetPositionsAndVelocities(model_instance, q_v, context);
}

Expand Down Expand Up @@ -2733,26 +2747,39 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// 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<T>* context,
const Eigen::Ref<const VectorX<T>>& q) const {
this->ValidateContext(context);
DRAKE_THROW_UNLESS(q.size() == num_positions());
if constexpr (!std::is_same_v<T, symbolic::Expression>) {
using std::isfinite;
DRAKE_THROW_UNLESS(all_of(q, [](const T& t) {
return isfinite(t);
}));
}
internal_tree().GetMutablePositions(context) = q;
}

/// Sets the generalized positions q for a particular model instance in a
/// 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<T>* context,
ModelInstanceIndex model_instance,
const Eigen::Ref<const VectorX<T>>& q_instance) const {
this->ValidateContext(context);
DRAKE_THROW_UNLESS(q_instance.size() == num_positions(model_instance));
if constexpr (!std::is_same_v<T, symbolic::Expression>) {
using std::isfinite;
DRAKE_THROW_UNLESS(all_of(q_instance, [](const T& t) {
return isfinite(t);
}));
}
Eigen::VectorBlock<VectorX<T>> q =
internal_tree().GetMutablePositions(context);
internal_tree().SetPositionsInArray(model_instance, q_instance, &q);
Expand All @@ -2763,15 +2790,22 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// @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<T>& context,
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));
if constexpr (!std::is_same_v<T, symbolic::Expression>) {
using std::isfinite;
DRAKE_THROW_UNLESS(all_of(q_instance, [](const T& t) {
return isfinite(t);
}));
}
Eigen::VectorBlock<VectorX<T>> q =
internal_tree().get_mutable_positions(state);
internal_tree().SetPositionsInArray(model_instance, q_instance, &q);
Expand All @@ -2792,17 +2826,18 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// 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<const Eigen::VectorXd>& q);

/// Sets the default positions for the model instance. Calls to
/// CreateDefaultContext 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, 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<const Eigen::VectorXd>& q_instance);

Expand Down Expand Up @@ -2850,26 +2885,40 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// 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<T>* context,
const Eigen::Ref<const VectorX<T>>& v) const {
this->ValidateContext(context);
DRAKE_THROW_UNLESS(v.size() == num_velocities());
if constexpr (!std::is_same_v<T, symbolic::Expression>) {
using std::isfinite;
DRAKE_THROW_UNLESS(all_of(v, [](const T& t) {
return isfinite(t);
}));
}
internal_tree().GetMutableVelocities(context) = v;
}

/// Sets the generalized velocities v for a particular model instance in a
/// 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<T>* context,
ModelInstanceIndex model_instance,
const Eigen::Ref<const VectorX<T>>& v_instance) const {
this->ValidateContext(context);
DRAKE_THROW_UNLESS(v_instance.size() == num_velocities(model_instance));
if constexpr (!std::is_same_v<T, symbolic::Expression>) {
using std::isfinite;
DRAKE_THROW_UNLESS(all_of(v_instance, [](const T& t) {
return isfinite(t);
}));
}
Eigen::VectorBlock<VectorX<T>> v =
internal_tree().GetMutableVelocities(context);
internal_tree().SetVelocitiesInArray(model_instance, v_instance, &v);
Expand All @@ -2880,8 +2929,9 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// @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<T>& context,
systems::State<T>* state,
Expand All @@ -2890,6 +2940,12 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
this->ValidateContext(context);
this->ValidateCreatedForThisSystem(state);
DRAKE_THROW_UNLESS(v_instance.size() == num_velocities(model_instance));
if constexpr (!std::is_same_v<T, symbolic::Expression>) {
using std::isfinite;
DRAKE_THROW_UNLESS(all_of(v_instance, [](const T& t) {
return isfinite(t);
}));
}
Eigen::VectorBlock<VectorX<T>> v =
internal_tree().get_mutable_velocities(state);
internal_tree().SetVelocitiesInArray(model_instance, v_instance, &v);
Expand Down Expand Up @@ -3139,12 +3195,19 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// 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<const VectorX<T>>& v_instance,
EigenPtr<VectorX<T>> v) const {
DRAKE_DEMAND(v != nullptr);
if constexpr (!std::is_same_v<T, symbolic::Expression>) {
using std::isfinite;
DRAKE_THROW_UNLESS(all_of(v_instance, [](const T& t) {
return isfinite(t);
}));
}
internal_tree().SetVelocitiesInArray(model_instance, v_instance, v);
}
/// @} <!-- State accessors and mutators -->
Expand Down
119 changes: 118 additions & 1 deletion multibody/plant/test/multibody_plant_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<ModelInstanceIndex> 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);
Expand Down Expand Up @@ -971,6 +975,8 @@ class AcrobotPlantTests : public ::testing::Test {
RevoluteJoint<double>* 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<double> acrobot_benchmark_{Vector3d::UnitZ() /* Plane normal */,
Expand Down Expand Up @@ -1135,6 +1141,117 @@ TEST_F(AcrobotPlantTests, SetDefaultState) {
EXPECT_EQ(shoulder_->get_angle(*plant_context_), 4.2);
}

TEST_F(AcrobotPlantTests, SetPositionWithNonFinites) {
VectorX<double> p = VectorX<double>::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<double>::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<double>::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<double> p = VectorX<double>::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<double>::quiet_NaN();
EXPECT_THROW(plant_->SetDefaultPositions(p), std::exception);
EXPECT_THROW(plant_->SetDefaultPositions(model_instance_, p), std::exception);

p[0] = std::numeric_limits<double>::infinity();
EXPECT_THROW(plant_->SetDefaultPositions(p), std::exception);
EXPECT_THROW(plant_->SetDefaultPositions(model_instance_, p), std::exception);
}

TEST_F(AcrobotPlantTests, SetVelocitiesWithNonFinites) {
VectorX<double> v = VectorX<double>::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<double>::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<double>::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<double> v_all = VectorX<double>::Zero(plant_->num_velocities());
ASSERT_GT(v_all.rows(), 0);
VectorX<double> v_instance =
VectorX<double>::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<double>::quiet_NaN();
EXPECT_THROW(
plant_->SetVelocitiesInArray(model_instance_, v_instance, &v_all),
std::exception);

v_instance[0] = std::numeric_limits<double>::infinity();
EXPECT_THROW(
plant_->SetVelocitiesInArray(model_instance_, v_instance, &v_all),
std::exception);
}

TEST_F(AcrobotPlantTests, SetPositionAndVelocitiesWithNonFinites) {
VectorX<double> q = VectorX<double>::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<double>::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<double>::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.
Expand Down

0 comments on commit 4b6b862

Please sign in to comment.