Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

MultibodyPlant hinders writing user non-finite values to context #22594

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

MultibodyPlant hinders writing user non-finite values to context #22594

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 12 additions & 0 deletions multibody/plant/multibody_plant.cc
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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