Unit tests support matrix for discrete MultibodyPlant models

[amcastro-tri]

Updates the multibody plant scalar conversion tests to reflect the current state of the support matrix for discrete models.

An important conclusion is that for TAMSI (and SAP), we do not support discrete updates when T = Symbolic at all. We (meaning I really) were expecting this to be supported when num_contacts=0, but it turns out that the proximity engine does not even allow queres on symbolic, regardelss the state of the system.

cc'ing @jwnimmer-tri. I know we talked about a Python script instead, but I already had this and it's not an extra large of lines (~150) to put together in C++ if I reuse the code from #21431

---

This change is 

[amcastro-tri]

+@SeanCurtis-TRI for feature review please

Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers

---

multibody/plant/test/iiwa7_model.h line 9 at r1 (raw file):

#include <gtest/gtest.h>

@SeanCurtis-TRI, this was entirely moved from sap_solver_autodiff_test.cc, with updates to support ToScalarType<U>(), and to provide contact_model at construction.

[amcastro-tri]

fyi, a seocond commit updates RobotModel so that we can make a model with no proximity properties.
This allows to test cases in no proximity in them.
Interestingly, this reveals that discrete updates on symbolic::Expression always throw. From different places, but they always trhow.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri)

[jwnimmer-tri]

I know we talked about a Python script instead, but I already had this and it's not an extra large of lines ...

Like I said, unit tests to lock in the features are good and this kind of thing makes sense.

My imagined Python tool to create a summary needs a lot more input permutations than anything here so far.

[SeanCurtis-TRI]

Pass done with a host of nitpicks.

Reviewed 1 of 7 files at r1, 6 of 6 files at r2, all commit messages.
Reviewable status: 23 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri)

---

multibody/plant/BUILD.bazel line 102 at r2 (raw file):

    deps = [
        ":multibody_plant_core",
        "//common:find_resource",

nit: unused

---

multibody/plant/BUILD.bazel line 103 at r2 (raw file):

        ":multibody_plant_core",
        "//common:find_resource",
        "//common/test_utilities:eigen_matrix_compare",

nit: unused

---

multibody/plant/BUILD.bazel line 108 at r2 (raw file):

        "//multibody/plant:compliant_contact_manager_tester",
        "//visualization:visualization_config_functions",
        "@gtest//:without_main",

nit: unused

---

multibody/plant/test/iiwa7_model.h line 9 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

@SeanCurtis-TRI, this was entirely moved from sap_solver_autodiff_test.cc, with updates to support ToScalarType<U>(), and to provide contact_model at construction.

Almost. I think this file evolved more than you realized it did after moving the contents. :)

---

multibody/plant/test/iiwa7_model.h line 3 at r2 (raw file):

#pragma once

#include <chrono>

nit: unused

---

multibody/plant/test/iiwa7_model.h line 8 at r2 (raw file):

#include <utility>

#include <gtest/gtest.h>

nit: unused

---

multibody/plant/test/iiwa7_model.h line 10 at r2 (raw file):

#include <gtest/gtest.h>

#include "drake/common/find_resource.h"

nit: unused

---

multibody/plant/test/iiwa7_model.h line 11 at r2 (raw file):

#include "drake/common/find_resource.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"

nit: unused

---

multibody/plant/test/iiwa7_model.h line 12 at r2 (raw file):

#include "drake/common/find_resource.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/common/test_utilities/maybe_pause_for_user.h"

nit: This include should go in the .cc file.

---

multibody/plant/test/iiwa7_model.h line 14 at r2 (raw file):

#include "drake/common/test_utilities/maybe_pause_for_user.h"
#include "drake/geometry/proximity_properties.h"
#include "drake/math/compute_numerical_gradient.h"

nit: unused

---

multibody/plant/test/iiwa7_model.h line 16 at r2 (raw file):

#include "drake/math/compute_numerical_gradient.h"
#include "drake/multibody/contact_solvers/sap/sap_contact_problem.h"
#include "drake/multibody/contact_solvers/sap/sap_solver.h"

nit: unused

---

multibody/plant/test/iiwa7_model.h line 17 at r2 (raw file):

