Bind Belief Road Map Planner (#275)

experimental/beacon_sim/BUILD

@@ -411,6 +411,31 @@ cc_test(
   ],
 )
 
+pybind_extension(
+  name = "belief_road_map_planner_python",
+  srcs = ["belief_road_map_planner_python.cc"],
+  data = [
+    ":robot_belief_python.so"
+  ],
+  deps = [
+    ":belief_road_map_planner",
+  ]
+)
+
+py_test(
+  name = "belief_road_map_planner_python_test",
+  srcs = ["belief_road_map_planner_python_test.py"],
+  data = [
+    "//planning:probabilistic_road_map_python.so",
+    ":belief_road_map_planner_python.so",
+    ":ekf_slam_python.so",
+    ":test_helpers_python.so",
+  ],
+  deps = [
+  ]
+
+)
+
 cc_binary(
   name = "run_trials",
   srcs = ["run_trials.cc"],
@@ -726,6 +751,14 @@ cc_library(
   ]
 )
 
+pybind_extension(
+  name = "robot_belief_python",
+  srcs = ["robot_belief_python.cc"],
+  deps = [
+    ":robot_belief",
+  ]
+)
+
 cc_library(
     name = "test_helpers",
     hdrs = ["test_helpers.hh"],
@@ -738,6 +771,18 @@ cc_library(
     ]
 )
 
