MultibodyPlant hinders writing user non-finite values to context

[SeanCurtis-TRI]

MultibodyPlant::SetFoo(context, vector): when writing user-provided configuration values (qs and vs) into the context, we throw if the input has NaNs.

Includes the following APIs:

• SetPositions() - 3 overloads
• SetPositionsAndVelocities() - 2 overloads
• SetDefaultPositions() - 2 overloads
• SetVelocities() - 3 overloads
• SetVelocitiesInArray()

---

This change is 

[SeanCurtis-TRI]

+(release notes: fix)
+@amcastro-tri for feature review, please.

Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

[SeanCurtis-TRI]

@amcastro-tri just wanted to ping you on this.

Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

[jwnimmer-tri]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

---

multibody/plant/multibody_plant.h line 2675 at r1 (raw file):

    this->ValidateContext(context);
    DRAKE_THROW_UNLESS(q_v.size() == (num_positions() + num_velocities()));
    if constexpr (!std::is_same_v<T, symbolic::Expression>) {

There is no reason to if-constexpr here. The DRAKE_THROW_UNLESS already turns itself into a no-op when asked to check a symbolic assertion.

Ditto throughout.

--- a/multibody/plant/multibody_plant.h
+++ b/multibody/plant/multibody_plant.h
@@ -2672,12 +2672,10 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
       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>) {
+    DRAKE_THROW_UNLESS(all_of(q_v, [](const T& t) {
       using std::isfinite;
-      DRAKE_THROW_UNLESS(all_of(q_v, [](const T& t) {
-        return isfinite(t);
-      }));
-    }
+      return isfinite(t);
+    }));
     internal_tree().GetMutablePositionsAndVelocities(context) = q_v;
   }
 

---

multibody/plant/multibody_plant.cc line 1720 at r1 (raw file):

  DRAKE_MBP_THROW_IF_NOT_FINALIZED();
  DRAKE_THROW_UNLESS(q.size() == num_positions());
  if constexpr (!std::is_same_v<T, symbolic::Expression>) {

The q here is already double-valued. There is no reason to opt-out for Expression.

Ditto for elsewhere when the q is not T-typed.