diff --git a/bindings/pydrake/systems/framework_py_semantics.cc b/bindings/pydrake/systems/framework_py_semantics.cc index f495999d0921..eab687addc79 100644 --- a/bindings/pydrake/systems/framework_py_semantics.cc +++ b/bindings/pydrake/systems/framework_py_semantics.cc @@ -239,7 +239,8 @@ void DefineFrameworkPySemantics(py::module m) { // `AddValueInstantiation` for more information. // Keep alive, ownership: `value` keeps `self` alive. py::keep_alive<2, 1>(), py::arg("abstract_params")) - .def("SetFrom", &Parameters::SetFrom); + .def("SetFrom", &Parameters::SetFrom) + .def("CopyFrom", &Parameters::CopyFrom); // State. DefineTemplateClassWithDefault>(m, "State", GetPyParam()) diff --git a/bindings/pydrake/systems/framework_py_values.cc b/bindings/pydrake/systems/framework_py_values.cc index 28ef07b8f4aa..c3dbd560b112 100644 --- a/bindings/pydrake/systems/framework_py_values.cc +++ b/bindings/pydrake/systems/framework_py_values.cc @@ -8,6 +8,7 @@ #include "drake/bindings/pydrake/pydrake_pybind.h" #include "drake/bindings/pydrake/systems/systems_pybind.h" #include "drake/bindings/pydrake/util/eigen_pybind.h" +#include "drake/bindings/pydrake/util/wrap_pybind.h" #include "drake/systems/framework/basic_vector.h" #include "drake/systems/framework/subvector.h" #include "drake/systems/framework/supervector.h" @@ -43,18 +44,17 @@ void DefineFrameworkPyValues(py::module m) { // N.B. Place `init>` `init` so that we do not implicitly // convert scalar-size `np.array` objects to `int` (since this is normally // permitted). - .def(py::init>()) + // N.B. Also ensure that we use `greedy_arg` to prevent ambiguous + // overloads when using scalars vs. lists vs. numpy arrays. See + // `greedy_arg` for more information. + .def(py::init([](greedy_arg> in) { + return new BasicVector(*in); + })) .def(py::init()) .def("get_value", [](const BasicVector* self) -> Eigen::Ref> { return self->get_value(); }, py_reference_internal) - // TODO(eric.cousineau): Remove this once `get_value` is changed, or - // reference semantics are changed for custom dtypes. - .def("_get_value_copy", - [](const BasicVector* self) -> VectorX { - return self->get_value(); - }) .def("get_mutable_value", [](BasicVector* self) -> Eigen::Ref> { return self->get_mutable_value(); diff --git a/bindings/pydrake/systems/test/test_util_py.cc b/bindings/pydrake/systems/test/test_util_py.cc index bd20f8414c8a..d00cf9c1c931 100644 --- a/bindings/pydrake/systems/test/test_util_py.cc +++ b/bindings/pydrake/systems/test/test_util_py.cc @@ -18,13 +18,14 @@ using systems::ConstantVectorSource; namespace pydrake { namespace { -using T = double; - // Informs listener when this class is deleted. +template class DeleteListenerSystem : public ConstantVectorSource { public: + using Base = ConstantVectorSource; + explicit DeleteListenerSystem(std::function delete_callback) - : ConstantVectorSource(VectorX::Constant(1, 0.)), + : Base(VectorX::Constant(1, 0.)), delete_callback_(delete_callback) {} ~DeleteListenerSystem() override { @@ -34,10 +35,11 @@ class DeleteListenerSystem : public ConstantVectorSource { std::function delete_callback_; }; +template class DeleteListenerVector : public BasicVector { public: explicit DeleteListenerVector(std::function delete_callback) - : BasicVector(VectorX::Constant(1, 0.)), + : BasicVector(VectorX::Constant(1, 0.)), delete_callback_(delete_callback) {} ~DeleteListenerVector() override { @@ -89,10 +91,10 @@ PYBIND11_MODULE(test_util, m) { py::module::import("pydrake.systems.framework"); py::module::import("pydrake.systems.primitives"); - py::class_>( + py::class_, ConstantVectorSource>( m, "DeleteListenerSystem") .def(py::init>()); - py::class_>( + py::class_, BasicVector>( m, "DeleteListenerVector") .def(py::init>()); @@ -104,7 +106,7 @@ PYBIND11_MODULE(test_util, m) { pysystems::AddValueInstantiation(m); // A 2-dimensional subclass of BasicVector. - py::class_, BasicVector>(m, "MyVector2") + py::class_, BasicVector>(m, "MyVector2") .def(py::init(), py::arg("data")); m.def("make_unknown_abstract_value", []() { @@ -112,72 +114,77 @@ PYBIND11_MODULE(test_util, m) { }); // Call overrides to ensure a custom Python class can override these methods. - - auto clone_vector = [](const VectorBase& vector) { - auto copy = std::make_unique>(vector.size()); - copy->SetFrom(vector); - return copy; + auto bind_common_scalar_types = [&m](auto dummy) { + using T = decltype(dummy); + + auto clone_vector = [](const VectorBase& vector) { + auto copy = std::make_unique>(vector.size()); + copy->SetFrom(vector); + return copy; + }; + + m.def("call_leaf_system_overrides", [clone_vector]( + const LeafSystem& system) { + py::dict results; + auto context = system.AllocateContext(); + { + // Call `Publish` to test `DoPublish`. + auto events = + LeafEventCollection>::MakeForcedEventCollection(); + system.Publish(*context, *events); + } + { + // Call `HasDirectFeedthrough` to test `DoHasDirectFeedthrough`. + results["has_direct_feedthrough"] = system.HasDirectFeedthrough(0, 0); + } + { + // Call `CalcDiscreteVariableUpdates` to test + // `DoCalcDiscreteVariableUpdates`. + auto& state = context->get_mutable_discrete_state(); + DiscreteValues state_copy(clone_vector(state.get_vector())); + system.CalcDiscreteVariableUpdates(*context, &state_copy); + + // From t=0, return next update time for testing discrete time. + // If there is an abstract / unrestricted update, this assumes that + // `dt_discrete < dt_abstract`. + systems::LeafCompositeEventCollection events; + results["discrete_next_t"] = system.CalcNextUpdateTime( + *context, &events); + } + return results; + }); + + m.def("call_vector_system_overrides", [clone_vector]( + const VectorSystem& system, Context* context, + bool is_discrete, double dt) { + // While this is not convention, update state first to ensure that our + // output incorporates it correctly, for testing purposes. + // TODO(eric.cousineau): Add (Continuous|Discrete)State::Clone(). + if (is_discrete) { + auto& state = context->get_mutable_discrete_state(); + DiscreteValues state_copy( + clone_vector(state.get_vector())); + system.CalcDiscreteVariableUpdates( + *context, &state_copy); + state.CopyFrom(state_copy); + } else { + auto& state = context->get_mutable_continuous_state(); + ContinuousState state_dot( + clone_vector(state.get_vector()), + state.get_generalized_position().size(), + state.get_generalized_velocity().size(), + state.get_misc_continuous_state().size()); + system.CalcTimeDerivatives(*context, &state_dot); + state.SetFromVector( + state.CopyToVector() + dt * state_dot.CopyToVector()); + } + // Calculate output. + auto output = system.AllocateOutput(*context); + system.CalcOutput(*context, output.get()); + return output; + }); }; - - m.def("call_leaf_system_overrides", [clone_vector]( - const LeafSystem& system) { - py::dict results; - auto context = system.AllocateContext(); - { - // Call `Publish` to test `DoPublish`. - auto events = - LeafEventCollection>::MakeForcedEventCollection(); - system.Publish(*context, *events); - } - { - // Call `HasDirectFeedthrough` to test `DoHasDirectFeedthrough`. - results["has_direct_feedthrough"] = system.HasDirectFeedthrough(0, 0); - } - { - // Call `CalcDiscreteVariableUpdates` to test - // `DoCalcDiscreteVariableUpdates`. - auto& state = context->get_mutable_discrete_state(); - DiscreteValues state_copy(clone_vector(state.get_vector())); - system.CalcDiscreteVariableUpdates(*context, &state_copy); - - // From t=0, return next update time for testing discrete time. - // If there is an abstract / unrestricted update, this assumes that - // `dt_discrete < dt_abstract`. - systems::LeafCompositeEventCollection events; - results["discrete_next_t"] = system.CalcNextUpdateTime(*context, &events); - } - return results; - }); - - m.def("call_vector_system_overrides", [clone_vector]( - const VectorSystem& system, Context* context, - bool is_discrete, double dt) { - // While this is not convention, update state first to ensure that our - // output incorporates it correctly, for testing purposes. - // TODO(eric.cousineau): Add (Continuous|Discrete)State::Clone(). - if (is_discrete) { - auto& state = context->get_mutable_discrete_state(); - DiscreteValues state_copy( - clone_vector(state.get_vector())); - system.CalcDiscreteVariableUpdates( - *context, &state_copy); - state.SetFrom(state_copy); - } else { - auto& state = context->get_mutable_continuous_state(); - ContinuousState state_dot( - clone_vector(state.get_vector()), - state.get_generalized_position().size(), - state.get_generalized_velocity().size(), - state.get_misc_continuous_state().size()); - system.CalcTimeDerivatives(*context, &state_dot); - state.SetFromVector( - state.CopyToVector() + dt * state_dot.CopyToVector()); - } - // Calculate output. - auto output = system.AllocateOutput(*context); - system.CalcOutput(*context, output.get()); - return output; - }); + type_visit(bind_common_scalar_types, pysystems::CommonScalarPack{}); } } // namespace pydrake diff --git a/bindings/pydrake/systems/test/value_test.py b/bindings/pydrake/systems/test/value_test.py index 3a4da6b29702..36e581d192e9 100644 --- a/bindings/pydrake/systems/test/value_test.py +++ b/bindings/pydrake/systems/test/value_test.py @@ -6,10 +6,12 @@ import unittest import numpy as np +from pydrake.autodiffutils import AutoDiffXd +from pydrake.symbolic import Expression from pydrake.systems.framework import ( AbstractValue, - BasicVector, - Parameters, + BasicVector, BasicVector_, + Parameters, Parameters_, Value, VectorBase, ) @@ -23,45 +25,53 @@ def pass_through(x): return x -# TODO(eric.cousineau): Add negative (or positive) test cases for AutoDiffXd -# and Symbolic once they are in the bindings. +def int_list(x): + return [int(xi) for xi in x] class TestValue(unittest.TestCase): - def test_basic_vector_double(self): + def assertArrayEqual(self, lhs, rhs): + # TODO(eric.cousineau): Place in `pydrake.test.unittest_mixins`. + lhs, rhs = np.array(lhs), np.array(rhs) + if lhs.dtype == Expression or rhs.dtype == Expression: + lhs, rhs = lhs.astype(Expression), rhs.astype(Expression) + self.assertTrue(Expression.equal_to(lhs, rhs).all()) + else: + self.assertTrue(np.allclose(lhs, rhs)) + + def test_basic_vector(self): + map(self._check_basic_vector, (float, AutoDiffXd, Expression)) + + def _check_basic_vector(self, T): # Test constructing vectors of sizes [0, 1, 2], and ensure that we can # construct from both lists and `np.array` objects with no ambiguity. for n in [0, 1, 2]: - for wrap in [pass_through, np.array]: + for wrap in [pass_through, int_list, np.array]: # Ensure that we can get vectors templated on double by # reference. expected_init = wrap(map(float, range(n))) expected_add = wrap([x + 1 for x in expected_init]) expected_set = wrap([x + 10 for x in expected_init]) - value_data = BasicVector(expected_init) + value_data = BasicVector_[T](expected_init) value = value_data.get_mutable_value() - self.assertTrue(np.allclose(value, expected_init)) + self.assertArrayEqual(value, expected_init) # Add value directly. - # TODO(eric.cousineau): Determine if there is a way to extract - # the pointer referred to by the buffer (e.g. `value.data`). value[:] += 1 - self.assertTrue(np.allclose(value, expected_add)) - self.assertTrue( - np.allclose(value_data.get_value(), expected_add)) - self.assertTrue( - np.allclose(value_data.get_mutable_value(), expected_add)) + self.assertArrayEqual(value, expected_add) + self.assertArrayEqual(value_data.get_value(), expected_add) + self.assertArrayEqual( + value_data.get_mutable_value(), expected_add) # Set value from `BasicVector`. value_data.SetFromVector(expected_set) - self.assertTrue(np.allclose(value, expected_set)) - self.assertTrue( - np.allclose(value_data.get_value(), expected_set)) - self.assertTrue( - np.allclose(value_data.get_mutable_value(), expected_set)) + self.assertArrayEqual(value, expected_set) + self.assertArrayEqual(value_data.get_value(), expected_set) + self.assertArrayEqual( + value_data.get_mutable_value(), expected_set) # Ensure we can construct from size. - value_data = BasicVector(n) + value_data = BasicVector_[T](n) self.assertEquals(value_data.size(), n) # Ensure we can clone. value_copies = [ @@ -142,13 +152,20 @@ def test_abstract_value_unknown(self): ]), cm.exception.message) def test_parameters_api(self): + map(self._check_parameters_api, (float, AutoDiffXd, Expression)) + + def _check_parameters_api(self, T): + Parameters = Parameters_[T] + BasicVector = BasicVector_[T] def compare(actual, expected): self.assertEquals(type(actual), type(expected)) - if isinstance(actual, VectorBase): - self.assertTrue( - np.allclose(actual.get_value(), expected.get_value())) + if isinstance(actual, BasicVector): + self.assertArrayEqual(actual.get_value(), expected.get_value()) else: + assert isinstance(actual, Value[str]) + # Strings getting converted to numpy arrays is no bueno. Do + # scalar comparison. self.assertEquals(actual.get_value(), expected.get_value()) model_numeric = BasicVector([0.]) @@ -170,7 +187,7 @@ def compare(actual, expected): params.set_abstract_parameters( params.get_abstract_parameters().Clone()) # WARNING: This may invalidate old references! - params.SetFrom(copy.deepcopy(params)) + params.CopyFrom(copy.deepcopy(params)) # Test alternative constructors. ctor_test = [ diff --git a/bindings/pydrake/util/wrap_pybind.h b/bindings/pydrake/util/wrap_pybind.h index fd550d89e1cf..d4d2da8527df 100644 --- a/bindings/pydrake/util/wrap_pybind.h +++ b/bindings/pydrake/util/wrap_pybind.h @@ -65,6 +65,10 @@ template struct wrap_callback> : public wrap_callback&> {}; +struct greedy_arg_no_check { + static bool run(py::handle) { return true; } +}; + } // namespace detail /// Ensures that any `std::function<>` arguments are wrapped such that any `T&` @@ -107,5 +111,44 @@ class UfuncMirrorDef { py::module math_; }; +/// Provides a mechanism to have an argument immediately attempt conversion, +/// even during the "no converion" pass during function dispatch, with an +/// additional check to prevent it from being *too* greedy. +/// This should be used if you are overloading a function with both an array +/// *and* scalar, as NumPy arrays can sometimes be interperted as scalars, and +/// due to possible conversions, the wrong overload may be selected. +/// For more information, see https://github.com/pybind/pybind11/issues/1392 +/// This solution comes from the posted workaround. +template +class greedy_arg { + public: + // NOLINTNEXTLINE[runtime/explicit]: This is desirable. + greedy_arg(T&& value) : value_(std::move(value)) {} + // TODO(eric.cousineau): Figure out how to handle referencing properly. + T&& operator*() { + return std::move(value_); + } + private: + T value_; +}; + } // namespace pydrake } // namespace drake + +namespace pybind11 { +namespace detail { + +template +struct type_caster> : type_caster { + using base = type_caster; + bool load(handle src, bool /*convert*/) { + if (!Check::run(src)) + return false; + return base::load(src, true); + } + template + using cast_op_type = T&&; +}; + +} // namespace detail +} // namespace pybind11 diff --git a/common/drake_assert_and_throw.cc b/common/drake_assert_and_throw.cc index ea011daabad3..a159d9a600ee 100644 --- a/common/drake_assert_and_throw.cc +++ b/common/drake_assert_and_throw.cc @@ -46,6 +46,7 @@ void Abort(const char* condition, const char* func, const char* file, std::cerr << "abort: "; PrintFailureDetailTo(std::cerr, condition, func, file, line); std::cerr << std::endl; + // TODO(eric.cousineau): Consider Throw'ing here for Python. std::abort(); } diff --git a/systems/framework/parameters.h b/systems/framework/parameters.h index 40bb7381a62e..ca0d65865e4b 100644 --- a/systems/framework/parameters.h +++ b/systems/framework/parameters.h @@ -137,6 +137,13 @@ class Parameters { return clone; } + /// Initializes the values from another Parameters of the same scalar type + /// into this instance. + void CopyFrom(const Parameters& other) { + numeric_parameters_->CopyFrom(other.get_numeric_parameters()); + abstract_parameters_->CopyFrom(other.get_abstract_parameters()); + } + /// Initializes this state (regardless of scalar type) from a /// Parameters. All scalar types in Drake must support /// initialization from doubles.