+pybind_extension(
+  name = "test_helpers_python",
+  srcs = ["test_helpers_python.cc"],
+  data = [
+    ":correlated_beacons_python.so",
+    "//planning:probabilistic_road_map_python.so",
+  ],
+  deps = [
+    ":test_helpers",
+  ]
+)
+
 cc_library(
     name = "information_lower_bound_search",
     hdrs = ["information_lower_bound_search.hh"],
@@ -833,6 +878,7 @@ py_package(
   name = "beacon_sim_package",
   deps = [
     ":ekf_slam_python.so",
+    ":belief_road_map_planner_python.so",
   ],
 )
 

experimental/beacon_sim/belief_road_map_planner_python.cc

@@ -0,0 +1,102 @@
+
+#include "common/time/robot_time.hh"
+#include "experimental/beacon_sim/belief_road_map_planner.hh"
+#include "pybind11/pybind11.h"
+#include "pybind11/stl.h"
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+namespace robot::experimental::beacon_sim {
+
+template <typename T>
+constexpr void bind_brm_plan(const auto &m, const std::string &type) {
+    const std::string type_name = "BeliefRoadMapPlan" + type;
+    py::class_<planning::BRMPlan<T>>(m, type_name.c_str())
+        .def_readwrite("nodes", &planning::BRMPlan<T>::nodes)
+        .def_readwrite("beliefs", &planning::BRMPlan<T>::beliefs);
+}
+
+PYBIND11_MODULE(belief_road_map_planner_python, m) {
+    py::module_::import("experimental.beacon_sim.robot_belief_python");
+    bind_brm_plan<RobotBelief>(m, "RobotBelief");
+    bind_brm_plan<LandmarkRobotBelief>(m, "LandmarkRobotBelief");
+
+    py::class_<ExpectedBeliefPlanResult>(m, "ExpectedBeliefPlanResult")
+        .def(py::init<>())
+        .def(py::init<std::vector<int>, double>(), "nodes"_a, "log_probability_mass_tracked"_a)
+        .def_readwrite("nodes", &ExpectedBeliefPlanResult::nodes)
+        .def_readwrite("log_probability_mass_tracked",
+                       &ExpectedBeliefPlanResult::log_probability_mass_tracked);
+
+    py::class_<BeliefRoadMapOptions>(m, "BeliefRoadMapOptions")
+        .def(py::init<>())
+        .def(py::init<double, std::optional<double>, int,
+                      std::optional<time::RobotTimestamp::duration>>(),
+             "max_sensor_range_m"_a, "uncertainty_tolerance"_a, "max_num_edge_transforms"_a,
+             "timeout"_a)
+        .def_readwrite("max_sensor_range_m", &BeliefRoadMapOptions::max_sensor_range_m)
+        .def_readwrite("uncertainty_tolerance", &BeliefRoadMapOptions::uncertainty_tolerance)
+        .def_readwrite("max_num_edge_transforms", &BeliefRoadMapOptions::max_num_edge_transforms)
+        .def_readwrite("timeout", &BeliefRoadMapOptions::timeout);
+
+    auto landmark_brm_options =
+        py::class_<LandmarkBeliefRoadMapOptions>(m, "LandmarkBeliefRoadMapOptions")
+            .def(py::init<>())
+            .def(py::init<double, LandmarkBeliefRoadMapOptions::UncertaintySizeOptions,
+                          std::optional<LandmarkBeliefRoadMapOptions::SampledBeliefOptions>,
+                          std::optional<time::RobotTimestamp::duration>>(),
+                 "max_sensor_range_m"_a, "uncertainty_size_options"_a, "sampled_belief_options"_a,
+                 "timeout"_a)
+            .def_readwrite("max_sensor_range_m", &LandmarkBeliefRoadMapOptions::max_sensor_range_m)
+            .def_readwrite("uncertainty_size_options",
+                           &LandmarkBeliefRoadMapOptions::uncertainty_size_options)
+            .def_readwrite("sampled_belief_options",
+                           &LandmarkBeliefRoadMapOptions::sampled_belief_options)
+            .def_readwrite("timeout", &LandmarkBeliefRoadMapOptions::timeout);
+
+    py::class_<LandmarkBeliefRoadMapOptions::SampledBeliefOptions>(landmark_brm_options,
+                                                                   "Sampled_belief_options")
+        .def(py::init<>())
+        .def(py::init<int, int>(), "max_num_components"_a, "seed"_a)
+        .def_readwrite("max_num_components",
+                       &LandmarkBeliefRoadMapOptions::SampledBeliefOptions::max_num_components)
+        .def_readwrite("seed", &LandmarkBeliefRoadMapOptions::SampledBeliefOptions::seed);
+
+    py::class_<LandmarkBeliefRoadMapOptions::ExpectedDeterminant>(landmark_brm_options,
+                                                                  "ExpectedDeterminant")
+        .def(py::init<>());
+
+    py::class_<LandmarkBeliefRoadMapOptions::ValueAtRiskDeterminant>(landmark_brm_options,
+                                                                     "ValueAtRiskDeterminant")
+        .def(py::init<>())
+        .def(py::init<double>(), "percentile"_a)
+        .def_readwrite("percentile",
+                       &LandmarkBeliefRoadMapOptions::ValueAtRiskDeterminant::percentile);
+
+    py::class_<LandmarkBeliefRoadMapOptions::ProbMassInRegion>(landmark_brm_options,
+                                                               "ProbMassInRegion")
+        .def(py::init<>())
+        .def(py::init<double, double, double>(), "position_x_half_width_m"_a,
+             "position_y_half_width_m"_a, "heading_half_width_rad"_a)
+        .def_readwrite("position_x_half_width_m",
+                       &LandmarkBeliefRoadMapOptions::ProbMassInRegion::position_x_half_width_m)
+        .def_readwrite("position_y_half_width_m",
+                       &LandmarkBeliefRoadMapOptions::ProbMassInRegion::position_y_half_width_m)
+        .def_readwrite("heading_half_width_rad",
+                       &LandmarkBeliefRoadMapOptions::ProbMassInRegion::heading_half_width_rad);
+
+    py::class_<ExpectedBeliefRoadMapOptions>(m, "ExpectedBeliefRoadMapOptions")
+        .def(py::init<>())
+        .def(py::init<int, int, BeliefRoadMapOptions>(), "num_configuration_samples"_a, "seed"_a,
+             "brm_options"_a)
+        .def_readwrite("num_configuration_samples",
+                       &ExpectedBeliefRoadMapOptions::num_configuration_samples)
+        .def_readwrite("seed", &ExpectedBeliefRoadMapOptions::seed)
+        .def_readwrite("brm_options", &ExpectedBeliefRoadMapOptions::brm_options);
+
+    m.def("compute_belief_road_map_plan", &compute_belief_road_map_plan);
+    m.def("compute_landmark_belief_road_map_plan", &compute_landmark_belief_road_map_plan);
+    m.def("compute_expected_belief_road_map_plan", &compute_expected_belief_road_map_plan);
+}
+}  // namespace robot::experimental::beacon_sim

experimental/beacon_sim/belief_road_map_planner_python_test.py

@@ -0,0 +1,211 @@
+import unittest
+
+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
+
+
+class BeliefRoadMapPlannerPythonTest(unittest.TestCase):
+    def test_belief_road_map_planner(self):
+        # Setup
+        ekf_config = esp.EkfSlamConfig(
+            max_num_beacons=11,
+            initial_beacon_uncertainty_m=100.0,
+            along_track_process_noise_m_per_rt_meter=0.05,
+            cross_track_process_noise_m_per_rt_meter=0.05,
+            pos_process_noise_m_per_rt_s=0.0,
+            heading_process_noise_rad_per_rt_meter=1e-3,
+            heading_process_noise_rad_per_rt_s=0.0,
+            beacon_pos_process_noise_m_per_rt_s=1e-6,
+            range_measurement_noise_m=1e-1,
+            bearing_measurement_noise_rad=1e-1,
+            on_map_load_position_uncertainty_m=2.0,
+            on_map_load_heading_uncertainty_rad=0.5,
+        )
+
+        brm_options = brm.BeliefRoadMapOptions(
+            max_sensor_range_m=3.0,
+            uncertainty_tolerance=None,
+            max_num_edge_transforms=1000,
+            timeout=None,
+        )
+
+        P_LONE_BEACON = 0.5
+        P_STACKED_BEACON = 0.1
+        P_NO_STACK_BEACON = 0.01
+        road_map, ekf, potential = helpers.create_diamond_environment(
+            ekf_config, P_LONE_BEACON, P_NO_STACK_BEACON, P_STACKED_BEACON
+        )
+
+        # Action
+        plan = brm.compute_belief_road_map_plan(road_map, ekf, potential, brm_options)
+
+        # Verification
+        self.assertIsNotNone(plan)
+        self.assertNotIn(1, plan.nodes)
+
+    def test_optimistic_belief_road_map_planner(self):
+        # Setup
+        ekf_config = esp.EkfSlamConfig(
+            max_num_beacons=1,
+            initial_beacon_uncertainty_m=100.0,
+            along_track_process_noise_m_per_rt_meter=0.05,
+            cross_track_process_noise_m_per_rt_meter=0.05,
+            pos_process_noise_m_per_rt_s=0.0,
+            heading_process_noise_rad_per_rt_meter=1e-3,
+            heading_process_noise_rad_per_rt_s=0.0,
+            beacon_pos_process_noise_m_per_rt_s=1e-6,
+            range_measurement_noise_m=1e-1,
+            bearing_measurement_noise_rad=1e-1,
+            on_map_load_position_uncertainty_m=2.0,
+            on_map_load_heading_uncertainty_rad=0.5,
+        )
+
+        brm_options = brm.BeliefRoadMapOptions(
+            max_sensor_range_m=3.0,
+            uncertainty_tolerance=None,
+            max_num_edge_transforms=1000,
+            timeout=None,
+        )
+
+        P_LONE_BEACON = 0.5
+        road_map, ekf, potential = helpers.create_grid_environment(
+            ekf_config, P_LONE_BEACON
+        )
+
+        potential = cbp.BeaconPotential()
+
+        # Action
+        plan = brm.compute_belief_road_map_plan(road_map, ekf, potential, brm_options)
+
+        # Verification
+        self.assertIsNotNone(plan)
+        self.assertIn(0, plan.nodes)
+        self.assertIn(3, plan.nodes)
+
+    def test_landmark_belief_road_map_planner(self):
+        # Setup
+        ekf_config = esp.EkfSlamConfig(
+            max_num_beacons=2,
+            initial_beacon_uncertainty_m=100.0,
+            along_track_process_noise_m_per_rt_meter=0.05,
+            cross_track_process_noise_m_per_rt_meter=0.05,
+            pos_process_noise_m_per_rt_s=0.0,
+            heading_process_noise_rad_per_rt_meter=1e-3,
+            heading_process_noise_rad_per_rt_s=0.0,
+            beacon_pos_process_noise_m_per_rt_s=1e-6,
+            range_measurement_noise_m=1e-1,
+            bearing_measurement_noise_rad=1e-1,
+            on_map_load_position_uncertainty_m=2.0,
+            on_map_load_heading_uncertainty_rad=0.5,
+        )
+
+        brm_options = brm.LandmarkBeliefRoadMapOptions(
+            max_sensor_range_m=3.0,
+            uncertainty_size_options=brm.LandmarkBeliefRoadMapOptions.ExpectedDeterminant(),
+            sampled_belief_options=None,
+            timeout=None,
+        )
+
+        road_map, ekf, potential = helpers.create_stress_test_environment(ekf_config)
+
+        # Action
+        plan = brm.compute_landmark_belief_road_map_plan(
+            road_map, ekf, potential, brm_options
+        )
+
+        # Verification
+        self.assertIsNotNone(plan)
+        self.assertIn(0, plan.nodes)
+        self.assertIn(2, plan.nodes)
+
+    def test_expected_belief_road_map_planner_high_prob(self):
+        # Setup
+        ekf_config = esp.EkfSlamConfig(
+            max_num_beacons=1,
+            initial_beacon_uncertainty_m=100.0,
+            along_track_process_noise_m_per_rt_meter=0.05,
+            cross_track_process_noise_m_per_rt_meter=0.05,
+            pos_process_noise_m_per_rt_s=0.0,
+            heading_process_noise_rad_per_rt_meter=1e-3,
+            heading_process_noise_rad_per_rt_s=0.0,
+            beacon_pos_process_noise_m_per_rt_s=1e-6,
+            range_measurement_noise_m=1e-1,
+            bearing_measurement_noise_rad=1e-1,
+            on_map_load_position_uncertainty_m=2.0,
+            on_map_load_heading_uncertainty_rad=0.5,
+        )
+
+        brm_options = brm.ExpectedBeliefRoadMapOptions(
+            num_configuration_samples=100,
+            seed=0,
+            brm_options=brm.BeliefRoadMapOptions(
+                max_sensor_range_m=3.0,
+                uncertainty_tolerance=None,
+                max_num_edge_transforms=1000,
+                timeout=None,
+            ),
+        )
+
+        P_LONE_BEACON = 0.9
+        road_map, ekf, potential = helpers.create_grid_environment(
+            ekf_config, P_LONE_BEACON
+        )
+
+        # Action
+        plan = brm.compute_expected_belief_road_map_plan(
+            road_map, ekf, potential, brm_options
+        )
+
+        # Verification
+        self.assertIsNotNone(plan)
+        self.assertIn(0, plan.nodes)
+        self.assertIn(3, plan.nodes)
+
+    def test_expected_belief_road_map_planner_low_prob(self):
+        # Setup
+        ekf_config = esp.EkfSlamConfig(
+            max_num_beacons=1,
+            initial_beacon_uncertainty_m=100.0,
+            along_track_process_noise_m_per_rt_meter=0.05,
+            cross_track_process_noise_m_per_rt_meter=0.05,
+            pos_process_noise_m_per_rt_s=0.0,
+            heading_process_noise_rad_per_rt_meter=1e-3,
+            heading_process_noise_rad_per_rt_s=0.0,
+            beacon_pos_process_noise_m_per_rt_s=1e-6,
+            range_measurement_noise_m=1e-1,
+            bearing_measurement_noise_rad=1e-1,
+            on_map_load_position_uncertainty_m=2.0,
+            on_map_load_heading_uncertainty_rad=0.5,
+        )
+
+        brm_options = brm.ExpectedBeliefRoadMapOptions(
+            num_configuration_samples=100,
+            seed=0,
+            brm_options=brm.BeliefRoadMapOptions(
+                max_sensor_range_m=3.0,
+                uncertainty_tolerance=None,
+                max_num_edge_transforms=1000,
+                timeout=None,
+            ),
+        )
+
+        P_LONE_BEACON = 0.01
+        road_map, ekf, potential = helpers.create_grid_environment(
+            ekf_config, P_LONE_BEACON
+        )
+
+        # Action
+        plan = brm.compute_expected_belief_road_map_plan(
+            road_map, ekf, potential, brm_options
+        )
+
+        # Verification
+        self.assertIsNotNone(plan)
+        self.assertNotIn(0, plan.nodes)
+        self.assertNotIn(3, plan.nodes)
+
+
+if __name__ == "__main__":
+    unittest.main()

experimental/beacon_sim/correlated_beacons_python.cc

@@ -41,6 +41,7 @@ PYBIND11_MODULE(correlated_beacons_python, m) {
         });
 
     py::class_<BeaconPotential>(m, "BeaconPotential")
+        .def(py::init<>())
         .def_static(
             "correlated_beacon_potential",
             [](const double p_present, const double p_beacon_given_present,