From b873ee2687034c56c7b5d91c86357c64dc83b7e9 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Thu, 3 Sep 2020 11:40:28 -0700 Subject: [PATCH] pydrake: Add bindings for common.schema This only binds the variable-sized stochastic vector types. The fixed-size stochastic vector type are not yet available. --- bindings/pydrake/common/BUILD.bazel | 22 ++ bindings/pydrake/common/schema_py.cc | 229 ++++++++++++++++++++ bindings/pydrake/common/test/schema_test.py | 121 +++++++++++ common/schema/transform.cc | 6 +- common/schema/transform.h | 2 +- 5 files changed, 376 insertions(+), 4 deletions(-) create mode 100644 bindings/pydrake/common/schema_py.cc create mode 100644 bindings/pydrake/common/test/schema_test.py diff --git a/bindings/pydrake/common/BUILD.bazel b/bindings/pydrake/common/BUILD.bazel index d82666ebf091..06a1bc870093 100644 --- a/bindings/pydrake/common/BUILD.bazel +++ b/bindings/pydrake/common/BUILD.bazel @@ -322,6 +322,20 @@ drake_py_library( ], ) +drake_pybind_library( + name = "schema_py", + cc_deps = [ + "//bindings/pydrake:documentation_pybind", + "//bindings/pydrake:symbolic_types_pybind", + ], + cc_srcs = ["schema_py.cc"], + package_info = PACKAGE_INFO, + py_deps = [ + ":module_py", + "//bindings/pydrake:math_py", + ], +) + drake_py_library( name = "yaml_py", srcs = ["yaml.py"], @@ -333,6 +347,7 @@ drake_py_library( PY_LIBRARIES_WITH_INSTALL = [ ":_init_py", ":eigen_geometry_py", + ":schema_py", ":value_py", ] @@ -598,6 +613,13 @@ drake_py_unittest( ], ) +drake_py_unittest( + name = "schema_test", + deps = [ + ":schema_py", + ], +) + drake_py_unittest( name = "yaml_test", deps = [ diff --git a/bindings/pydrake/common/schema_py.cc b/bindings/pydrake/common/schema_py.cc new file mode 100644 index 000000000000..a42962ba364f --- /dev/null +++ b/bindings/pydrake/common/schema_py.cc @@ -0,0 +1,229 @@ +#include +#include + +#include "pybind11/eigen.h" +#include "pybind11/pybind11.h" +#include "pybind11/stl.h" + +#include "drake/bindings/pydrake/documentation_pybind.h" +#include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/bindings/pydrake/symbolic_types_pybind.h" +#include "drake/common/schema/rotation.h" +#include "drake/common/schema/stochastic.h" +#include "drake/common/schema/transform.h" + +namespace drake { +namespace pydrake { + +PYBIND11_MODULE(schema, m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::schema; + constexpr auto& doc = pydrake_doc.drake.schema; + + m.doc() = "Bindings for the common.schema package."; + + py::module::import("pydrake.common"); + py::module::import("pydrake.math"); + + // Bindings for stochastic.h. + + { + using Class = Distribution; + constexpr auto& cls_doc = doc.Distribution; + py::class_(m, "Distribution", cls_doc.doc) + .def("Sample", &Class::Sample, py::arg("generator"), cls_doc.Sample.doc) + .def("Mean", &Class::Mean, cls_doc.Mean.doc) + .def("ToSymbolic", &Class::ToSymbolic, cls_doc.ToSymbolic.doc); + } + + { + using Class = Deterministic; + constexpr auto& cls_doc = doc.Deterministic; + py::class_(m, "Deterministic", cls_doc.doc) + .def(py::init<>(), cls_doc.ctor.doc) + .def(py::init(), py::arg("value"), cls_doc.ctor.doc) + .def_readwrite("value", &Class::value, cls_doc.value.doc); + } + + { + using Class = Gaussian; + constexpr auto& cls_doc = doc.Gaussian; + py::class_(m, "Gaussian", cls_doc.doc) + .def(py::init<>(), cls_doc.ctor.doc) + .def(py::init(), py::arg("mean"), py::arg("stddev"), + cls_doc.ctor.doc) + .def_readwrite("mean", &Class::mean, cls_doc.mean.doc) + .def_readwrite("stddev", &Class::stddev, cls_doc.stddev.doc); + } + + { + using Class = Uniform; + constexpr auto& cls_doc = doc.Uniform; + py::class_(m, "Uniform", cls_doc.doc) + .def(py::init<>(), cls_doc.ctor.doc) + .def(py::init(), py::arg("min"), py::arg("max"), + cls_doc.ctor.doc) + .def_readwrite("min", &Class::min, cls_doc.min.doc) + .def_readwrite("max", &Class::max, cls_doc.max.doc); + } + + { + using Class = UniformDiscrete; + constexpr auto& cls_doc = doc.UniformDiscrete; + py::class_(m, "UniformDiscrete", cls_doc.doc) + .def(py::init<>(), cls_doc.ctor.doc) + .def(py::init>(), py::arg("values"), + cls_doc.ctor.doc) + .def_readwrite("values", &Class::values, cls_doc.values.doc); + } + + { + m // BR + .def("ToDistribution", + pydrake::overload_cast_explicit, + const DistributionVariant&>(ToDistribution), + py::arg("var"), doc.ToDistribution.doc) + .def("Sample", + pydrake::overload_cast_explicit(Sample), + py::arg("var"), py::arg("generator"), + doc.Sample.doc_2args_var_generator) + .def("Mean", + pydrake::overload_cast_explicit( + Mean), + py::arg("var"), doc.Mean.doc_1args_var) + .def("ToSymbolic", + pydrake::overload_cast_explicit(ToSymbolic), + py::arg("var"), doc.ToSymbolic.doc_1args_var) + .def("IsDeterministic", + pydrake::overload_cast_explicit( + IsDeterministic), + py::arg("var"), doc.IsDeterministic.doc_1args_var) + .def("GetDeterministicValue", + pydrake::overload_cast_explicit( + GetDeterministicValue), + py::arg("var"), doc.GetDeterministicValue.doc_1args_var); + } + + // TODO(jwnimmer-tri) In the below, we hard-code the template argument `int + // Size` to be `Eigen::Dynamic`, i.e., we only bind one of the allowed + // values. In the future, we should also bind Size == 1 through Size == 6 + // (to match the C++ code). When we do that, we'll replace the suffix X with + // the template pattern `_[Size]`, e.g., `DeterministicVector[3]`. + + { + using Class = DistributionVector; + constexpr auto& cls_doc = doc.DistributionVector; + py::class_(m, "DistributionVector", cls_doc.doc) + .def("Sample", &Class::Sample, py::arg("generator"), cls_doc.Sample.doc) + .def("Mean", &Class::Mean, cls_doc.Mean.doc) + .def("ToSymbolic", &Class::ToSymbolic, cls_doc.ToSymbolic.doc); + } + + { + constexpr int size = Eigen::Dynamic; + using Class = DeterministicVector; + constexpr auto& cls_doc = doc.DeterministicVector; + py::class_( + m, "DeterministicVectorX", cls_doc.doc) + .def(py::init<>(), cls_doc.ctor.doc) + .def(py::init&>(), py::arg("value"), + cls_doc.ctor.doc) + .def_readwrite("value", &Class::value, cls_doc.value.doc); + } + + { + constexpr int size = Eigen::Dynamic; + using Class = GaussianVector; + constexpr auto& cls_doc = doc.GaussianVector; + py::class_(m, "GaussianVectorX", cls_doc.doc) + .def(py::init<>(), cls_doc.ctor.doc) + .def(py::init&, + const drake::VectorX&>(), + py::arg("mean"), py::arg("stddev"), cls_doc.ctor.doc) + .def_readwrite("mean", &Class::mean, cls_doc.mean.doc) + .def_readwrite("stddev", &Class::stddev, cls_doc.stddev.doc); + } + + { + constexpr int size = Eigen::Dynamic; + using Class = UniformVector; + constexpr auto& cls_doc = doc.UniformVector; + py::class_(m, "UniformVectorX", cls_doc.doc) + .def(py::init<>(), cls_doc.ctor.doc) + .def(py::init&, + const drake::Vector&>(), + py::arg("min"), py::arg("max"), cls_doc.ctor.doc) + .def_readwrite("min", &Class::min, cls_doc.min.doc) + .def_readwrite("max", &Class::max, cls_doc.max.doc); + } + + { + constexpr int size = Eigen::Dynamic; + m // BR + .def("ToDistributionVector", + pydrake::overload_cast_explicit, + const DistributionVectorVariant&>(ToDistributionVector), + py::arg("vec"), doc.ToDistributionVector.doc) + .def("IsDeterministic", + pydrake::overload_cast_explicit&>(IsDeterministic), + py::arg("vec"), + doc.IsDeterministic.doc_1args_constDistributionVectorVariant) + .def("GetDeterministicValue", + pydrake::overload_cast_explicit&>(GetDeterministicValue), + py::arg("vec"), + doc.GetDeterministicValue.doc_1args_constDistributionVectorVariant); + } + + // Bindings for rotation.h. + + { + using Class = Rotation; + constexpr auto& cls_doc = doc.Rotation; + py::class_ cls(m, "Rotation", cls_doc.doc); + cls // BR + .def(py::init<>(), cls_doc.ctor.doc_0args) + .def(py::init(), cls_doc.ctor.doc_1args) + .def(py::init(), cls_doc.ctor.doc_1args) + .def("IsDeterministic", &Class::IsDeterministic, + cls_doc.IsDeterministic.doc) + .def("GetDeterministicValue", &Class::GetDeterministicValue, + cls_doc.GetDeterministicValue.doc) + .def("ToSymbolic", &Class::ToSymbolic, cls_doc.ToSymbolic.doc) + .def("set_rpy_deg", &Class::set_rpy_deg, py::arg("rpy_deg"), + cls_doc.set_rpy_deg.doc); + // TODO(jwnimmer-tri) Bind the .value field. + // In the meantime, a work-around for read access is to call ToSymbolic or + // GetDeterministicValue. + } + + // Bindings for transform.h. + + { + using Class = Transform; + constexpr auto& cls_doc = doc.Transform; + py::class_ cls(m, "Transform", cls_doc.doc); + cls // BR + .def(py::init<>(), cls_doc.ctor.doc_0args) + .def(py::init(), cls_doc.ctor.doc_1args) + .def("set_rotation_rpy_deg", &Class::set_rotation_rpy_deg, + py::arg("rpy_deg"), cls_doc.set_rotation_rpy_deg.doc) + .def("IsDeterministic", &Class::IsDeterministic, + cls_doc.IsDeterministic.doc) + .def("GetDeterministicValue", &Class::GetDeterministicValue, + cls_doc.GetDeterministicValue.doc) + .def("ToSymbolic", &Class::ToSymbolic, cls_doc.ToSymbolic.doc) + .def("Sample", &Class::Sample, py::arg("generator"), cls_doc.Sample.doc) + .def_readwrite( + "base_frame", &Class::base_frame, cls_doc.base_frame.doc); + // TODO(jwnimmer-tri) Bind the .translation and .rotation fields. + // In the meantime, a work-around for read access is to call ToSymbolic or + // GetDeterministicValue. + } +} + +} // namespace pydrake +} // namespace drake diff --git a/bindings/pydrake/common/test/schema_test.py b/bindings/pydrake/common/test/schema_test.py new file mode 100644 index 000000000000..9861643e6507 --- /dev/null +++ b/bindings/pydrake/common/test/schema_test.py @@ -0,0 +1,121 @@ +import unittest + +from pydrake.common import RandomGenerator +import pydrake.common.schema as mut +import pydrake.math + + +class TestSchema(unittest.TestCase): + + def _check_distribution(self, dut): + """Confirms that subclasses of Distribution bind the base methods.""" + self.assertIsInstance(dut, mut.Distribution) + dut.Sample(generator=RandomGenerator()) + dut.Mean() + dut.ToSymbolic() + + def test_deterministic(self): + mut.Deterministic() + dut = mut.Deterministic(1.0) + dut.value = 2.0 + self._check_distribution(dut) + + def test_gaussian(self): + mut.Gaussian() + dut = mut.Gaussian(mean=1.0, stddev=0.1) + dut.mean = 2.0 + dut.stddev = 0.2 + self._check_distribution(dut) + + def test_uniform(self): + mut.Uniform() + dut = mut.Uniform(min=-1.0, max=1.0) + dut.min = -2.0 + dut.max = 2.0 + self._check_distribution(dut) + + def test_uniform_discrete(self): + mut.UniformDiscrete() + dut = mut.UniformDiscrete(values=[0.0, 0.1]) + dut.values = [0.0, 0.2] + self._check_distribution(dut) + + def test_distribution_variant(self): + """Confirms that the free functions that operate on a variant are + bound.""" + items = [ + mut.Deterministic(1.0), + mut.Gaussian(1.0, 0.1), + mut.Uniform(-1.0, 1.0), + mut.UniformDiscrete([0.0, 1.0]), + ] + for item in items: + copied = mut.ToDistribution(item) + self._check_distribution(copied) + mut.Sample(var=item, generator=RandomGenerator()) + mut.Mean(var=item) + mut.ToSymbolic(var=item) + if mut.IsDeterministic(var=item): + mut.GetDeterministicValue(var=item) + + def _check_distribution_vector(self, dut): + """Confirms that subclasses of DistributionVector bind the base + methods. + """ + self.assertIsInstance(dut, mut.DistributionVector) + dut.Sample(generator=RandomGenerator()) + dut.Mean() + dut.ToSymbolic() + + def test_deterministic_vector(self): + mut.DeterministicVectorX() + dut = mut.DeterministicVectorX(value=[0.0, 1.0]) + dut.value = [0.0, 2.0] + self._check_distribution_vector(dut) + + def test_gaussian_vector(self): + mut.GaussianVectorX() + dut = mut.GaussianVectorX(mean=[-1.0, 1.0], stddev=[0.1, 0.1]) + dut.mean = [-2.0, 2.0] + dut.stddev = [0.2, 0.2] + self._check_distribution_vector(dut) + + def test_uniform_vector(self): + mut.UniformVectorX() + dut = mut.UniformVectorX(min=[-1.0, -10.0], max=[1.0, 10.0]) + dut.min = [-2.0, -20.0] + dut.max = [2.0, 20.0] + self._check_distribution_vector(dut) + + def test_distribution_vector_variant(self): + """Confirms that the free functions that operate on a vector variant + are bound.""" + items = [ + mut.DeterministicVectorX(value=[1.0]), + mut.GaussianVectorX(mean=[1.0], stddev=[0.1]), + mut.UniformVectorX(min=[-1.0], max=[1.0]), + ] + for item in items: + copied = mut.ToDistributionVector(item) + self._check_distribution_vector(copied) + if mut.IsDeterministic(vec=item): + mut.GetDeterministicValue(vec=item) + + def test_rotation(self): + mut.Rotation() + mut.Rotation(pydrake.math.RotationMatrix()) + dut = mut.Rotation(pydrake.math.RollPitchYaw([0.0, 0.0, 0.0])) + dut.IsDeterministic() + dut.GetDeterministicValue() + dut.ToSymbolic() + dut.set_rpy_deg([0.1, 0.2, 0.3]) + + def test_transform(self): + mut.Transform() + dut = mut.Transform(pydrake.math.RigidTransform()) + dut.set_rotation_rpy_deg([0.1, 0.2, 0.3]) + dut.IsDeterministic() + dut.GetDeterministicValue() + dut.ToSymbolic() + dut.Sample(generator=RandomGenerator()) + dut.base_frame = "name" diff --git a/common/schema/transform.cc b/common/schema/transform.cc index 7306139b13df..80415940eff4 100644 --- a/common/schema/transform.cc +++ b/common/schema/transform.cc @@ -60,7 +60,7 @@ math::RigidTransformd Transform::Mean() const { } math::RigidTransformd Transform::Sample( - RandomGenerator* random) const { + RandomGenerator* generator) const { // It is somewhat yak-shavey to get an actual materialized transform here. // We convert to symbolic, convert the symbolic to a vector and matrix of // symbolic, `Evaluate` those, convert the result back to a @@ -76,12 +76,12 @@ math::RigidTransformd Transform::Sample( const Vector3 symbolic_translation = symbolic_transform.translation(); const Eigen::Vector3d concrete_translation = - symbolic::Evaluate(symbolic_translation, {}, random); + symbolic::Evaluate(symbolic_translation, {}, generator); const math::RotationMatrix symbolic_rotation = symbolic_transform.rotation(); const math::RotationMatrixd concrete_rotation( - symbolic::Evaluate(symbolic_rotation.matrix(), {}, random)); + symbolic::Evaluate(symbolic_rotation.matrix(), {}, generator)); return math::RigidTransformd{concrete_rotation, concrete_translation}; } diff --git a/common/schema/transform.h b/common/schema/transform.h index 1366af14481a..256f14f653a4 100644 --- a/common/schema/transform.h +++ b/common/schema/transform.h @@ -186,7 +186,7 @@ class Transform { /// Samples this Transform. If this is deterministic, the result is the same /// as GetDeterministicValue. - math::RigidTransformd Sample(RandomGenerator* random) const; + math::RigidTransformd Sample(RandomGenerator* generator) const; template void Serialize(Archive* a) {