#include "drake/multibody/contact_solvers/sap/sap_contact_problem.h"
#include "drake/multibody/contact_solvers/sap/sap_solver.h"
#include "drake/multibody/contact_solvers/sap/sap_solver_results.h"

nit: unused

---

multibody/plant/test/iiwa7_model.h line 25 at r2 (raw file):

#include "drake/visualization/visualization_config_functions.h"

using drake::geometry::ProximityProperties;

nit: Don't put any of these aliases in a header file.

---

multibody/plant/test/iiwa7_model.h line 62 at r2 (raw file):

struct RobotModelConfig {
  // If true, loads a model with collision geometry. Otherwise all proximity
  // gometry is removed.

nit: typo

Suggestion:

geometry

---

multibody/plant/test/iiwa7_model.h line 74 at r2 (raw file):

  bool state_in_contact{true};

  // Specifies the geomteric modeling of contact.

nit: typo

Suggestion:

geometric

---

multibody/plant/test/iiwa7_model.h line 74 at r2 (raw file):

  bool state_in_contact{true};

  // Specifies the geomteric modeling of contact.

BTW "...of contact if with_contact_geometry = true". Given that dependency, it might be nice to cluster the two related parameters together.

Alternatively, you could make the ContactModel optional and then encode both parameters with a single parameter. If nullopt, there is no contact geometry and the contact model is irrelevant. Otherwise, a valid ContactModel value then requires proximity geometry?

---

multibody/plant/test/iiwa7_model.h line 88 at r2 (raw file):

// treatment within the solver.
template <typename T>
class RobotModel {

BTW Why the disagreement between file name and class name? RobotModel vs iiwa7_model? As I look at the usage, the fact that it uses an iiwa arm seems largely irrelevant.

---

multibody/plant/test/iiwa7_model.h line 95 at r2 (raw file):

  RobotModel() = default;

  // This constructor is provided to make a RoboModel<double>. Use the scalar

nit: typo

Suggestion:

RobotModel<double>

---

multibody/plant/test/iiwa7_model.h line 284 at r2 (raw file):

  // Returns the robot's "zero state".
  static VectorXd RobotStateIsZero() { return VectorX<double>::Zero(14); }

nit: This type of name implies a bool return value. You might choose a name like RobotZeroState().

Code quote:

RobotStateIsZero()

---

multibody/plant/test/iiwa7_model.h line 287 at r2 (raw file):

 private:
  // Friendship to give ToAutoDiffXd() access to private members.

nit: The reference to ToAutoDiffXd() is now stale, it should be ToScalarType().

Code quote:

ToAutoDiffXd()

---

multibody/plant/test/iiwa7_model.h line 305 at r2 (raw file):

