From f1033ec42e06d388780da5a6b09907ea2dc7cf20 Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Tue, 12 Mar 2024 09:49:21 -0400 Subject: [PATCH] separate drake from beacon_potential_python (#280) --- experimental/beacon_sim/BUILD | 19 +++++-- .../beacon_sim/beacon_potential_python.cc | 53 +++++++++++++++++++ .../beacon_sim/belief_road_map_planner.cc | 1 - .../beacon_sim/belief_road_map_planner.hh | 2 +- .../belief_road_map_planner_python_test.py | 4 +- .../beacon_sim/correlated_beacons_python.cc | 49 +---------------- .../beacon_sim/make_belief_updater.hh | 2 +- 7 files changed, 74 insertions(+), 56 deletions(-) create mode 100644 experimental/beacon_sim/beacon_potential_python.cc diff --git a/experimental/beacon_sim/BUILD b/experimental/beacon_sim/BUILD index 0b2a44e3..3ef6596b 100644 --- a/experimental/beacon_sim/BUILD +++ b/experimental/beacon_sim/BUILD @@ -393,6 +393,7 @@ cc_library( ":robot", ":robot_belief", ":make_belief_updater", + ":beacon_potential", "//common:check", "//common/math:combinations", "//common/math:multivariate_normal_cdf", @@ -418,7 +419,8 @@ pybind_extension( name = "belief_road_map_planner_python", srcs = ["belief_road_map_planner_python.cc"], data = [ - ":robot_belief_python.so" + ":robot_belief_python.so", + ":beacon_potential_python.so", ], deps = [ ":belief_road_map_planner", @@ -525,6 +527,14 @@ cc_library( ], ) +pybind_extension( + name = "beacon_potential_python", + srcs = ["beacon_potential_python.cc"], + deps = [ + ":beacon_potential", + ] +) + cc_test( name = "conditioned_potential_test", srcs = ["conditioned_potential_test.cc"], @@ -641,10 +651,12 @@ pybind_extension( name = "correlated_beacons_python", srcs = ["correlated_beacons_python.cc"], visibility=["//common/python:__subpackages__"], + data = [ + ":beacon_potential_python.so", + ], deps = [ ":correlated_beacons", ":beacon_potential", - ":correlated_beacon_potential", ] ) @@ -701,7 +713,7 @@ cc_library( deps = [ "@eigen", ":ekf_slam", - ":correlated_beacons", + ":beacon_potential", ":robot_belief", "//common:check", "//common/liegroups:se2", @@ -861,6 +873,7 @@ cc_binary( name = "run_experiment", srcs = ["run_experiment.cc"], data = [":experiment_configs"], + linkopts = ["-ltbb"], deps = [ "@cxxopts", "//common:check", diff --git a/experimental/beacon_sim/beacon_potential_python.cc b/experimental/beacon_sim/beacon_potential_python.cc new file mode 100644 index 00000000..f577c34c --- /dev/null +++ b/experimental/beacon_sim/beacon_potential_python.cc @@ -0,0 +1,53 @@ + +#include "experimental/beacon_sim/beacon_potential.hh" +#include "experimental/beacon_sim/beacon_potential_to_proto.hh" +#include "pybind11/pybind11.h" +#include "pybind11/stl.h" + +namespace py = pybind11; + +namespace robot::experimental::beacon_sim { +PYBIND11_MODULE(beacon_potential_python, m) { + py::class_(m, "LogMarginal") + .def_readwrite("present_beacons", &LogMarginal::present_beacons) + .def_readwrite("log_marginal", &LogMarginal::log_marginal) + .def("__repr__", [](const LogMarginal &self) { + std::ostringstream oss; + oss << "(self.present_beacons.size()); i++) { + oss << self.present_beacons.at(i); + if (i < static_cast(self.present_beacons.size()) - 1) { + oss << ", "; + } + } + oss << "] log_marginal=" << self.log_marginal << ">"; + return oss.str(); + }); + + py::class_(m, "BeaconPotential") + .def(py::init<>()) + .def_static("from_proto_string", + [](const std::string &proto_string) -> BeaconPotential { + proto::BeaconPotential proto; + proto.ParseFromString(proto_string); + return unpack_from(proto); + }) + .def("log_prob", + py::overload_cast &, bool>( + &BeaconPotential::log_prob, py::const_), + py::arg("assignment"), py::arg("allow_partial_assignment") = false) + .def("log_prob", + py::overload_cast &>(&BeaconPotential::log_prob, py::const_)) + .def("__mul__", + py::overload_cast(operator*)) + .def("log_marginals", &BeaconPotential::log_marginals) + .def("members", &BeaconPotential::members) + .def("to_proto_string", [](const BeaconPotential &self) { + proto::BeaconPotential proto; + pack_into(self, &proto); + std::string out; + proto.SerializeToString(&out); + return py::bytes(out); + }); +} +} // namespace robot::experimental::beacon_sim diff --git a/experimental/beacon_sim/belief_road_map_planner.cc b/experimental/beacon_sim/belief_road_map_planner.cc index f6d3eb22..d5069b2d 100644 --- a/experimental/beacon_sim/belief_road_map_planner.cc +++ b/experimental/beacon_sim/belief_road_map_planner.cc @@ -24,7 +24,6 @@ #include "common/math/multivariate_normal_cdf.hh" #include "common/math/redheffer_star.hh" #include "common/time/robot_time.hh" -#include "experimental/beacon_sim/correlated_beacons.hh" #include "experimental/beacon_sim/ekf_slam.hh" #include "experimental/beacon_sim/generate_observations.hh" #include "experimental/beacon_sim/make_belief_updater.hh" diff --git a/experimental/beacon_sim/belief_road_map_planner.hh b/experimental/beacon_sim/belief_road_map_planner.hh index 31971a91..8f8eab8f 100644 --- a/experimental/beacon_sim/belief_road_map_planner.hh +++ b/experimental/beacon_sim/belief_road_map_planner.hh @@ -5,7 +5,7 @@ #include "common/liegroups/se2.hh" #include "common/time/robot_time.hh" -#include "experimental/beacon_sim/correlated_beacons.hh" +#include "experimental/beacon_sim/beacon_potential.hh" #include "experimental/beacon_sim/ekf_slam.hh" #include "experimental/beacon_sim/robot_belief.hh" #include "planning/belief_road_map.hh" diff --git a/experimental/beacon_sim/belief_road_map_planner_python_test.py b/experimental/beacon_sim/belief_road_map_planner_python_test.py index 5d54fcbe..cc3458ce 100644 --- a/experimental/beacon_sim/belief_road_map_planner_python_test.py +++ b/experimental/beacon_sim/belief_road_map_planner_python_test.py @@ -3,7 +3,7 @@ import experimental.beacon_sim.belief_road_map_planner_python as brm import experimental.beacon_sim.test_helpers_python as helpers import experimental.beacon_sim.ekf_slam_python as esp -import experimental.beacon_sim.correlated_beacons_python as cbp +import experimental.beacon_sim.beacon_potential_python as bpp class BeliefRoadMapPlannerPythonTest(unittest.TestCase): @@ -74,7 +74,7 @@ def test_optimistic_belief_road_map_planner(self): ekf_config, P_LONE_BEACON ) - potential = cbp.BeaconPotential() + potential = bpp.BeaconPotential() # Action plan = brm.compute_belief_road_map_plan(road_map, ekf, potential, brm_options) diff --git a/experimental/beacon_sim/correlated_beacons_python.cc b/experimental/beacon_sim/correlated_beacons_python.cc index ef17bb96..235b2954 100644 --- a/experimental/beacon_sim/correlated_beacons_python.cc +++ b/experimental/beacon_sim/correlated_beacons_python.cc @@ -2,10 +2,7 @@ #include -#include "experimental/beacon_sim/beacon_potential_to_proto.hh" -#include "experimental/beacon_sim/correlated_beacon_potential.hh" #include "experimental/beacon_sim/correlated_beacons.hh" -#include "experimental/beacon_sim/precision_matrix_potential.hh" #include "pybind11/eigen.h" #include "pybind11/pybind11.h" #include "pybind11/stl.h" @@ -15,6 +12,7 @@ using namespace pybind11::literals; namespace robot::experimental::beacon_sim { PYBIND11_MODULE(correlated_beacons_python, m) { + py::module_::import("experimental.beacon_sim.beacon_potential_python"); m.doc() = "Correlated Beacons"; py::class_(m, "BeaconClique") @@ -24,51 +22,6 @@ PYBIND11_MODULE(correlated_beacons_python, m) { .def_readwrite("p_no_beacons", &BeaconClique::p_no_beacons) .def_readwrite("members", &BeaconClique::members); - py::class_(m, "LogMarginal") - .def_readwrite("present_beacons", &LogMarginal::present_beacons) - .def_readwrite("log_marginal", &LogMarginal::log_marginal) - .def("__repr__", [](const LogMarginal &self) { - std::ostringstream oss; - oss << "(self.present_beacons.size()); i++) { - oss << self.present_beacons.at(i); - if (i < static_cast(self.present_beacons.size()) - 1) { - oss << ", "; - } - } - oss << "] log_marginal=" << self.log_marginal << ">"; - return oss.str(); - }); - - py::class_(m, "BeaconPotential") - .def(py::init<>()) - .def_static( - "correlated_beacon_potential", - [](const double p_present, const double p_beacon_given_present, - const std::vector &members) -> BeaconPotential { - return CorrelatedBeaconPotential{.p_present = p_present, - .p_beacon_given_present = p_beacon_given_present, - .members = members}; - }, - "p_present"_a, "p_beacon_given_present"_a, "members"_a) - .def("log_prob", - py::overload_cast &, bool>( - &BeaconPotential::log_prob, py::const_), - py::arg("assignment"), py::arg("allow_partial_assignment") = false) - .def("log_prob", - py::overload_cast &>(&BeaconPotential::log_prob, py::const_)) - .def("__mul__", - py::overload_cast(operator*)) - .def("log_marginals", &BeaconPotential::log_marginals) - .def("members", &BeaconPotential::members) - .def("to_proto_string", [](const BeaconPotential &self) { - proto::BeaconPotential proto; - pack_into(self, &proto); - std::string out; - proto.SerializeToString(&out); - return py::bytes(out); - }); - m.def("create_correlated_beacons", create_correlated_beacons); } } // namespace robot::experimental::beacon_sim diff --git a/experimental/beacon_sim/make_belief_updater.hh b/experimental/beacon_sim/make_belief_updater.hh index e53dbf9b..6f768bff 100644 --- a/experimental/beacon_sim/make_belief_updater.hh +++ b/experimental/beacon_sim/make_belief_updater.hh @@ -8,7 +8,7 @@ #include "Eigen/Core" #include "common/liegroups/se2.hh" #include "common/math/redheffer_star.hh" -#include "experimental/beacon_sim/correlated_beacons.hh" +#include "experimental/beacon_sim/beacon_potential.hh" #include "experimental/beacon_sim/ekf_slam.hh" #include "experimental/beacon_sim/robot_belief.hh" #include "planning/belief_road_map.hh"