Parallelize the GCS preprocessing stage.

Merge remote-tracking branch 'upstream/master' into gcs-preprocessing-parallel

[tools] Suppress more Valgrind false positives in MOSEK (RobotLocomotion#22140)

Update parsing infrastructure (RobotLocomotion#22102)

1. MultibodyPlant::RegisterVisualGeometry() can now take a GeometryInstance.
  - Clean up documentation.
  - Extend tests to include what is *actually* done.
2. SDF parser makes use of new API.
   - Stop returning optional<unique_ptr<>> (simply return the unique ptr).
   - Instead of constructing, deconstructing, and reconstructing geometry
     instances, we just pass the instance.
[schema] Add Rotation::Sample (RobotLocomotion#22113)

This bring it on par with Transform::Sample.
Move the warning on small-size PSD matrix to each solver backend. (RobotLocomotion#22136)

[solvers] Fix Gurobi console logging config to happen first (RobotLocomotion#22134)

[bindings] Re-enable some gym test cases on macOS (RobotLocomotion#22112)

[drake_gym] Add info_handler callback (RobotLocomotion#21900)

Set gradient sparsity pattern in LorentzConeConstraint and RotatedLorentzConeConstraint. (RobotLocomotion#22125)

Merge master.

Drop a TODO.

Fix bug when inadvertantly trying to preprocess edges into the start and goal vertex.

Hold a limited number of programs in memory at one time.

Move the construction of the preprocessing program to a separate function for better portability.

Merge up-to-date version of SolveInParallel.

Merge in preprocessing solver options behavior from branch.

wip

lint

Use the preprocessing solver options even if a specific preprocessing solver is not specified.

Fix another segfault.

Bring back the parallelization, revealing the segfault.

Update this test to reflect the new error message.

First pass at parallelizing PreprocessShortestPath.

Fix a segfault when initial_guesses is unset.

Temporarily disable the parallelism for debugging.

finalize interface

wip

bindings/pydrake/examples/gym/envs/cart_pole.py

@@ -1,5 +1,4 @@
 import gymnasium as gym
-import matplotlib.pyplot as plt
 import numpy as np
 
 from pydrake.common import FindResourceOrThrow
@@ -82,7 +81,7 @@ def make_sim(meshcat=None,
         time_step=sim_time_step,
         contact_model=contact_model,
         discrete_contact_approximation=contact_approximation,
-        )
+    )
 
     plant, scene_graph = AddMultibodyPlant(multibody_plant_config, builder)
 
@@ -125,6 +124,7 @@ def make_sim(meshcat=None,
         print("Actuation view: ", actuation_view(np.ones(na)), '\n')
 
         # Visualize the plant.
+        import matplotlib.pyplot as plt
         plt.figure()
         plot_graphviz(plant.GetTopologyGraphvizString())
         plt.plot(1)
@@ -371,6 +371,12 @@ def reset_handler(simulator, diagram_context, seed):
         body.SetMass(plant_context, mass+pair[1])
 
 
+def info_handler(simulator: Simulator) -> dict:
+    info = dict()
+    info["timestamp"] = simulator.get_context().get_time()
+    return info
+
+
 def DrakeCartPoleEnv(
         meshcat=None,
         time_limit=gym_time_limit,
@@ -415,6 +421,7 @@ def DrakeCartPoleEnv(
         action_port_id="actions",
         observation_port_id="observations",
         reset_handler=reset_handler,
+        info_handler=info_handler,
         render_rgb_port_id="color_image" if monitoring_camera else None)
 
     # Expose parameters that could be useful for learning.

bindings/pydrake/examples/gym/play_cart_pole.py

@@ -2,7 +2,6 @@
 Play a policy for //bindings/pydrake/examples/gym/envs:cart_pole.
 """
 import argparse
-import sys
 import warnings
 
 import gymnasium as gym
@@ -71,12 +70,6 @@ def _main():
     parser.add_argument('--log_path', help="path to the logs directory.")
     args = parser.parse_args()
 
-    if args.test and (sys.platform == "darwin"):
-        # TODO(#21577) Importing Gym on macOS Homebrew goes up in flames.
-        # We need to skip this test in Drake CI.
-        print("Testing is disabled when on macOS")
-        return
-
     if not args.debug:
         warnings.filterwarnings("ignore")
     gym.envs.register(id="DrakeCartPole-v0",

bindings/pydrake/geometry/geometry_py_common.cc

@@ -322,24 +322,27 @@ void DefineGeometryPropertiesSubclasses(py::module m) {
   {
     py::class_<IllustrationProperties, GeometryProperties> cls(
         m, "IllustrationProperties", doc.IllustrationProperties.doc);
-    cls.def(py::init(), doc.IllustrationProperties.ctor.doc)
-        .def(py::init<const IllustrationProperties&>(), py::arg("other"),
+    cls  // BR
+        .def(py::init(), doc.IllustrationProperties.ctor.doc)
+        .def(py::init<const GeometryProperties&>(), py::arg("other"),
             "Creates a copy of the properties");
     DefCopyAndDeepCopy(&cls);
   }
   {
     py::class_<PerceptionProperties, GeometryProperties> cls(
         m, "PerceptionProperties", doc.PerceptionProperties.doc);
-    cls.def(py::init(), doc.PerceptionProperties.ctor.doc)
-        .def(py::init<const PerceptionProperties&>(), py::arg("other"),
+    cls  // BR
+        .def(py::init(), doc.PerceptionProperties.ctor.doc)
+        .def(py::init<const GeometryProperties&>(), py::arg("other"),
             "Creates a copy of the properties");
     DefCopyAndDeepCopy(&cls);
   }
   {
     py::class_<ProximityProperties, GeometryProperties> cls(
         m, "ProximityProperties", doc.ProximityProperties.doc);
-    cls.def(py::init(), doc.ProximityProperties.ctor.doc)
-        .def(py::init<const ProximityProperties&>(), py::arg("other"),
+    cls  // BR
+        .def(py::init(), doc.ProximityProperties.ctor.doc)
+        .def(py::init<const GeometryProperties&>(), py::arg("other"),
             "Creates a copy of the properties");
     DefCopyAndDeepCopy(&cls);
   }

bindings/pydrake/geometry/geometry_py_optimization.cc

@@ -740,6 +740,10 @@ void DefineGraphOfConvexSetsAndRelated(py::module m) {
                       std::move(preprocessing_solver_options);
                 }),
             cls_doc.preprocessing_solver_options.doc)
+        .def_readwrite("preprocessing_parallelism",
+            &GraphOfConvexSetsOptions::preprocessing_parallelism)
+        .def_readwrite("preprocessing_parallel_batch_size",
+            &GraphOfConvexSetsOptions::preprocessing_parallel_batch_size)
         .def("__repr__", [](const GraphOfConvexSetsOptions& self) {
           return py::str(
               "GraphOfConvexSetsOptions("
@@ -755,13 +759,15 @@ void DefineGraphOfConvexSetsAndRelated(py::module m) {
               "solver_options={}, "
               "restriction_solver_options={}, "
               "preprocessing_solver_options={}, "
+              "preprocessing_parallel_batch_size={}, "
               ")")
               .format(self.convex_relaxation, self.preprocessing,
                   self.max_rounded_paths, self.max_rounding_trials,
                   self.flow_tolerance, self.rounding_seed, self.solver,
                   self.restriction_solver, self.preprocessing_solver,
                   self.solver_options, self.restriction_solver_options,
-                  self.preprocessing_solver_options);
+                  self.preprocessing_solver_options,
+                  self.preprocessing_parallel_batch_size);
         });
 
     DefReadWriteKeepAlive(&gcs_options, "solver",

bindings/pydrake/geometry/test/common_test.py

@@ -157,6 +157,17 @@ def test_geometry_properties_api(self):
             props_copy3 = copy.deepcopy(props)
             self.assertTrue(props_copy3.HasProperty("g", "p"))
 
+        # Cross-property-set copying. We don't do all cross possibilities.
+        # Merely confirm that each set can be copied from another set.
+        source = mut.PerceptionProperties()
+        source.AddProperty("a", "b", 10)
+        illustration = mut.IllustrationProperties(source)
+        self.assertEqual(illustration.GetProperty("a", "b"), 10)
+        proximity = mut.ProximityProperties(illustration)
+        self.assertEqual(proximity.GetProperty("a", "b"), 10)
+        perception = mut.PerceptionProperties(proximity)
+        self.assertEqual(perception.GetProperty("a", "b"), 10)
+
     def test_geometry_properties_cpp_types(self):
         """
         Confirms that types stored in properties in python, resolve to expected

bindings/pydrake/geometry/test/optimization_test.py

@@ -796,6 +796,8 @@ def test_graph_of_convex_sets(self):
         options.preprocessing_solver_options = SolverOptions()
         options.preprocessing_solver_options.SetOption(
             ClpSolver.id(), "log_level", 3)
+        options.preprocessing_parallelism = True
+        options.preprocessing_parallel_batch_size = 1000
         self.assertIn("scaling",
                       options.solver_options.GetOptions(ClpSolver.id()))
         self.assertIn("log_level",

bindings/pydrake/gym/_drake_gym_env.py

@@ -36,6 +36,7 @@ def __init__(self,
                  render_rgb_port_id: Union[OutputPortIndex, str] = None,
                  render_mode: str = 'human',
                  reset_handler: Callable[[Simulator, Context], None] = None,
+                 info_handler: Callable[[Simulator], dict] = None,
                  hardware: bool = False):
         """
         Args:
@@ -87,6 +88,12 @@ def __init__(self,
                 (e.g. ``joint.set_random_pose_distribution()``
                 using the ``reset()`` seed), (otherwise) using
                 ``reset_handler()``.
+            info_handler: A function that returns a ``dict[str, Any]``
+                containing auxiliary diagnostic information (helpful for
+                debugging, learning, and logging). Note: if ``step()``
+                terminates with a ``RuntimeError``, then, to avoid
+                unexpected behavior, `info_handler()`` will not be called
+                and an empty info will be returned instead.
             hardware: If True, it prevents from setting random context at
                 ``reset()`` when using ``random_generator``, but it does
                 execute ``reset_handler()`` if given.
@@ -158,6 +165,12 @@ def __init__(self,
         else:
             raise ValueError("reset_handler is not callable.")
 
+        # Default return value of `info_handler()` is an empty `dict`.
+        if info_handler is callable(info_handler):
+            self.info_handler = info_handler
+        else:
+            self.info_handler = lambda _: dict()
+
         self.hardware = hardware
 
         if self.simulator:
@@ -223,7 +236,6 @@ def step(self, action):
         truncated = False
         # Observation prior to advancing the simulation.
         prev_observation = self.observation_port.Eval(context)
-        info = dict()
         try:
             status = self.simulator.AdvanceTo(time + self.time_step)
         except RuntimeError as e:
@@ -245,6 +257,9 @@ def step(self, action):
             truncated = True
             terminated = False
             reward = 0
+            # Do not call info handler, as the simulator has faulted.
+            info = dict()
+
             return prev_observation, reward, terminated, truncated, info
 
         observation = self.observation_port.Eval(context)
@@ -253,6 +268,7 @@ def step(self, action):
             not truncated
             and (status.reason()
                  == SimulatorStatus.ReturnReason.kReachedTerminationCondition))
+        info = self.info_handler(self.simulator)
 
         return observation, reward, terminated, truncated, info
 
@@ -293,7 +309,9 @@ def reset(self, *,
         # Note: The output port will be evaluated without fixing the input
         # port.
         observations = self.observation_port.Eval(context)
-        return observations, dict()
+        info = self.info_handler(self.simulator)
+
+        return observations, info
 
     def render(self):
         """

bindings/pydrake/gym/test/drake_gym_test.py

@@ -1,4 +1,3 @@
-import sys
 import unittest
 
 import gymnasium as gym
@@ -26,13 +25,9 @@ def setUpClass(cls):
     def make_env(self):
         return gym.make("DrakeCartPole-v0")
 
-    # TODO(#21577) Importing Gym on macOS Homebrew goes up in flames.
-    @unittest.skipIf(sys.platform == "darwin", "Disabled macOS")
     def test_make_env(self):
         self.make_env()
 
-    # TODO(#21577) Importing Gym on macOS Homebrew goes up in flames.
-    @unittest.skipIf(sys.platform == "darwin", "Disabled macOS")
     def test_sb3_check_env(self):
         """Run stable-baselines's built-in test suite for our env."""
         dut = self.make_env()
@@ -45,8 +40,6 @@ def test_sb3_check_env(self):
     # supported versions of `gymnasium` and `stable_baselines3`, stable
     # baselines vector envs do not pass stable baselines' `check_env` tests.
 
-    # TODO(#21577) Importing Gym on macOS Homebrew goes up in flames.
-    @unittest.skipIf(sys.platform == "darwin", "Disabled macOS")
     def test_reset(self):
         # reset(int) sets a deterministic seed.
         dut = self.make_env()
@@ -68,8 +61,6 @@ def test_reset(self):
         self.assertIsInstance(opts, dict)
         self.assertTrue(dut.observation_space.contains(observation))
 
-    # TODO(#21577) Importing Gym on macOS Homebrew goes up in flames.
-    @unittest.skipIf(sys.platform == "darwin", "Disabled macOS")
     def test_step(self):
         dut = self.make_env()
         dut.reset()

bindings/pydrake/multibody/plant_py.cc

@@ -977,6 +977,12 @@ void DoScalarDependentDefinitions(py::module m, T) {
             py::arg("diffuse_color"),
             cls_doc.RegisterVisualGeometry
                 .doc_5args_body_X_BG_shape_name_diffuse_color)
+        .def("RegisterVisualGeometry",
+            py::overload_cast<const RigidBody<T>&,
+                std::unique_ptr<geometry::GeometryInstance>>(
+                &Class::RegisterVisualGeometry),
+            py::arg("body"), py::arg("geometry_instance"),
+            cls_doc.RegisterVisualGeometry.doc_2args_body_geometry_instance)
         .def("RegisterCollisionGeometry",
             py::overload_cast<const RigidBody<T>&,
                 const RigidTransform<double>&, const geometry::Shape&,

bindings/pydrake/multibody/test/plant_test.py

@@ -232,6 +232,11 @@ def test_multibody_plant_construction_api(self, T):
             plant.RegisterVisualGeometry(
                 body=body, X_BG=body_X_BG, shape=box, name="new_body_visual",
                 diffuse_color=[1., 0.64, 0.0, 0.5])
+            plant.RegisterVisualGeometry(
+                body=body,
+                geometry_instance=GeometryInstance(X_PG=body_X_BG,
+                                                   shape=Sphere(1.0),
+                                                   name="from_instance"))
             plant.RegisterCollisionGeometry(
                 body=body, X_BG=body_X_BG, shape=box,
                 name="new_body_collision", coulomb_friction=body_friction)

common/schema/rotation.cc

@@ -87,5 +87,12 @@ math::RotationMatrix<Expression> Rotation::ToSymbolic() const {
   }, value);
 }
 
+math::RotationMatrixd Rotation::Sample(RandomGenerator* generator) const {
+  const math::RotationMatrix<Expression> symbolic_rotation = this->ToSymbolic();
+  const math::RotationMatrixd concrete_rotation(
+      symbolic::Evaluate(symbolic_rotation.matrix(), {}, generator));
+  return concrete_rotation;
+}
+
 }  // namespace schema
 }  // namespace drake

common/schema/rotation.h

@@ -94,6 +94,10 @@ class Rotation {
   /// contain one or more random variables, based on the distributions in use.
   math::RotationMatrix<symbolic::Expression> ToSymbolic() const;
 
+  /// Samples this Rotation.  If this is deterministic, the result is the same
+  /// as GetDeterministicValue.
+  math::RotationMatrixd Sample(RandomGenerator* generator) const;
+
   /// Sets this value to the given deterministic RPY, in degrees.
   void set_rpy_deg(const Eigen::Vector3d& rpy_deg) {
     value.emplace<Rotation::Rpy>().deg = rpy_deg;

common/schema/test/rotation_test.cc

@@ -97,6 +97,25 @@ GTEST_TEST(RotationTest, RpyToYaml) {
             "root:\n  value: !Rpy\n    deg: [1.0, 2.0, 3.0]\n");
 }
 
+GTEST_TEST(RotationTest, Sample) {
+  constexpr const char* const yaml_data = R"""(
+  value: !Rpy { deg: !UniformVector { min: [0, 10, 20], max: [30, 40, 50] } }
+  )""";
+  const auto rotation = LoadYamlString<Rotation>(yaml_data);
+  drake::RandomGenerator generator(0);
+  const RollPitchYawd sample{rotation.Sample(&generator)};
+  const Vector3d rpy = sample.vector() * 180 / M_PI;
+  EXPECT_GE(rpy[0], 0);
+  EXPECT_GE(rpy[1], 10);
+  EXPECT_GE(rpy[2], 20);
+  EXPECT_LE(rpy[0], 30);
+  EXPECT_LE(rpy[1], 40);
+  EXPECT_LE(rpy[2], 50);
+
+  const RollPitchYawd sample2{rotation.Sample(&generator)};
+  EXPECT_NE(sample.vector(), sample2.vector());
+}
+
 }  // namespace
 }  // namespace schema
 }  // namespace drake

geometry/BUILD.bazel

@@ -962,6 +962,13 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "geometry_roles_test",
+    deps = [
+        ":geometry_roles",
+    ],
+)
+
 drake_cc_googletest(
     name = "geometry_version_test",
     deps = [

geometry/geometry_roles.cc

@@ -5,10 +5,19 @@
 namespace drake {
 namespace geometry {
 
+ProximityProperties::ProximityProperties(const GeometryProperties& other)
+    : GeometryProperties(other) {}
+
 ProximityProperties::~ProximityProperties() = default;
 
+PerceptionProperties::PerceptionProperties(const GeometryProperties& other)
+    : GeometryProperties(other) {}
+
 PerceptionProperties::~PerceptionProperties() = default;
 
+IllustrationProperties::IllustrationProperties(const GeometryProperties& other)
+    : GeometryProperties(other) {}
+
 IllustrationProperties::~IllustrationProperties() = default;
 
 std::string to_string(const Role& role) {