  Context<T>* plant_context_{nullptr};
  // N.B. manager_ and driver_ will be available for testing only when using the
  // SAP solver for T != symblic::Expression.

nit: typo

Suggestion:

symbolic

---

multibody/plant/test/iiwa7_model.cc line 9 at r2 (raw file):

void VisualizeRobotModel() {
  // Visualize the contact configuration.
  RobotModelConfig config{

This function gets called in the double-only constructor. What we construct may have with_contact_geometry = false or state_in_contact = false. But they what we visualize is hard-coded this way. Does it make sense that what we visualize is different from what we've configured?

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc line 295 at r2 (raw file):

// description.
std::ostream& operator<<(std::ostream& out, const IiwaRobotTestConfig& c) {
  out << c.description;

nit; When using your struct description as part of the parameterized test name, the string should not contain underscores.

(This specifically applies to the names like Sap_Point_NoGeometry etc down below in MakeSupportMatrixTestCases.)

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc line 300 at r2 (raw file):

template <typename T>
class IiwaRobotTest : public ::testing::TestWithParam<IiwaRobotTestConfig> {

btw: Emphasizing IiwaRobot makes it sound like that's important to this test fixture. In reality, the fact that the robot model has an iiwa arm in it is immaterial. The fact that the plant is configured to use a discrete solver is of paramount importance. Perhaps a name that better represents the significant property would help readers in the future.

[amcastro-tri]

Thanks!

Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri)

---

multibody/plant/test/iiwa7_model.h line 9 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Almost. I think this file evolved more than you realized it did after moving the contents. :)

yes, indeed you are right. Thanks for reviewing and sorry for the trouble.

---

multibody/plant/test/iiwa7_model.h line 74 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW "...of contact if with_contact_geometry = true". Given that dependency, it might be nice to cluster the two related parameters together.

Alternatively, you could make the ContactModel optional and then encode both parameters with a single parameter. If nullopt, there is no contact geometry and the contact model is irrelevant. Otherwise, a valid ContactModel value then requires proximity geometry?

It turns out that even if we do not have geometry, the current code sometimes does call the SG corresponding query anyway. I wanted to trigger exceptions coming from those queries. I added a comment.

---

multibody/plant/test/iiwa7_model.cc line 9 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

This function gets called in the double-only constructor. What we construct may have with_contact_geometry = false or state_in_contact = false. But they what we visualize is hard-coded this way. Does it make sense that what we visualize is different from what we've configured?

sorry, I don't understand your comment.

[SeanCurtis-TRI]

+@jwnimmer-tri for platform review (if there's still time left in the day).

Reviewed 4 of 4 files at r3, all commit messages.
Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri)

---

multibody/plant/test/iiwa7_model.h line 74 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

It turns out that even if we do not have geometry, the current code sometimes does call the SG corresponding query anyway. I wanted to trigger exceptions coming from those queries. I added a comment.

Ah....I see. It's the distinction between knowing you're going to call certain APIs and knowing those APIs will always come back empty.

---

multibody/plant/test/iiwa7_model.cc line 9 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

sorry, I don't understand your comment.

Oops. That's me being sloppy in my review.

VisualizeRobotModel() is not a member of RobotModel(). And it isn't invoked in the constructor; visualization::AddDefaultVisualization() is invoked.

You can't make sense of a comment that is otherwise nonsense. :) No problems here.

[jwnimmer-tri]

Sure.

I've posted a first checkpoint for now. Since some of the discussions will require shuffling files around, I'm going to pause my review into the code moves into the files we aim to use.

Reviewed 1 of 7 files at r1, 1 of 6 files at r2, all commit messages.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri)

---

multibody/plant/BUILD.bazel line 99 at r3 (raw file):

        "@drake_models//:iiwa_description",
    ],
    visibility = ["//visibility:public"],

When a cc_library is testonly, in Drake allow only two choices:

(1) Put it in a subdir named test and mark it as //visibility:private.

(2) Put it in a subdir named test_utilities and have it be //visibility:public (usually via the default_visibility atop the test_utilities/BUILD.bazel file).

In other words: for clarity we do not allow code in a test folder to be used beyond its immediate siblings. Allowing test folders to depend on each other is too difficult (overly-coupled) to maintain over the longer term.

---

Notably the compliant_contact_manager_tester already violates this rule, so should also be cleaned up, though doing that in a separate PR is OK by me.

---

multibody/plant/test/robot_model.h line 80 at r3 (raw file):

