From 82ffdb5c188811c12b22d6962915e71834b2b691 Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Wed, 6 Mar 2024 11:54:37 -0500 Subject: [PATCH 1/3] bind belief road map planner --- experimental/beacon_sim/BUILD | 45 +++++++++++++++++ .../belief_road_map_planner_python.cc | 36 ++++++++++++++ .../belief_road_map_planner_python_test.py | 49 +++++++++++++++++++ .../beacon_sim/robot_belief_python.cc | 14 ++++++ .../beacon_sim/test_helpers_python.cc | 15 ++++++ 5 files changed, 159 insertions(+) create mode 100644 experimental/beacon_sim/belief_road_map_planner_python.cc create mode 100644 experimental/beacon_sim/belief_road_map_planner_python_test.py create mode 100644 experimental/beacon_sim/robot_belief_python.cc create mode 100644 experimental/beacon_sim/test_helpers_python.cc diff --git a/experimental/beacon_sim/BUILD b/experimental/beacon_sim/BUILD index cb17f9b..339d555 100644 --- a/experimental/beacon_sim/BUILD +++ b/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"], diff --git a/experimental/beacon_sim/belief_road_map_planner_python.cc b/experimental/beacon_sim/belief_road_map_planner_python.cc new file mode 100644 index 0000000..8735943 --- /dev/null +++ b/experimental/beacon_sim/belief_road_map_planner_python.cc @@ -0,0 +1,36 @@ + +#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 +constexpr void bind_brm_plan(const auto &m, const std::string &type) { + const std::string type_name = "BeliefRoadMapPlan" + type; + py::class_>(m, type_name.c_str()) + .def_readwrite("nodes", &planning::BRMPlan::nodes) + .def_readwrite("beliefs", &planning::BRMPlan::beliefs); +} + +PYBIND11_MODULE(belief_road_map_planner_python, m) { + py::module_::import("experimental.beacon_sim.robot_belief_python"); + bind_brm_plan(m, "RobotBelief"); + py::class_(m, "BeliefRoadMapOptions") + .def(py::init<>()) + .def(py::init, int, + std::optional>(), + "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); + + m.def("compute_belief_road_map_plan", &compute_belief_road_map_plan); +} +} // namespace robot::experimental::beacon_sim diff --git a/experimental/beacon_sim/belief_road_map_planner_python_test.py b/experimental/beacon_sim/belief_road_map_planner_python_test.py new file mode 100644 index 0000000..8dd966d --- /dev/null +++ b/experimental/beacon_sim/belief_road_map_planner_python_test.py @@ -0,0 +1,49 @@ + +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 + +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) + + + +if __name__ == "__main__": + unittest.main() diff --git a/experimental/beacon_sim/robot_belief_python.cc b/experimental/beacon_sim/robot_belief_python.cc new file mode 100644 index 0000000..5dfa5b4 --- /dev/null +++ b/experimental/beacon_sim/robot_belief_python.cc @@ -0,0 +1,14 @@ + +#include "experimental/beacon_sim/robot_belief.hh" +#include "pybind11/eigen.h" +#include "pybind11/pybind11.h" + +namespace py = pybind11; + +namespace robot::experimental::beacon_sim { +PYBIND11_MODULE(robot_belief_python, m) { + py::class_(m, "RobotBelief") + .def_readwrite("local_from_robot", &RobotBelief::local_from_robot) + .def_readwrite("cov_in_robot", &RobotBelief::cov_in_robot); +} +} // namespace robot::experimental::beacon_sim diff --git a/experimental/beacon_sim/test_helpers_python.cc b/experimental/beacon_sim/test_helpers_python.cc new file mode 100644 index 0000000..c1913a1 --- /dev/null +++ b/experimental/beacon_sim/test_helpers_python.cc @@ -0,0 +1,15 @@ + +#include "experimental/beacon_sim/test_helpers.hh" +#include "pybind11/pybind11.h" + +namespace py = pybind11; + +namespace robot::experimental::beacon_sim { +PYBIND11_MODULE(test_helpers_python, m) { + py::module_::import("experimental.beacon_sim.correlated_beacons_python"); + py::module_::import("planning.probabilistic_road_map_python"); + m.def("create_grid_environment", &create_grid_environment); + m.def("create_diamond_environment", &create_diamond_environment); + m.def("create_stress_test_environment", &create_stress_test_environment); +} +} // namespace robot::experimental::beacon_sim From e94630f2f4d2512a38cb568f2c230954f1af2395 Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Fri, 8 Mar 2024 11:32:28 -0500 Subject: [PATCH 2/3] Bind remaining planning methods --- .../belief_road_map_planner_python.cc | 66 +++++++ .../belief_road_map_planner_python_test.py | 166 +++++++++++++++++- .../beacon_sim/correlated_beacons_python.cc | 1 + .../beacon_sim/robot_belief_python.cc | 14 ++ 4 files changed, 245 insertions(+), 2 deletions(-) diff --git a/experimental/beacon_sim/belief_road_map_planner_python.cc b/experimental/beacon_sim/belief_road_map_planner_python.cc index 8735943..d203d74 100644 --- a/experimental/beacon_sim/belief_road_map_planner_python.cc +++ b/experimental/beacon_sim/belief_road_map_planner_python.cc @@ -20,6 +20,15 @@ constexpr void bind_brm_plan(const auto &m, const std::string &type) { PYBIND11_MODULE(belief_road_map_planner_python, m) { py::module_::import("experimental.beacon_sim.robot_belief_python"); bind_brm_plan(m, "RobotBelief"); + bind_brm_plan(m, "LandmarkRobotBelief"); + + py::class_(m, "ExpectedBeliefPlanResult") + .def(py::init<>()) + .def(py::init, 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_(m, "BeliefRoadMapOptions") .def(py::init<>()) .def(py::init, int, @@ -31,6 +40,63 @@ PYBIND11_MODULE(belief_road_map_planner_python, m) { .def_readwrite("max_num_edge_transforms", &BeliefRoadMapOptions::max_num_edge_transforms) .def_readwrite("timeout", &BeliefRoadMapOptions::timeout); + auto landmark_brm_options = + py::class_(m, "LandmarkBeliefRoadMapOptions") + .def(py::init<>()) + .def(py::init, + std::optional>(), + "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_(landmark_brm_options, + "Sampled_belief_options") + .def(py::init<>()) + .def(py::init(), "max_num_components"_a, "seed"_a) + .def_readwrite("max_num_components", + &LandmarkBeliefRoadMapOptions::SampledBeliefOptions::max_num_components) + .def_readwrite("seed", &LandmarkBeliefRoadMapOptions::SampledBeliefOptions::seed); + + py::class_(landmark_brm_options, + "ExpectedDeterminant") + .def(py::init<>()); + + py::class_(landmark_brm_options, + "ValueAtRiskDeterminant") + .def(py::init<>()) + .def(py::init(), "percentile"_a) + .def_readwrite("percentile", + &LandmarkBeliefRoadMapOptions::ValueAtRiskDeterminant::percentile); + + py::class_(landmark_brm_options, + "ProbMassInRegion") + .def(py::init<>()) + .def(py::init(), "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_(m, "ExpectedBeliefRoadMapOptions") + .def(py::init<>()) + .def(py::init(), "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 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 8dd966d..5d54fcb 100644 --- a/experimental/beacon_sim/belief_road_map_planner_python_test.py +++ b/experimental/beacon_sim/belief_road_map_planner_python_test.py @@ -1,9 +1,10 @@ - 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): @@ -27,7 +28,8 @@ def test_belief_road_map_planner(self): max_sensor_range_m=3.0, uncertainty_tolerance=None, max_num_edge_transforms=1000, - timeout=None) + timeout=None, + ) P_LONE_BEACON = 0.5 P_STACKED_BEACON = 0.1 @@ -43,6 +45,166 @@ def test_belief_road_map_planner(self): 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__": diff --git a/experimental/beacon_sim/correlated_beacons_python.cc b/experimental/beacon_sim/correlated_beacons_python.cc index 74c575b..ef17bb9 100644 --- a/experimental/beacon_sim/correlated_beacons_python.cc +++ b/experimental/beacon_sim/correlated_beacons_python.cc @@ -41,6 +41,7 @@ PYBIND11_MODULE(correlated_beacons_python, m) { }); py::class_(m, "BeaconPotential") + .def(py::init<>()) .def_static( "correlated_beacon_potential", [](const double p_present, const double p_beacon_given_present, diff --git a/experimental/beacon_sim/robot_belief_python.cc b/experimental/beacon_sim/robot_belief_python.cc index 5dfa5b4..b397aae 100644 --- a/experimental/beacon_sim/robot_belief_python.cc +++ b/experimental/beacon_sim/robot_belief_python.cc @@ -10,5 +10,19 @@ PYBIND11_MODULE(robot_belief_python, m) { py::class_(m, "RobotBelief") .def_readwrite("local_from_robot", &RobotBelief::local_from_robot) .def_readwrite("cov_in_robot", &RobotBelief::cov_in_robot); + + auto landmark_robot_belief = + py::class_(m, "LandmarkRobotBelief") + .def_readwrite("local_from_robot", &LandmarkRobotBelief::local_from_robot) + .def_readwrite("log_probability_mass_tracked", + &LandmarkRobotBelief::log_probability_mass_tracked) + .def_readwrite("belief_from_config", &LandmarkRobotBelief::belief_from_config); + + py::class_( + landmark_robot_belief, "LandmarkConditionedRobotBelief") + .def_readwrite("cov_in_robot", + &LandmarkRobotBelief::LandmarkConditionedRobotBelief::cov_in_robot) + .def_readwrite("log_config_prob", + &LandmarkRobotBelief::LandmarkConditionedRobotBelief::log_config_prob); } } // namespace robot::experimental::beacon_sim From 675cdf2afc83e9dfc4cdb3f6b9a3637a8d2e5b1a Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Fri, 8 Mar 2024 12:21:19 -0500 Subject: [PATCH 3/3] add planner to python wheel --- experimental/beacon_sim/BUILD | 1 + 1 file changed, 1 insertion(+) diff --git a/experimental/beacon_sim/BUILD b/experimental/beacon_sim/BUILD index 339d555..847b007 100644 --- a/experimental/beacon_sim/BUILD +++ b/experimental/beacon_sim/BUILD @@ -878,6 +878,7 @@ py_package( name = "beacon_sim_package", deps = [ ":ekf_slam_python.so", + ":belief_road_map_planner_python.so", ], )