diff --git a/bindings/pydrake/examples/gym/envs/cart_pole.py b/bindings/pydrake/examples/gym/envs/cart_pole.py index 1448d9e0067a..0f10ddf7787b 100644 --- a/bindings/pydrake/examples/gym/envs/cart_pole.py +++ b/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. diff --git a/bindings/pydrake/examples/gym/play_cart_pole.py b/bindings/pydrake/examples/gym/play_cart_pole.py index 21f57b06f865..58e6f842d5da 100644 --- a/bindings/pydrake/examples/gym/play_cart_pole.py +++ b/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", diff --git a/bindings/pydrake/geometry/geometry_py_common.cc b/bindings/pydrake/geometry/geometry_py_common.cc index abb79b102668..7f7f56fb5ae0 100644 --- a/bindings/pydrake/geometry/geometry_py_common.cc +++ b/bindings/pydrake/geometry/geometry_py_common.cc @@ -322,24 +322,27 @@ void DefineGeometryPropertiesSubclasses(py::module m) { { py::class_ cls( m, "IllustrationProperties", doc.IllustrationProperties.doc); - cls.def(py::init(), doc.IllustrationProperties.ctor.doc) - .def(py::init(), py::arg("other"), + cls // BR + .def(py::init(), doc.IllustrationProperties.ctor.doc) + .def(py::init(), py::arg("other"), "Creates a copy of the properties"); DefCopyAndDeepCopy(&cls); } { py::class_ cls( m, "PerceptionProperties", doc.PerceptionProperties.doc); - cls.def(py::init(), doc.PerceptionProperties.ctor.doc) - .def(py::init(), py::arg("other"), + cls // BR + .def(py::init(), doc.PerceptionProperties.ctor.doc) + .def(py::init(), py::arg("other"), "Creates a copy of the properties"); DefCopyAndDeepCopy(&cls); } { py::class_ cls( m, "ProximityProperties", doc.ProximityProperties.doc); - cls.def(py::init(), doc.ProximityProperties.ctor.doc) - .def(py::init(), py::arg("other"), + cls // BR + .def(py::init(), doc.ProximityProperties.ctor.doc) + .def(py::init(), py::arg("other"), "Creates a copy of the properties"); DefCopyAndDeepCopy(&cls); } diff --git a/bindings/pydrake/geometry/geometry_py_optimization.cc b/bindings/pydrake/geometry/geometry_py_optimization.cc index e1f4e9c3689c..e8ed64ce96ae 100644 --- a/bindings/pydrake/geometry/geometry_py_optimization.cc +++ b/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", diff --git a/bindings/pydrake/geometry/test/common_test.py b/bindings/pydrake/geometry/test/common_test.py index d676e98d7a02..ca1e6e584181 100644 --- a/bindings/pydrake/geometry/test/common_test.py +++ b/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 diff --git a/bindings/pydrake/geometry/test/optimization_test.py b/bindings/pydrake/geometry/test/optimization_test.py index 789d6e85b08a..d6846af05914 100644 --- a/bindings/pydrake/geometry/test/optimization_test.py +++ b/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", diff --git a/bindings/pydrake/gym/_drake_gym_env.py b/bindings/pydrake/gym/_drake_gym_env.py index a7ef44986f77..5deae892d9ae 100644 --- a/bindings/pydrake/gym/_drake_gym_env.py +++ b/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): """ diff --git a/bindings/pydrake/gym/test/drake_gym_test.py b/bindings/pydrake/gym/test/drake_gym_test.py index 7e83fc38c795..69658f0472bc 100644 --- a/bindings/pydrake/gym/test/drake_gym_test.py +++ b/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() diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc index 73e4dd04bc54..3204186e5e19 100644 --- a/bindings/pydrake/multibody/plant_py.cc +++ b/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&, + std::unique_ptr>( + &Class::RegisterVisualGeometry), + py::arg("body"), py::arg("geometry_instance"), + cls_doc.RegisterVisualGeometry.doc_2args_body_geometry_instance) .def("RegisterCollisionGeometry", py::overload_cast&, const RigidTransform&, const geometry::Shape&, diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py index a0d8f2d6d39f..d3dd9a0faaef 100644 --- a/bindings/pydrake/multibody/test/plant_test.py +++ b/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) diff --git a/common/schema/rotation.cc b/common/schema/rotation.cc index bda53d781135..84d8b9096356 100644 --- a/common/schema/rotation.cc +++ b/common/schema/rotation.cc @@ -87,5 +87,12 @@ math::RotationMatrix Rotation::ToSymbolic() const { }, value); } +math::RotationMatrixd Rotation::Sample(RandomGenerator* generator) const { + const math::RotationMatrix symbolic_rotation = this->ToSymbolic(); + const math::RotationMatrixd concrete_rotation( + symbolic::Evaluate(symbolic_rotation.matrix(), {}, generator)); + return concrete_rotation; +} + } // namespace schema } // namespace drake diff --git a/common/schema/rotation.h b/common/schema/rotation.h index 02f58007ff33..f2428b9b97d4 100644 --- a/common/schema/rotation.h +++ b/common/schema/rotation.h @@ -94,6 +94,10 @@ class Rotation { /// contain one or more random variables, based on the distributions in use. math::RotationMatrix 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().deg = rpy_deg; diff --git a/common/schema/test/rotation_test.cc b/common/schema/test/rotation_test.cc index 701a49b28402..867607c9bcbe 100644 --- a/common/schema/test/rotation_test.cc +++ b/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(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 diff --git a/geometry/BUILD.bazel b/geometry/BUILD.bazel index 0c057b6cbbdf..4fa6cca2470f 100644 --- a/geometry/BUILD.bazel +++ b/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 = [ diff --git a/geometry/geometry_roles.cc b/geometry/geometry_roles.cc index 04dea0017292..4034ca747c2d 100644 --- a/geometry/geometry_roles.cc +++ b/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) { diff --git a/geometry/geometry_roles.h b/geometry/geometry_roles.h index d11948fca1d7..c39d4d370401 100644 --- a/geometry/geometry_roles.h +++ b/geometry/geometry_roles.h @@ -142,6 +142,17 @@ namespace geometry { type of role that it depends on, and the properties (if any) associated with that role that it requires/prefers. + There are times where different roles may have common properties (e.g., when + visualizing proximity geometries, we can specify their appearance in the + visualizer, or the appearance of a geometry should be the same for both + perception and illustration roles). To ease the sharing of these property + values, one set of properties can be instantiated as a copy of any other + set of geometry properties -- regardless of role. Doing so indiscriminately + may introduce meaningless properties, so use this power judiciously. A + property would be meaningless if there is no consumer that would use it. This + isn't an error, but it does mean that property values would be copied and + persisted without value. + Next topic: @ref proximity_queries */ /** The set of properties for geometry used in a _proximity_ role. @@ -155,8 +166,8 @@ namespace geometry { class ProximityProperties final : public GeometryProperties { public: DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ProximityProperties); - // TODO(SeanCurtis-TRI): Should this have the physical properties built in? ProximityProperties() = default; + explicit ProximityProperties(const GeometryProperties& other); ~ProximityProperties() final; }; @@ -168,8 +179,8 @@ class ProximityProperties final : public GeometryProperties { class PerceptionProperties final : public GeometryProperties { public: DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PerceptionProperties); - // TODO(SeanCurtis-TRI): Should this have a render label built in? PerceptionProperties() = default; + explicit PerceptionProperties(const GeometryProperties& other); ~PerceptionProperties() final; }; @@ -182,6 +193,7 @@ class IllustrationProperties final : public GeometryProperties { public: DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(IllustrationProperties); IllustrationProperties() = default; + explicit IllustrationProperties(const GeometryProperties& other); ~IllustrationProperties() final; }; diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel index 998d016a5c0a..f7e4802b3c69 100644 --- a/geometry/optimization/BUILD.bazel +++ b/geometry/optimization/BUILD.bazel @@ -201,6 +201,7 @@ drake_cc_library( "//solvers:get_program_type", "//solvers:mathematical_program_result", "//solvers:mosek_solver", + "//solvers:solve", "//solvers:solver_interface", ], ) diff --git a/geometry/optimization/graph_of_convex_sets.cc b/geometry/optimization/graph_of_convex_sets.cc index e07e9c725edb..d9ff373598ca 100644 --- a/geometry/optimization/graph_of_convex_sets.cc +++ b/geometry/optimization/graph_of_convex_sets.cc @@ -13,12 +13,14 @@ #include +#include "drake/common/parallelism.h" #include "drake/math/quadratic_form.h" #include "drake/solvers/choose_best_solver.h" #include "drake/solvers/create_constraint.h" #include "drake/solvers/create_cost.h" #include "drake/solvers/get_program_type.h" #include "drake/solvers/mosek_solver.h" +#include "drake/solvers/solve.h" namespace drake { namespace geometry { @@ -55,6 +57,7 @@ using solvers::ProgramType; using solvers::QuadraticCost; using solvers::RotatedLorentzConeConstraint; using solvers::SolutionResult; +using solvers::SolveInParallel; using solvers::VariableRefList; using solvers::VectorXDecisionVariable; using solvers::internal::CreateBinding; @@ -63,25 +66,6 @@ using symbolic::Variable; using symbolic::Variables; namespace { -MathematicalProgramResult SolvePreprocessingProgram( - const MathematicalProgram& prog, const GraphOfConvexSetsOptions& options) { - MathematicalProgramResult result; - solvers::SolverOptions preprocessing_solver_options = - options.preprocessing_solver_options.value_or(options.solver_options); - if (options.preprocessing_solver) { - options.preprocessing_solver->Solve(prog, {}, preprocessing_solver_options, - &result); - } else if (options.solver) { - options.solver->Solve(prog, {}, preprocessing_solver_options, &result); - } else { - solvers::SolverId solver_id = solvers::ChooseBestSolver(prog); - auto solver = solvers::MakeSolver(solver_id); - DRAKE_DEMAND(solver != nullptr); - solver->Solve(prog, {}, preprocessing_solver_options, &result); - } - return result; -} - MathematicalProgramResult SolveMainProgram( const MathematicalProgram& prog, const GraphOfConvexSetsOptions& options) { MathematicalProgramResult result; @@ -688,60 +672,35 @@ std::string GraphOfConvexSets::GetGraphvizString( return graphviz.str(); } -// Implements the preprocessing scheme put forth in Appendix A.2 of -// "Motion Planning around Obstacles with Convex Optimization": -// https://arxiv.org/abs/2205.04422 -std::set GraphOfConvexSets::PreprocessShortestPath( - VertexId source_id, VertexId target_id, - const GraphOfConvexSetsOptions& options) const { - if (vertices_.find(source_id) == vertices_.end()) { - throw std::runtime_error(fmt::format( - "Source vertex {} is not a vertex in this GraphOfConvexSets.", - source_id)); - } - if (vertices_.find(target_id) == vertices_.end()) { - throw std::runtime_error(fmt::format( - "Target vertex {} is not a vertex in this GraphOfConvexSets.", - target_id)); - } - - std::map> incoming_edges; - std::map> outgoing_edges; - std::set unusable_edges; - - int edge_count = 0; - for (const auto& [edge_id, e] : edges_) { - // Turn off edges into source or out of target - if (e->v().id() == source_id || e->u().id() == target_id) { - unusable_edges.insert(edge_id); - } else { - outgoing_edges[e->u().id()].push_back(edge_count); - incoming_edges[e->v().id()].push_back(edge_count); - } - - edge_count++; - } - +copyable_unique_ptr +GraphOfConvexSets::ConstructPreprocessingProgram( + EdgeId edge_id, const std::map>& incoming_edges, + const std::map>& outgoing_edges, + VertexId source_id, VertexId target_id) const { + const auto& e = edges_.at(edge_id); int nE = edges_.size(); - - // Given an edge (u,v) check if a path from source to u and another from v to - // target exist without sharing edges. - MathematicalProgram prog; + copyable_unique_ptr prog(MathematicalProgram{}); // Flow for each edge is between 0 and 1 for both paths. - VectorXDecisionVariable f = prog.NewContinuousVariables(nE, "flow_su"); + VectorXDecisionVariable f = prog->NewContinuousVariables(nE, "flow_su"); Binding f_limits = - prog.AddBoundingBoxConstraint(0, 1, f); - VectorXDecisionVariable g = prog.NewContinuousVariables(nE, "flow_vt"); + prog->AddBoundingBoxConstraint(0, 1, f); + VectorXDecisionVariable g = prog->NewContinuousVariables(nE, "flow_vt"); Binding g_limits = - prog.AddBoundingBoxConstraint(0, 1, g); + prog->AddBoundingBoxConstraint(0, 1, g); std::map> conservation_f; std::map> conservation_g; std::map> degree; for (const auto& [vertex_id, v] : vertices_) { - std::vector Ev_in = incoming_edges[vertex_id]; - std::vector Ev_out = outgoing_edges[vertex_id]; + auto maybe_Ev_in = incoming_edges.find(vertex_id); + std::vector Ev_in = maybe_Ev_in != incoming_edges.end() + ? maybe_Ev_in->second + : std::vector{}; + auto maybe_Ev_out = outgoing_edges.find(vertex_id); + std::vector Ev_out = maybe_Ev_out != outgoing_edges.end() + ? maybe_Ev_out->second + : std::vector{}; std::vector Ev = Ev_in; Ev.insert(Ev.end(), Ev_out.begin(), Ev_out.end()); @@ -759,19 +718,19 @@ std::set GraphOfConvexSets::PreprocessShortestPath( // Conservation of flow for f: ∑ f_in - ∑ f_out = -δ(is_source). if (vertex_id == source_id) { conservation_f.insert( - {vertex_id, prog.AddLinearEqualityConstraint(A_flow, -1, fv)}); + {vertex_id, prog->AddLinearEqualityConstraint(A_flow, -1, fv)}); } else { conservation_f.insert( - {vertex_id, prog.AddLinearEqualityConstraint(A_flow, 0, fv)}); + {vertex_id, prog->AddLinearEqualityConstraint(A_flow, 0, fv)}); } // Conservation of flow for g: ∑ g_in - ∑ g_out = δ(is_target). if (vertex_id == target_id) { conservation_g.insert( - {vertex_id, prog.AddLinearEqualityConstraint(A_flow, 1, gv)}); + {vertex_id, prog->AddLinearEqualityConstraint(A_flow, 1, gv)}); } else { conservation_g.insert( - {vertex_id, prog.AddLinearEqualityConstraint(A_flow, 0, gv)}); + {vertex_id, prog->AddLinearEqualityConstraint(A_flow, 0, gv)}); } } @@ -785,72 +744,145 @@ std::set GraphOfConvexSets::PreprocessShortestPath( fgin(Ev_in.size() + ii) = g(Ev_in[ii]); } degree.insert( - {vertex_id, prog.AddLinearConstraint(A_degree, 0, 1, fgin)}); + {vertex_id, prog->AddLinearConstraint(A_degree, 0, 1, fgin)}); } } - for (const auto& [edge_id, e] : edges_) { - if (unusable_edges.contains(edge_id)) { - continue; - } + // Update bounds of conservation of flow: + // ∑ f_in,u - ∑ f_out,u = 1 - δ(is_source). + if (e->u().id() == source_id) { + f_limits.evaluator()->set_bounds(VectorXd::Zero(nE), VectorXd::Zero(nE)); + conservation_f.at(e->u().id()) + .evaluator() + ->set_bounds(Vector1d(0), Vector1d(0)); + } else { + conservation_f.at(e->u().id()) + .evaluator() + ->set_bounds(Vector1d(1), Vector1d(1)); + } + // ∑ g_in,v - ∑ f_out,v = δ(is_target) - 1. + if (e->v().id() == target_id) { + g_limits.evaluator()->set_bounds(VectorXd::Zero(nE), VectorXd::Zero(nE)); + conservation_g.at(e->v().id()) + .evaluator() + ->set_bounds(Vector1d(0), Vector1d(0)); + } else { + conservation_g.at(e->v().id()) + .evaluator() + ->set_bounds(Vector1d(-1), Vector1d(-1)); + } - // Update bounds of conservation of flow: - // ∑ f_in,u - ∑ f_out,u = 1 - δ(is_source). - if (e->u().id() == source_id) { - f_limits.evaluator()->set_bounds(VectorXd::Zero(nE), VectorXd::Zero(nE)); - conservation_f.at(e->u().id()) - .evaluator() - ->set_bounds(Vector1d(0), Vector1d(0)); - } else { - conservation_f.at(e->u().id()) - .evaluator() - ->set_bounds(Vector1d(1), Vector1d(1)); - } - // ∑ g_in,v - ∑ f_out,v = δ(is_target) - 1. - if (e->v().id() == target_id) { - g_limits.evaluator()->set_bounds(VectorXd::Zero(nE), VectorXd::Zero(nE)); - conservation_g.at(e->v().id()) - .evaluator() - ->set_bounds(Vector1d(0), Vector1d(0)); + // Update bounds of degree constraints: + // ∑ f_in,v + ∑ g_in,v = 0. + if (degree.contains(e->v().id())) { + degree.at(e->v().id()).evaluator()->set_bounds(Vector1d(0), Vector1d(0)); + } + + return prog; +} + +// Implements the preprocessing scheme put forth in Appendix A.2 of +// "Motion Planning around Obstacles with Convex Optimization": +// https://arxiv.org/abs/2205.04422 +std::set GraphOfConvexSets::PreprocessShortestPath( + VertexId source_id, VertexId target_id, + const GraphOfConvexSetsOptions& options) const { + if (vertices_.find(source_id) == vertices_.end()) { + throw std::runtime_error(fmt::format( + "Source vertex {} is not a vertex in this GraphOfConvexSets.", + source_id)); + } + if (vertices_.find(target_id) == vertices_.end()) { + throw std::runtime_error(fmt::format( + "Target vertex {} is not a vertex in this GraphOfConvexSets.", + target_id)); + } + + std::vector edge_id_list; + std::map> incoming_edges; + std::map> outgoing_edges; + std::set unusable_edges; + + int edge_count = 0; + for (const auto& [edge_id, e] : edges_) { + // Turn off edges into source or out of target + if (e->v().id() == source_id || e->u().id() == target_id) { + unusable_edges.insert(edge_id); } else { - conservation_g.at(e->v().id()) - .evaluator() - ->set_bounds(Vector1d(-1), Vector1d(-1)); + outgoing_edges[e->u().id()].push_back(edge_count); + incoming_edges[e->v().id()].push_back(edge_count); + + // Note: we only add the edge id to this list if we still need to check + // it. + edge_id_list.push_back(edge_id); } - // Update bounds of degree constraints: - // ∑ f_in,v + ∑ g_in,v = 0. - degree.at(e->v().id()).evaluator()->set_bounds(Vector1d(0), Vector1d(0)); + edge_count++; + } - // Check if edge e = (u,v) could be on a path from start to goal. - auto result = SolvePreprocessingProgram(prog, options); - if (!result.is_success()) { - unusable_edges.insert(edge_id); + // edge_id_list is now the canonical list of edges we will check in the + // remainder of the function. + int nE = edge_id_list.size(); + + // TODO(cohnt): Rewrite with a parallel for loop where each thread creates and + // solves the preprocessing program, to avoid having to use batching together + // with SolveInParallel. + + // Given an edge (u,v) check if a path from source to u and another from v to + // target exist without sharing edges. + + int num_batches = 1 + (nE / options.preprocessing_parallel_batch_size); + for (int batch_idx = 0; batch_idx < num_batches; ++batch_idx) { + int this_batch_start = + batch_idx * options.preprocessing_parallel_batch_size; + int this_batch_end = std::min( + (batch_idx + 1) * options.preprocessing_parallel_batch_size, nE); + // TODO(cohnt): No longer use pointers here. + std::vector> progs; + std::vector idx_to_edge_id; + + int this_batch_nE = this_batch_end - this_batch_start; + progs.reserve(this_batch_nE); + idx_to_edge_id.reserve(this_batch_nE); + + for (int i = this_batch_start; i < this_batch_end; ++i) { + idx_to_edge_id.push_back(edge_id_list.at(i)); } - // Reset constraint bounds. - if (e->u().id() == source_id) { - f_limits.evaluator()->set_bounds(VectorXd::Zero(nE), VectorXd::Ones(nE)); - conservation_f.at(e->u().id()) - .evaluator() - ->set_bounds(Vector1d(-1), Vector1d(-1)); - } else { - conservation_f.at(e->u().id()) - .evaluator() - ->set_bounds(Vector1d(0), Vector1d(0)); + for (int i = 0; i < this_batch_nE; ++i) { + EdgeId edge_id = idx_to_edge_id.at(i); + progs.emplace_back(ConstructPreprocessingProgram( + edge_id, incoming_edges, outgoing_edges, source_id, target_id)); } - if (e->v().id() == target_id) { - g_limits.evaluator()->set_bounds(VectorXd::Zero(nE), VectorXd::Ones(nE)); - conservation_g.at(e->v().id()) - .evaluator() - ->set_bounds(Vector1d(1), Vector1d(1)); + + // Check if edge e = (u,v) could be on a path from start to goal. + std::vector prog_ptrs(progs.size()); + for (int i = 0; i < ssize(progs); ++i) { + prog_ptrs[i] = progs[i].get(); + } + std::optional maybe_solver_id; + solvers::SolverOptions preprocessing_solver_options = + options.preprocessing_solver_options.value_or(options.solver_options); + if (options.preprocessing_solver) { + maybe_solver_id = options.preprocessing_solver->solver_id(); + } else if (options.solver) { + maybe_solver_id = options.solver->solver_id(); } else { - conservation_g.at(e->v().id()) - .evaluator() - ->set_bounds(Vector1d(0), Vector1d(0)); + maybe_solver_id = std::nullopt; + } + + std::vector results = SolveInParallel( + prog_ptrs, nullptr, &preprocessing_solver_options, maybe_solver_id, + options.preprocessing_parallelism, false); + + for (int i = 0; i < this_batch_nE; ++i) { + const auto& result = results.at(i); + if (!result.is_success()) { + unusable_edges.insert(idx_to_edge_id.at(i)); + } } - degree.at(e->v().id()).evaluator()->set_bounds(Vector1d(0), Vector1d(1)); } + return unusable_edges; } diff --git a/geometry/optimization/graph_of_convex_sets.h b/geometry/optimization/graph_of_convex_sets.h index b404c3276c41..e6088bf35bb2 100644 --- a/geometry/optimization/graph_of_convex_sets.h +++ b/geometry/optimization/graph_of_convex_sets.h @@ -12,6 +12,7 @@ #include #include "drake/common/eigen_types.h" +#include "drake/common/parallelism.h" #include "drake/common/symbolic/expression.h" #include "drake/geometry/optimization/convex_set.h" #include "drake/solvers/mathematical_program_result.h" @@ -34,6 +35,7 @@ struct GraphOfConvexSetsOptions { a->Visit(DRAKE_NVP(max_rounding_trials)); a->Visit(DRAKE_NVP(flow_tolerance)); a->Visit(DRAKE_NVP(rounding_seed)); + a->Visit(DRAKE_NVP(preprocessing_parallel_batch_size)); // N.B. We skip the DRAKE_NVP(solver), DRAKE_NVP(restriction_solver), and // DRAKE_NVP(preprocessing_solver), because it cannot be serialized. // TODO(#20967) Serialize the DRAKE_NVP(solver_options). @@ -124,6 +126,13 @@ struct GraphOfConvexSetsOptions { optimization, but not from the many smaller preprocessing optimizations. */ std::optional preprocessing_solver_options{ std::nullopt}; + + /** Degree of parallelism to use when performing the preprocessing. */ + Parallelism preprocessing_parallelism{Parallelism::Max()}; + + /** Set the maximum number of preprocessing programs that are constructed in + * memory at once. */ + int preprocessing_parallel_batch_size{1000}; }; struct GcsGraphvizOptions { @@ -855,6 +864,16 @@ class GraphOfConvexSets { private: /* Facilitates testing. */ friend class PreprocessShortestPathTest; + // Modify prog so that it contains the variables and constraints of the + // preprocessing program for a given edge. + copyable_unique_ptr + ConstructPreprocessingProgram( + EdgeId edge_id, + const std::map>& incoming_edges, + + const std::map>& outgoing_edges, + VertexId source_id, VertexId target_id) const; + std::set PreprocessShortestPath( VertexId source_id, VertexId target_id, const GraphOfConvexSetsOptions& options) const; diff --git a/geometry/test/geometry_roles_test.cc b/geometry/test/geometry_roles_test.cc new file mode 100644 index 000000000000..11435b4d73a7 --- /dev/null +++ b/geometry/test/geometry_roles_test.cc @@ -0,0 +1,44 @@ +#include "drake/geometry/geometry_roles.h" + +#include + +#include + +namespace drake { +namespace geometry { +namespace { + +// An instance of XProperties can be constructed from YProperties (the result is +// that it simply copies all of the properties). Remember, that the derived +// FooProperties classes add no functionality to GeometryProperties -- they are +// merely a type distinction. +GTEST_TEST(GeometryRoleTest, CopyFromOther) { + PerceptionProperties source; + source.AddProperty("test", "int", 10); + source.AddProperty("test", "string", "value"); + + const GeometryProperties& generic = source; + + ProximityProperties proximity(generic); + EXPECT_EQ(proximity.GetProperty("test", "int"), + source.GetProperty("test", "int")); + EXPECT_EQ(proximity.GetProperty("test", "string"), + source.GetProperty("test", "string")); + + IllustrationProperties illustration(generic); + EXPECT_EQ(illustration.GetProperty("test", "int"), + source.GetProperty("test", "int")); + EXPECT_EQ(illustration.GetProperty("test", "string"), + source.GetProperty("test", "string")); + + // Proximity -> Perception. + PerceptionProperties perception(generic); + EXPECT_EQ(perception.GetProperty("test", "int"), + source.GetProperty("test", "int")); + EXPECT_EQ(perception.GetProperty("test", "string"), + source.GetProperty("test", "string")); +} + +} // namespace +} // namespace geometry +} // namespace drake diff --git a/multibody/parsing/detail_sdf_geometry.cc b/multibody/parsing/detail_sdf_geometry.cc index f2d11f6c94c6..36f806a9c598 100644 --- a/multibody/parsing/detail_sdf_geometry.cc +++ b/multibody/parsing/detail_sdf_geometry.cc @@ -35,6 +35,7 @@ using std::string; using drake::internal::DiagnosticPolicy; using geometry::GeometryInstance; using geometry::IllustrationProperties; +using geometry::PerceptionProperties; using geometry::ProximityProperties; using math::RigidTransformd; @@ -67,9 +68,8 @@ std::optional GetChildElementValue(const SDFormatDiagnostic& diagnostic, } // namespace -std::optional> MakeShapeFromSdfGeometry( - const SDFormatDiagnostic& diagnostic, - const sdf::Geometry& sdf_geometry, +std::unique_ptr MakeShapeFromSdfGeometry( + const SDFormatDiagnostic& diagnostic, const sdf::Geometry& sdf_geometry, ResolveFilename resolve_filename) { // TODO(amcastro-tri): unit tests for different error paths are needed. const std::set supported_geometry_elements{ @@ -104,10 +104,10 @@ std::optional> MakeShapeFromSdfGeometry( diagnostic, capsule_element, {"radius", "length"}); std::optional radius = GetChildElementValue(diagnostic, capsule_element, "radius"); - if (!radius.has_value()) return std::nullopt; + if (!radius.has_value()) return nullptr; std::optional length = GetChildElementValue(diagnostic, capsule_element, "length"); - if (!length.has_value()) return std::nullopt; + if (!length.has_value()) return nullptr; return make_unique(*radius, *length); } else if (sdf_geometry.Element()->HasElement("drake:ellipsoid")) { const sdf::ElementPtr ellipsoid_element = @@ -116,17 +116,17 @@ std::optional> MakeShapeFromSdfGeometry( diagnostic, ellipsoid_element, {"a", "b", "c"}); std::optional a = GetChildElementValue(diagnostic, ellipsoid_element, "a"); - if (!a.has_value()) return std::nullopt; + if (!a.has_value()) return nullptr; std::optional b = GetChildElementValue(diagnostic, ellipsoid_element, "b"); - if (!b.has_value()) return std::nullopt; + if (!b.has_value()) return nullptr; std::optional c = GetChildElementValue(diagnostic, ellipsoid_element, "c"); - if (!c.has_value()) return std::nullopt; + if (!c.has_value()) return nullptr; return make_unique(*a, *b, *c); } - return std::unique_ptr(nullptr); + return nullptr; } case sdf::GeometryType::BOX: { const sdf::Box& shape = *sdf_geometry.BoxShape(); @@ -160,14 +160,17 @@ std::optional> MakeShapeFromSdfGeometry( DRAKE_DEMAND(mesh_element != nullptr); std::optional mesh_uri = GetChildElementValue(diagnostic, mesh_element, "uri"); - if (!mesh_uri.has_value()) return std::nullopt; + // We don't register an error; this is a required element and the sdformat + // parsing should handle the case of this missing. + DRAKE_DEMAND(mesh_uri.has_value()); + if (!mesh_uri.has_value()) return nullptr; const std::string file_name = resolve_filename(diagnostic, *mesh_uri); double scale = 1.0; if (mesh_element->HasElement("scale")) { std::optional scale_vector = GetChildElementValue( diagnostic, mesh_element, "scale"); - if (!scale_vector.has_value()) return std::nullopt; + if (!scale_vector.has_value()) return nullptr; // geometry::Mesh only supports isotropic scaling and therefore we // enforce it. if (!(scale_vector->X() == scale_vector->Y() && @@ -176,7 +179,7 @@ std::optional> MakeShapeFromSdfGeometry( "Drake meshes only support isotropic scaling. Therefore all " "three scaling factors must be exactly equal."; diagnostic.Error(mesh_element, std::move(message)); - return std::nullopt; + return nullptr; } scale = scale_vector->X(); } @@ -212,11 +215,9 @@ std::optional> MakeShapeFromSdfGeometry( static constexpr char kAcceptingTag[] = "drake:accepting_renderer"; -std::optional> - MakeGeometryInstanceFromSdfVisual( - const SDFormatDiagnostic& diagnostic, - const sdf::Visual& sdf_visual, ResolveFilename resolve_filename, - const math::RigidTransformd& X_LG) { +std::unique_ptr MakeGeometryInstanceFromSdfVisual( + const SDFormatDiagnostic& diagnostic, const sdf::Visual& sdf_visual, + ResolveFilename resolve_filename, const math::RigidTransformd& X_LG) { const std::set supported_visual_elements{ "geometry", "material", @@ -232,7 +233,7 @@ std::optional> // drake:capsule, before we can decide to return a null geometry. if (!sdf_geometry.Element()->HasElement("drake:capsule") && !sdf_geometry.Element()->HasElement("drake:ellipsoid")) { - return std::unique_ptr(nullptr); + return nullptr; } } @@ -283,19 +284,25 @@ std::optional> } } - auto shape = MakeShapeFromSdfGeometry( + std::unique_ptr shape = MakeShapeFromSdfGeometry( diagnostic, sdf_geometry, resolve_filename); - if (!shape.has_value()) return std::nullopt; - if (*shape == nullptr) { - return nullptr; - } + if (shape == nullptr) return nullptr; + auto instance = make_unique(X_LC, std::move(*shape), sdf_visual.Name()); std::optional illustration_properties = MakeVisualPropertiesFromSdfVisual( diagnostic, sdf_visual, resolve_filename); - if (!illustration_properties.has_value()) return std::nullopt; - instance->set_illustration_properties(*illustration_properties); + if (!illustration_properties.has_value()) return nullptr; + instance->set_perception_properties( + PerceptionProperties(*illustration_properties)); + if (illustration_properties->HasProperty("renderer", "accepting")) { + // We stashed the perception property in the illustration properties. It has + // been copied into the perception properties, so we can remove it from here + // now. + illustration_properties->RemoveProperty("renderer", "accepting"); + } + instance->set_illustration_properties(*std::move(illustration_properties)); return instance; } @@ -371,11 +378,10 @@ std::optional MakeVisualPropertiesFromSdfVisual( add_property("emissive", &properties); } - // TODO(SeanCurtis-TRI): Including this property in illustration properties is - // a bit misleading; it isn't used by illustration, but we're not currently - // parsing illustration and perception properties separately. So, we stash - // them in the illustration properties relying on it to be ignored by - // illustration consumers but copied over to the perception properties. + // We're putting this perception property into the illustration properties. + // We'll create the PerceptionProperties from these illustration properties + // and simply remove this property from the illustration properties as clean + // up. if (visual_element->HasElement(kAcceptingTag)) { set accepting_names; sdf::ElementPtr accepting = visual_element->GetElement(kAcceptingTag); diff --git a/multibody/parsing/detail_sdf_geometry.h b/multibody/parsing/detail_sdf_geometry.h index 68855789740d..39e2aa81b9ef 100644 --- a/multibody/parsing/detail_sdf_geometry.h +++ b/multibody/parsing/detail_sdf_geometry.h @@ -28,23 +28,26 @@ using ResolveFilename = std::function element from an SDF file, this method makes a new drake::geometry::Shape object from this specification. - If no recognizable geometry is specified, nullptr is returned. If the geometry - is recognized, but malformed, emits an error. When the error policy is set to - not throw it returns std::nullopt. */ -std::optional> MakeShapeFromSdfGeometry( - const SDFormatDiagnostic& diagnostic, - const sdf::Geometry& sdf_geometry, + If no recognizable geometry is specified or the geometry is recognized but + malformed, nullptr is returned. In the case of a malformed specification, an + error is also emitted on `diagnostic`. */ +std::unique_ptr MakeShapeFromSdfGeometry( + const SDFormatDiagnostic& diagnostic, const sdf::Geometry& sdf_geometry, ResolveFilename resolve_filename); /* Given an sdf::Visual object representing a element from an SDF file, this method makes a new drake::geometry::GeometryInstance object from - this specification at a pose `X_LG` relatve to its parent link. - This method returns nullptr when the given SDF specification corresponds - to an uninterpreted geometry type: - - `sdf::GeometryType::EMPTY` (`` SDF tag.) - - `sdf::GeometryType::HEIGHTMAP` (`` SDF tag.) - If the geometry is malformed, emits an error. When the error policy is not - set to throw it returns an std::nullopt. + this specification at a pose `X_LG` relative to its parent link. + + This method returns null when: + 1. the given SDF specification corresponds to an uninterpreted geometry type: + - `sdf::GeometryType::EMPTY` (`` SDF tag.) + - `sdf::GeometryType::HEIGHTMAP` (`` SDF tag.) + 2. the has had both of its Drake roles disabled. + 3. the specification was in some way malformed. + + In the case of a malformed specification, an error is also emitted to + `diagnostic`.