    requires std::is_same_v<T, double>
  {  // NOLINT(whitespace/braces)
    systems::DiagramBuilder<double> builder;

Many of the functions in the h file do not meet the requirements of https://drake.mit.edu/styleguide/cppguide.html#Inline_Functions, so must be moved to the cc file.

[amcastro-tri]

Okay, I believe I performed the requested shuffling of flies.
PTAL @jwnimmer-tri

Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri)

---

multibody/plant/BUILD.bazel line 99 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

When a cc_library is testonly, in Drake allow only two choices:

(1) Put it in a subdir named test and mark it as //visibility:private.

(2) Put it in a subdir named test_utilities and have it be //visibility:public (usually via the default_visibility atop the test_utilities/BUILD.bazel file).

In other words: for clarity we do not allow code in a test folder to be used beyond its immediate siblings. Allowing test folders to depend on each other is too difficult (overly-coupled) to maintain over the longer term.

---

Notably the compliant_contact_manager_tester already violates this rule, so should also be cleaned up, though doing that in a separate PR is OK by me.

I see, thanks.
I moved robot model into multibody/test_utilities.

---

multibody/plant/test/robot_model.h line 80 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Many of the functions in the h file do not meet the requirements of https://drake.mit.edu/styleguide/cppguide.html#Inline_Functions, so must be moved to the cc file.

Done.

[jwnimmer-tri]

Yes, shuffling is good now, thanks. I'll resume the review soon.

Reviewed 1 of 6 files at r2, 6 of 9 files at r4, all commit messages.
Reviewable status: LGTM missing from assignee jwnimmer-tri(platform)

[SeanCurtis-TRI]

Reviewed 9 of 9 files at r4, all commit messages.
Reviewable status: LGTM missing from assignee jwnimmer-tri(platform)

[jwnimmer-tri]

platform

Reviewed 3 of 9 files at r4.
Reviewable status: 10 unresolved discussions

---

multibody/plant/BUILD.bazel line 1168 at r4 (raw file):

drake_cc_googletest(
    name = "multibody_plant_scalar_conversion_test",

This test is timing out now in CI in Debug builds:

TIMEOUT: //multibody/plant:multibody_plant_scalar_conversion_test

The best fix is probably setting shard_count = 4 or similar.

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc line 322 at r4 (raw file):

using DiscretePlantTestExpression = DiscretePlantTest<symbolic::Expression>;

std::vector<DiscretePlantTestConfig> MakeSupportMatrixTestCases() {

BTW An overview here would have helped me, along the lines of "Returns all permutations of {Similar, Tamsi} x {NoGeom, NoContact, Contact}".

It also seems like it'd easy to compact the code here by using loops, but maybe you anticipate that the options will vary to not be a straightforward, full permutation eventually?

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc line 324 at r4 (raw file):

std::vector<DiscretePlantTestConfig> MakeSupportMatrixTestCases() {
  return std::vector<DiscretePlantTestConfig>{
      // DiscreteContactApproximation::kSap

typo

Suggestion:

kSimilar

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc line 479 at r4 (raw file):

    failure_cause_message =
        "Discrete updates with the SAP solver are not supported for T = "
        "symbolic::Expression";

nit For this regex and all of the following ones, I'm not sure why we're going to all of the trouble to check in specific detail exactly which namespace qualifiers are printed out in our error messages? Either .* or [^ ]* seem plenty safe, yet add lot less clutter to the test code, and therefore make the actually important parts stand out.

Suggestion:

[^ ]*Expression

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc line 484 at r4 (raw file):

        ContactModel::kHydroelasticWithFallback) {
      failure_cause_message =
          "MultibodyPlant<T>::CalcHydroelasticWithFallback\\(\\): This method "

nit De-clutter

Suggestion:

.*CalcHydroelasticWithFallback.*:

---

multibody/test_utilities/robot_model.h line 27 at r4 (raw file):

  static const ContactProblemCache<T>& EvalContactProblemCache(
      const SapDriver<T>& driver, const systems::Context<T>& context) {
    return driver.EvalContactProblemCache(context);

https://drake.mit.edu/styleguide/cppguide.html#Friends

This is extremely unclear and surprising. Friendship should not span across such a wide valley. This code is not part of SapDriverTest. When code is marked private:, future developers will rightly assume than they can refactor it with abandon, but that's not true in this case. The "Eval" function is load-bearing for a lot of unit tests now.

The class SapDriver is already in a namespace internal. If we are going to call functions on it during unit tests of other code, then there is a simple and clear answer: change the EvalContactProblemCache function to be C++ public: visibility, with sufficient internal-developer-targeted comments to explain the situation.

---

multibody/test_utilities/robot_model.h line 112 at r4 (raw file):

  // Makes a state in which the robot's end effector touches the ground.
  // Velocities are zero.
  static Eigen::VectorXd RobotStateWithOneContactStiction() {

nit In https://drake.mit.edu/styleguide/cppguide.html#Inline_Functions there is a 5-prong test for function definitions in a header file. This function (and the next two also) does not meet any of them, so need to be in the cc file.

---

multibody/test_utilities/robot_model.h line 140 at r4 (raw file):

  // sparsity treatment withing the SAP solver, which removes non-participating
  // DOFs from the problem, and solves a reduced problem instead.
  void SetPlateInitialState() {

nit Ditto. Cannot be inline.

---

multibody/test_utilities/robot_model.h line 156 at r4 (raw file):

  // Parameters of the problem.
  const double kTimeStep_{0.001};      // Discrete time step of the plant.

nit See https://drake.mit.edu/styleguide/cppguide.html#Constant_Names. The names cannot use both kFoo constant spelling and foo_ member field spelling at the same time. Choose one or the other.

---

multibody/test_utilities/robot_model.h line 163 at r4 (raw file):

};

// Function that can be used to manually inspect the model used for testing. To

nit GSG https://drake.mit.edu/styleguide/cppguide.html#Function_Comments

The verb phrasing "can be used" is contrary to style. The sentence should be something more along the lines of "Publishes the default robot model for visualization. To see the model ...".

[amcastro-tri]

Reviewable status: 1 unresolved discussion

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc line 322 at r4 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW An overview here would have helped me, along the lines of "Returns all permutations of {Similar, Tamsi} x {NoGeom, NoContact, Contact}".

It also seems like it'd easy to compact the code here by using loops, but maybe you anticipate that the options will vary to not be a straightforward, full permutation eventually?

I was anticipating that originally, but clearly that's no longer true.
Changed to loops.

---

multibody/test_utilities/robot_model.h line 27 at r4 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

https://drake.mit.edu/styleguide/cppguide.html#Friends

This is extremely unclear and surprising. Friendship should not span across such a wide valley. This code is not part of SapDriverTest. When code is marked private:, future developers will rightly assume than they can refactor it with abandon, but that's not true in this case. The "Eval" function is load-bearing for a lot of unit tests now.

The class SapDriver is already in a namespace internal. If we are going to call functions on it during unit tests of other code, then there is a simple and clear answer: change the EvalContactProblemCache function to be C++ public: visibility, with sufficient internal-developer-targeted comments to explain the situation.

Done.

[jwnimmer-tri]

Reviewed 5 of 5 files at r5, all commit messages.
Reviewable status: 2 unresolved discussions

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc line 287 at r5 (raw file):

struct DiscretePlantTestConfig {
  // This is a gtest test suffix; no underscores or spaces at the start.
  std::string description;

BTW This is somewhat redundant. Given a robot_config, we could easily create the description string on the fly in operator<< based on the config. That would be less code, and reduces the possibility of accidental typos. It also means we don't need this weird struct, we could just parameterize the test on RobotModelConfig itself.

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc line 337 at r5 (raw file):

//   3. InContactState corresponds to a model (with geometry) in a state with
//      contact (i.e. with_contact_geometry=true and state_in_contact=true).
std::vector<DiscretePlantTestConfig> MakeSupportMatrixTestPermutations() {

BTW This is what googletest's testing::Combine(testing::Values(...), testing::Values...), ...) is for -- the full permutation of a set of choices.

It would mean replacing the pair of bools (of which only 3 of the 4 permutations are sensible) with an enum, but that would be a net benefit anyway. The two bools is a weird formation in the first place.

[jwnimmer-tri]

Reviewed 4 of 4 files at r6, all commit messages.
Reviewable status: 2 unresolved discussions

---

multibody/plant/BUILD.bazel line 1169 at r6 (raw file):

drake_cc_googletest(
    name = "multibody_plant_scalar_conversion_test",
    #    shard_count = 4,

typo

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc line 327 at r6 (raw file):

INSTANTIATE_TEST_SUITE_P(
    SupportMatrixTests, DiscretePlantTestDouble,
    ::testing::Combine(

BTW I'd imagine that the Combine...() boilerplate that's copied 3x could be compressed, either by assigning it to a constexpr variable, or making a helper function auto AllPermutations() { ... } that returns it.

[jwnimmer-tri]

Reviewed 2 of 2 files at r7, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees jwnimmer-tri(platform),SeanCurtis-TRI(platform)