diff --git a/bindings/pydrake/systems/framework_py_systems.cc b/bindings/pydrake/systems/framework_py_systems.cc index e10d0c30375a..f1f3f4b276f2 100644 --- a/bindings/pydrake/systems/framework_py_systems.cc +++ b/bindings/pydrake/systems/framework_py_systems.cc @@ -17,6 +17,7 @@ #include "drake/systems/framework/system_visitor.h" #include "drake/systems/framework/vector_system.h" #include "drake/systems/framework/witness_function.h" +#include "drake/systems/framework/wrapped_system.h" using std::make_unique; using std::string; @@ -45,6 +46,7 @@ using systems::SystemVisitor; using systems::UnrestrictedUpdateEvent; using systems::VectorSystem; using systems::WitnessFunction; +using systems::internal::WrappedSystem; // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. using namespace drake::systems; @@ -91,8 +93,7 @@ struct Impl { // (LeafSystemPublic::*)(...), since this typeid is not exposed in pybind. // If needed, solution is to expose it as an intermediate type if needed. - // Expose protected methods for binding, no need for virtual overrides - // (ordered by how they are bound). + // Expose protected methods for binding, no need for virtual overrides. using Base::DeclareAbstractInputPort; using Base::DeclareAbstractOutputPort; using Base::DeclareAbstractParameter; @@ -110,6 +111,7 @@ struct Impl { using Base::get_mutable_forced_discrete_update_events; using Base::get_mutable_forced_publish_events; using Base::get_mutable_forced_unrestricted_update_events; + using Base::HandlePostConstructionScalarConversion; using Base::MakeWitnessFunction; // Because `LeafSystem::DoCalcTimeDerivatives` is protected, and we had @@ -557,13 +559,24 @@ Note: The above is for the C++ documentation. For Python, use doc.System.ToSymbolicMaybe.doc) .def("FixInputPortsFrom", &System::FixInputPortsFrom, py::arg("other_system"), py::arg("other_context"), - py::arg("target_context"), doc.System.FixInputPortsFrom.doc); + py::arg("target_context"), doc.System.FixInputPortsFrom.doc) + .def("get_system_scalar_converter", + &System::get_system_scalar_converter, py_rvp::reference_internal, + doc.System.get_system_scalar_converter.doc); auto def_to_scalar_type = [&cls](auto dummy) { using U = decltype(dummy); AddTemplateMethod( cls, "ToScalarType", [](const System& self) { return self.template ToScalarType(); }, GetPyParam(), doc.System.ToScalarType.doc_0args); + AddTemplateMethod( + cls, "_HandlePostConstructionScalarConversion", + [](System& self, const System& from) { + LeafSystemPublic::HandlePostConstructionScalarConversion( + from, &self); + }, + GetPyParam(), + doc.System.HandlePostConstructionScalarConversion.doc); }; type_visit(def_to_scalar_type, CommonScalarPack{}); @@ -1067,6 +1080,16 @@ Note: The above is for the C++ documentation. For Python, use } } + static void DefineWrappedSystem(py::module m) { + using Class = WrappedSystem; + auto cls = DefineTemplateClassWithDefault>(m, + "_WrappedSystem", GetPyParam(), + "Wrapper that enables scalar-conversion of Python leaf systems."); + cls // BR + .def("unwrap", &Class::unwrap, py_rvp::reference_internal, + "Returns the underlying system."); + } + template static void DefineSystemVisitor(py::module m, PyClass* system_cls) { // TODO(eric.cousineau): Bind virtual methods once we provide a function @@ -1254,17 +1277,37 @@ void DefineSystemScalarConverter(PyClass* cls) { &SystemScalarConverter::IsConvertible, GetPyParam(), cls_doc.IsConvertible.doc); using system_scalar_converter_internal::AddPydrakeConverterFunction; - using ConverterFunction = - std::function>(const System&)>; - AddTemplateMethod(converter, "_Add", - WrapCallbacks( - [](SystemScalarConverter* self, const ConverterFunction& func) { - const std::function*(const System&)> bare_func = - [func](const System& other) { - return func(other).release(); - }; - AddPydrakeConverterFunction(self, bare_func); - }), + // N.B. The "_AddConstructor" method is called by scalar_conversion.py + // to register a constructor, similar to MaybeAddConstructor in C++. + using ConverterFunction = std::function*(const System&)>; + AddTemplateMethod( + converter, "_AddConstructor", + [](SystemScalarConverter* self, + py::function python_converter_function) { + AddPydrakeConverterFunction(self, + ConverterFunction{ + [python_converter_function](const System& system_u_cpp) { + py::gil_scoped_acquire guard; + // Call the Python converter function. + py::object system_u_py = + py::cast(system_u_cpp, py_rvp::reference_internal); + py::object system_t_py = + python_converter_function(system_u_py); + DRAKE_THROW_UNLESS(!system_t_py.is_none()); + // Cast its result to a shared_ptr. + std::shared_ptr> system_t_cpp = + make_shared_ptr_from_py_object>( + std::move(system_t_py)); + // Wrap the result in a Diagram so we have a unique_ptr + // instead of a shared_ptr. + std::unique_ptr> result = + std::make_unique>( + std::move(system_t_cpp)); + // Our contract is to return an owned raw pointer. Our + // caller will wrap the unique_ptr back around it. + return result.release(); + }}); + }, GetPyParam()); }; // N.B. When changing the pairs of supported types below, ensure that these @@ -1314,6 +1357,7 @@ void DefineFrameworkPySystems(py::module m) { Impl::DefineLeafSystem(m); Impl::DefineDiagram(m); Impl::DefineVectorSystem(m); + Impl::DefineWrappedSystem(m); Impl::DefineSystemVisitor(m, cls_system); }; type_visit(bind_common_scalar_types, CommonScalarPack{}); diff --git a/bindings/pydrake/systems/scalar_conversion.py b/bindings/pydrake/systems/scalar_conversion.py index 5a3bcebd0ac2..450f78c0bb23 100644 --- a/bindings/pydrake/systems/scalar_conversion.py +++ b/bindings/pydrake/systems/scalar_conversion.py @@ -3,7 +3,9 @@ import copy from functools import partial +from pydrake.autodiffutils import AutoDiffXd from pydrake.common import pretty_class_name +from pydrake.symbolic import Expression from pydrake.systems.framework import ( LeafSystem_, SystemScalarConverter, @@ -11,6 +13,7 @@ from pydrake.common.cpp_template import ( _get_module_from_stack, TemplateClass, + TemplateMethod, ) @@ -194,6 +197,30 @@ def system_init(self, *args, **kwargs): self, *args, converter=converter, **kwargs) cls.__init__ = system_init + + # Patch the scalar-conversion functions (only when called from Python, + # not when called from C++) to return the Python type instead of the + # WrappedSystem shim. + cls.ToScalarType = TemplateMethod( + cls=cls, name="ToScalarType") + cls.ToScalarTypeMaybe = TemplateMethod( + cls=cls, name="ToScalarTypeMaybe") + for U in SystemScalarConverter.SupportedScalars: + new_method = template._make_new_to_scalar_type_method( + T=U, U=T, maybe=False) + new_method_maybe = template._make_new_to_scalar_type_method( + T=U, U=T, maybe=True) + if U == AutoDiffXd: + cls.ToAutoDiffXd = new_method + cls.ToAutoDiffXdMaybe = new_method_maybe + elif U == Expression: + cls.ToSymbolic = new_method + cls.ToSymbolicMaybe = new_method_maybe + cls.ToScalarType.add_instantiation( + param=[U,], instantiation=new_method) + cls.ToScalarTypeMaybe.add_instantiation( + param=[U,], instantiation=new_method_maybe) + return cls def _check_if_copying(self, obj, *args, **kwargs): @@ -222,5 +249,27 @@ def _make_converter(self): # to when the conversion is called. for (T, U) in self._T_pairs: conversion = partial(self._make, T, U) - converter._Add[T, U](conversion) + converter._AddConstructor[T, U](conversion) return converter + + def _make_new_to_scalar_type_method(self, *, T, U, maybe): + # Helper for _on_add for replacing the ToScalarType{Maybe}_ methods + # with pure-python implementations that avoid C++ lifetime challenges. + # U is the "from" type (the type of the System being called). + # T is the "to" type (the type of System to return). + def _to_scalar_type(system_U): + converter = system_U.get_system_scalar_converter() + if not converter.IsConvertible[U, T](): + if maybe: + return None + raise RuntimeError( + f"System {system_U.GetSystemPathname()} " + f"of type {type(system_U)} " + f"does not support scalar conversion to type {T}") + result = self._make(T=T, U=U, system_U=system_U) + result._HandlePostConstructionScalarConversion[U](system_U) + return result + _to_scalar_type.T = T + _to_scalar_type.U = U + _to_scalar_type.maybe = maybe + return _to_scalar_type diff --git a/bindings/pydrake/systems/test/general_test.py b/bindings/pydrake/systems/test/general_test.py index c0f8ad0d6c42..feee340a51d4 100644 --- a/bindings/pydrake/systems/test/general_test.py +++ b/bindings/pydrake/systems/test/general_test.py @@ -52,6 +52,7 @@ SystemVisitor, SystemBase, SystemOutput, SystemOutput_, + SystemScalarConverter, VectorBase, VectorBase_, TriggerType, VectorSystem, VectorSystem_, @@ -439,6 +440,8 @@ def test_scalar_type_conversion(self): float_system.get_input_port(0).FixValue(float_context, 1.) for T in [float, AutoDiffXd, Expression]: system = Adder_[T](1, 1) + self.assertIsInstance(system.get_system_scalar_converter(), + SystemScalarConverter) # N.B. Current scalar conversion does not permit conversion to and # from the same type. if T != float: diff --git a/bindings/pydrake/systems/test/scalar_conversion_test.py b/bindings/pydrake/systems/test/scalar_conversion_test.py index 71b08eab4c7e..431c31448425 100644 --- a/bindings/pydrake/systems/test/scalar_conversion_test.py +++ b/bindings/pydrake/systems/test/scalar_conversion_test.py @@ -37,6 +37,25 @@ def _construct_copy(self, other, converter=None): Example = Example_[None] +@mut.TemplateSystem.define("NonsymbolicExample_", T_list=(float, AutoDiffXd)) +def NonsymbolicExample_(T): + + class Impl(LeafSystem_[T]): + """Testing non-symbolic example.""" + + def _construct(self, value, converter=None): + LeafSystem_[T].__init__(self, converter=converter) + self.value = value + + def _construct_copy(self, other, converter=None): + Impl._construct(self, other.value, converter=converter) + + return Impl + + +NonsymbolicExample = NonsymbolicExample_[None] + + class TestScalarConversion(unittest.TestCase): def test_converter_attributes(self): conversion_scalars = ( @@ -207,6 +226,43 @@ def _construct_copy(self, other, converter=None): with self.assertRaises(AssertionError): mut.TemplateSystem.define("C", T_pairs=T_pairs_unsupported) + def test_nonsymbolic_example(self): + """Tests the NonsymbolicExample_ system.""" + # Test private properties (do NOT use these in your code!). + self.assertEqual(len(NonsymbolicExample_._T_list), 2) + self.assertEqual(len(NonsymbolicExample_._T_pairs), 2) + + # Test calls that we have available for scalar conversion. + for (T, U), use_maybe_variation in itertools.product( + SystemScalarConverter.SupportedConversionPairs, [False, True]): + if U is Expression: + continue + expected_is_convertible = T is not Expression + system_U = NonsymbolicExample_[U](2) + system_U._AddExternalConstraint(_ExternalSystemConstraint()) + if expected_is_convertible: + if use_maybe_variation: + system_T = system_U.ToScalarTypeMaybe[T]() + else: + system_T = system_U.ToScalarType[T]() + self.assertEqual(system_T.value, system_U.value) + if T is AutoDiffXd: + if use_maybe_variation: + system_ad = system_U.ToAutoDiffXdMaybe() + else: + system_ad = system_U.ToAutoDiffXd() + self.assertEqual(system_ad.value, system_U.value) + continue + # Carefully check when happens when NOT convertible. + if use_maybe_variation: + system_T = system_U.ToScalarTypeMaybe[T]() + self.assertIsNone(system_T) + continue + with self.assertRaisesRegex( + RuntimeError, + ".*NonsymbolicExample.*not support.*Expression"): + system_U.ToScalarType[T]() + def test_inheritance(self): @mut.TemplateSystem.define("Child_") diff --git a/systems/framework/BUILD.bazel b/systems/framework/BUILD.bazel index e2d0b102de61..d252b7256823 100644 --- a/systems/framework/BUILD.bazel +++ b/systems/framework/BUILD.bazel @@ -58,6 +58,7 @@ drake_cc_package_library( ":vector", ":vector_system", ":witness_function", + ":wrapped_system", ], ) @@ -609,7 +610,16 @@ drake_cc_library( drake_cc_library( name = "diagram", - srcs = ["diagram.cc"], + srcs = [ + "diagram.cc", + # The internal-use-only wrapped_system is a close friend of Diagram. + # It subclasses Diagram and Diagram's implementation uses it, so we + # must group that dependency cycle into a single library. However, note + # that one constructor is defined in wrapped_system_builder.cc as part + # of the actual :wrapped_system library. + "wrapped_system.cc", + "wrapped_system.h", + ], hdrs = ["diagram.h"], deps = [ ":diagram_context", @@ -617,11 +627,11 @@ drake_cc_library( ":system", "//common:default_scalars", "//common:essential", + "//common:string_container", ], implementation_deps = [ ":abstract_value_cloner", "//common:pointer_cast", - "//common:string_container", ], ) @@ -696,6 +706,22 @@ drake_cc_library( ], ) +drake_cc_library( + name = "wrapped_system", + srcs = [ + # This file just defines one constructor. Most functions are defined in + # wrapped_system.cc which is necessarily part of the :diagram library. + "wrapped_system_builder.cc", + ], + hdrs = ["wrapped_system.h"], + deps = [ + ":diagram", + ], + implementation_deps = [ + ":diagram_builder", + ], +) + # === test/ === drake_cc_googletest( @@ -1096,4 +1122,14 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "wrapped_system_test", + deps = [ + ":wrapped_system", + "//common/test_utilities", + "//systems/framework/test_utilities:scalar_conversion", + "//systems/primitives:adder", + ], +) + add_lint_tests(enable_clang_format_lint = False) diff --git a/systems/framework/diagram.cc b/systems/framework/diagram.cc index 45103bef490e..9bac56459cff 100644 --- a/systems/framework/diagram.cc +++ b/systems/framework/diagram.cc @@ -14,6 +14,7 @@ #include "drake/systems/framework/subvector.h" #include "drake/systems/framework/system_constraint.h" #include "drake/systems/framework/system_visitor.h" +#include "drake/systems/framework/wrapped_system.h" namespace drake { namespace systems { @@ -1380,11 +1381,31 @@ Diagram::ConvertScalarType() const { std::map*, const System*> old_to_new_map; for (const auto& old_system : registered_systems_) { // Convert old_system to new_system using the old_system's converter. - std::unique_ptr> new_system = + std::shared_ptr> new_system = old_system->get_system_scalar_converter(). template Convert(*old_system); DRAKE_DEMAND(new_system != nullptr); + // Special case to work around Python difficulties -- if the new_system is a + // leaf system implemented in Python, then undo the wrapping that helped it + // survive the SystemScalarConverter. Refer to the pydrake/bindings/systems + // helper function DefineSystemScalarConverter() to understand where the + // WrappedSystem was originally injected into this control flow. + if (auto* wrapped = + dynamic_cast*>(new_system.get()); + wrapped != nullptr) { + // Extract the inner system. + const std::string name = new_system->get_name(); + DRAKE_DEMAND(wrapped->registered_systems_.size() == 1); + std::shared_ptr> inner = wrapped->registered_systems_[0]; + // Discard the wrapper. + SystemBase::set_parent_service(inner.get(), nullptr); + new_system.reset(); + // Use the inner system when creating this Diagram. + new_system = inner; + new_system->set_name(name); + } + // Because we called the scalar converter directly without going through // System::ToScalarTypeMaybe, we need to re-implement its logic to copy any // external constraints. Note that we must call this function on Diagram for diff --git a/systems/framework/system_base.cc b/systems/framework/system_base.cc index 442a669e2e84..8cf1069a1bbe 100644 --- a/systems/framework/system_base.cc +++ b/systems/framework/system_base.cc @@ -430,8 +430,7 @@ void SystemBase::set_parent_service( SystemBase* child, const internal::SystemParentServiceInterface* parent_service) { DRAKE_DEMAND(child != nullptr); - DRAKE_DEMAND(parent_service != nullptr); - if (child->parent_service_ != nullptr) { + if (parent_service != nullptr && child->parent_service_ != nullptr) { throw std::logic_error(fmt::format( "Cannot build subsystem '{}' into Diagram '{}' because it has already " "been built into a different Diagram '{}'", diff --git a/systems/framework/system_base.h b/systems/framework/system_base.h index e154a75a0458..05ab3625d6aa 100644 --- a/systems/framework/system_base.h +++ b/systems/framework/system_base.h @@ -1099,7 +1099,7 @@ class SystemBase : public internal::SystemMessageInterface { /** (Internal use only) Declares that `parent_service` is the service interface of the Diagram that owns this subsystem. Throws if the parent - service has already been set. */ + service has already been set and `parent_service` is non-null. */ // Use static method so Diagram can invoke this on behalf of a child. // Output argument is listed first because it is serving as the 'this' // pointer here. diff --git a/systems/framework/test/diagram_test.cc b/systems/framework/test/diagram_test.cc index c3296b480429..75f573c86ca7 100644 --- a/systems/framework/test/diagram_test.cc +++ b/systems/framework/test/diagram_test.cc @@ -19,6 +19,7 @@ #include "drake/systems/framework/test_utilities/initialization_test_system.h" #include "drake/systems/framework/test_utilities/pack_value.h" #include "drake/systems/framework/test_utilities/scalar_conversion.h" +#include "drake/systems/framework/wrapped_system.h" #include "drake/systems/primitives/adder.h" #include "drake/systems/primitives/constant_value_source.h" #include "drake/systems/primitives/constant_vector_source.h" @@ -1235,6 +1236,29 @@ TEST_F(DiagramTest, Clone) { ExpectDefaultOutputs(); } +// Tests that the special handling of WrappedSystem is effective. +GTEST_TEST(PythonDiagramTest, Unwrap) { + // Directly create a diagram using a wrapped system. (This doesn't really + // match the control flow that Python actually uses, but it's the simplest + // way to inject a WrappedSystem into Diagram's scalar conversion logic.) + DiagramBuilder builder; + auto adder = std::make_shared>(1, 1); + adder->set_name("my_adder"); + builder.AddSystem>(std::move(adder)); + auto diagram_double = builder.Build(); + ASSERT_EQ(diagram_double->GetSystems().size(), 1); + + // Convert double -> AutoDiffXd. + auto diagram_autodiff = Diagram::ToAutoDiffXd(*diagram_double); + ASSERT_EQ(diagram_autodiff->GetSystems().size(), 1); + + // Check that the child is an Adder, not a WrappedSystem. + const System* child = diagram_autodiff->GetSystems().at(0); + ASSERT_THAT(child, testing::WhenDynamicCastTo*>( + testing::NotNull())); + EXPECT_EQ(child->get_name(), "my_adder"); +} + // Tests that, when asked for the state derivatives of Systems that are // stateless, Diagram returns an empty state. TEST_F(DiagramTest, DerivativesOfStatelessSystemAreEmpty) { diff --git a/systems/framework/test/wrapped_system_test.cc b/systems/framework/test/wrapped_system_test.cc new file mode 100644 index 000000000000..dc7035253956 --- /dev/null +++ b/systems/framework/test/wrapped_system_test.cc @@ -0,0 +1,50 @@ +#include "drake/systems/framework/wrapped_system.h" + +#include +#include + +#include "drake/common/unused.h" +#include "drake/systems/framework/test_utilities/scalar_conversion.h" +#include "drake/systems/primitives/adder.h" + +namespace drake { +namespace systems { +namespace internal { +namespace { + +using symbolic::Expression; + +GTEST_TEST(WrappedSystemTest, Basic) { + auto adder = std::make_shared>(2, 1); + adder->set_name("my_adder"); + WrappedSystem dut(std::move(adder)); + EXPECT_EQ(dut.get_name(), "my_adder"); + EXPECT_EQ(dut.num_input_ports(), 2); + EXPECT_EQ(dut.num_output_ports(), 1); + EXPECT_TRUE(dynamic_cast*>(&dut.unwrap())); +} + +GTEST_TEST(WrappedSystemTest, ConvertScalarType) { + auto adder = std::make_shared>(2, 1); + adder->set_name("my_adder"); + WrappedSystem dut(std::move(adder)); + EXPECT_TRUE(is_autodiffxd_convertible(dut, [&](const auto& converted) { + EXPECT_EQ(converted.get_name(), "my_adder"); + EXPECT_EQ(converted.num_input_ports(), 2); + EXPECT_EQ(converted.num_output_ports(), 1); + EXPECT_NO_THROW( + unused(dynamic_cast&>(converted.unwrap()))); + })); + EXPECT_TRUE(is_symbolic_convertible(dut, [&](const auto& converted) { + EXPECT_EQ(converted.get_name(), "my_adder"); + EXPECT_EQ(converted.num_input_ports(), 2); + EXPECT_EQ(converted.num_output_ports(), 1); + EXPECT_NO_THROW( + unused(dynamic_cast&>(converted.unwrap()))); + })); +} + +} // namespace +} // namespace internal +} // namespace systems +} // namespace drake diff --git a/systems/framework/wrapped_system.cc b/systems/framework/wrapped_system.cc new file mode 100644 index 000000000000..44cb51f6dffd --- /dev/null +++ b/systems/framework/wrapped_system.cc @@ -0,0 +1,25 @@ +#include "drake/systems/framework/wrapped_system.h" + +#include "drake/common/default_scalars.h" + +namespace drake { +namespace systems { +namespace internal { + +// N.B. The shared_ptr constructor is defined in wrapped_system_builder.cc. + +template +WrappedSystem::~WrappedSystem() = default; + +template +const System& WrappedSystem::unwrap() const { + auto diagram_subsystems = this->GetSystems(); + return *diagram_subsystems.at(0); +} + +} // namespace internal +} // namespace systems +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::drake::systems::internal::WrappedSystem); diff --git a/systems/framework/wrapped_system.h b/systems/framework/wrapped_system.h new file mode 100644 index 000000000000..56da934e0281 --- /dev/null +++ b/systems/framework/wrapped_system.h @@ -0,0 +1,52 @@ +#pragma once + +#include + +#include "drake/systems/framework/diagram.h" + +namespace drake { +namespace systems { +namespace internal { + +/* WrappedSystem implements the System interface using a pre-existing +System object held internally as a shared_ptr (i.e., the "decorator" design +pattern). All calls are forwarded to the nested system. + +Currently, this is only used to facilitate the scalar conversion for leaf +systems implemented in Python. + +For the implementation, the easiest way to forward all System calls along to +another System is actually through the composite pattern (aka part-whole) that +we've already implemented as Diagram. The WrappedSystem is-a Diagram that has +exactly one subsystem. + +@tparam_default_scalar */ +template +class WrappedSystem final : public Diagram { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(WrappedSystem); + + /* Wraps the given system, which must not be nullptr. Even though ownership is + shared, the given `system` must NOT be mutated in any way (e.g., added to a + different Diagram). + @tparam Ignored is ignored; it's only job is to allow us to define this + constructor in wrapped_system_builder.cc instead of wrapped_system.cc. */ + template + explicit WrappedSystem(std::shared_ptr> system); + + /* Scalar-converting copy constructor. See @ref system_scalar_conversion. + Ideally this would not be inline, but with our weird linking situation it's + vastly simpler to keep it here. */ + template + explicit WrappedSystem(const WrappedSystem& other) + : Diagram(systems::SystemTypeTag{}, other) {} + + ~WrappedSystem() final; + + /* Returns the underlying System. */ + const System& unwrap() const; +}; + +} // namespace internal +} // namespace systems +} // namespace drake diff --git a/systems/framework/wrapped_system_builder.cc b/systems/framework/wrapped_system_builder.cc new file mode 100644 index 000000000000..403732b460f2 --- /dev/null +++ b/systems/framework/wrapped_system_builder.cc @@ -0,0 +1,45 @@ +/* clang-format off to disable clang-format-includes */ +#include "drake/systems/framework/wrapped_system.h" +/* clang-format on */ + +#include "drake/common/default_scalars.h" +#include "drake/systems/framework/diagram_builder.h" + +namespace drake { +namespace systems { +namespace internal { + +// This constructor for wrapped_system.h must be defined in a separate file, +// because it uses DiagramBuilder. +template +template +WrappedSystem::WrappedSystem(std::shared_ptr> system) + : Diagram(SystemTypeTag{}) { + DRAKE_THROW_UNLESS(system != nullptr); + this->set_name(system->get_name()); + system->set_name("wrapped"); + DiagramBuilder builder; + const System* alias = builder.AddSystem(std::move(system)); + for (InputPortIndex i{0}; i < alias->num_input_ports(); ++i) { + const InputPort& port = + alias->get_input_port(i, /* warn_deprecated = */ false); + builder.ExportInput(port); + } + for (OutputPortIndex i{0}; i < alias->num_output_ports(); ++i) { + const OutputPort& port = + alias->get_output_port(i, /* warn_deprecated = */ false); + builder.ExportOutput(port); + } + builder.BuildInto(this); +} + +template WrappedSystem::WrappedSystem( + std::shared_ptr>); +template WrappedSystem::WrappedSystem( + std::shared_ptr>); +template WrappedSystem::WrappedSystem( + std::shared_ptr>); + +} // namespace internal +} // namespace systems +} // namespace drake