diff --git a/experimental/beacon_sim/BUILD b/experimental/beacon_sim/BUILD index 3720b54c..ecac2491 100644 --- a/experimental/beacon_sim/BUILD +++ b/experimental/beacon_sim/BUILD @@ -533,3 +533,22 @@ cc_library( "//planning:probabilistic_road_map", ] ) + +cc_library( + name = "information_lower_bound_search", + hdrs = ["information_lower_bound_search.hh"], + srcs = ["information_lower_bound_search.cc"], + deps = [ + "@eigen", + "//planning:probabilistic_road_map", + ] +) + +cc_test( + name = "information_lower_bound_search_test", + srcs = ["information_lower_bound_search_test.cc"], + deps = [ + ":information_lower_bound_search", + "@com_google_googletest//:gtest_main", + ] +) diff --git a/experimental/beacon_sim/information_lower_bound_search.cc b/experimental/beacon_sim/information_lower_bound_search.cc new file mode 100644 index 00000000..d6320c63 --- /dev/null +++ b/experimental/beacon_sim/information_lower_bound_search.cc @@ -0,0 +1,149 @@ + +#include "experimental/beacon_sim/information_lower_bound_search.hh" + +#include +#include +#include + +namespace robot::experimental::beacon_sim { +namespace { +std::ostream &operator<<(std::ostream &out, const detail::InProgressPath &path) { + out << "["; + for (const auto id : path.path_to_goal) { + out << id << ", "; + } + out << "] cost: " << path.cost_to_go << " info_lb: " << path.info_lower_bound; + return out; +} +} +namespace detail { +MergeResult should_merge(const std::vector &existing, + const InProgressPath &new_path) { + if (existing.empty()) { + return {.should_merge = true, .to_boot = {}}; + } + + std::vector to_boot; + bool should_merge = false; + for (int i = 0; i < static_cast(existing.size()); i++) { + const InProgressPath &existing_path = existing.at(i); + if (existing_path.info_lower_bound >= new_path.info_lower_bound && + existing_path.cost_to_go >= new_path.cost_to_go) { + to_boot.push_back(i); + should_merge = true; + } else if (existing_path.info_lower_bound <= new_path.info_lower_bound && + existing_path.cost_to_go <= new_path.cost_to_go) { + should_merge = false; + break; + } else { + // Either one of the info_lower_bound or the cost to go is lower than the existing + // path, but the other is higher, so we keep both. + should_merge = true; + } + } + return {.should_merge = should_merge, .to_boot = std::move(to_boot)}; +} +} // namespace detail + +InformationLowerBoundResult information_lower_bound_search( + const planning::RoadMap &road_map, const int start_idx, const int end_idx, + const double start_information, const double end_information_lower_bound, + const LowerBoundReversePropagator &rev_propagator) { + std::unordered_map> paths_from_node_id; + std::priority_queue, + std::greater> + open_list; + open_list.push(detail::InProgressPath{.info_lower_bound = end_information_lower_bound, + .cost_to_go = 0.0, + .path_to_goal{end_idx}}); + + while (!open_list.empty()) { + std::cout << "------------------------ Open List Size: " << open_list.size() << std::endl; + // Note that this creates a copy. It is non-const so we can move it later. + detail::InProgressPath in_progress = open_list.top(); + open_list.pop(); + + std::cout << "Popping: " << in_progress << std::endl; + + const int current_node_id = in_progress.path_to_goal.back(); + std::vector &best_paths_at_node = + paths_from_node_id[current_node_id]; + + if (in_progress.path_to_goal.back() == start_idx && + start_information >= in_progress.info_lower_bound) { + std::cout << "Found valid path! Terminating" << std::endl; + std::reverse(in_progress.path_to_goal.begin(), in_progress.path_to_goal.end()); + return { + .info_lower_bound = in_progress.info_lower_bound, + .cost_to_go = in_progress.cost_to_go, + .path_to_goal = in_progress.path_to_goal, + }; + } + + std::cout << "Current Node Id: " << current_node_id << std::endl; + std::cout << "Candidates: " << std::endl; + for (const auto &path : best_paths_at_node) { + std::cout << "\t" << path << std::endl; + } + + // Try to merge the path into the per node queues + const detail::MergeResult merge_result = should_merge(best_paths_at_node, in_progress); + std::cout << "Merge Result: should merge? " << merge_result.should_merge << " to boot: ["; + for (const auto to_boot : merge_result.to_boot) { + std::cout << to_boot << ", "; + } + std::cout << "]" << std::endl; + + // If the path wasn't added, then we continue to the next path + if (!merge_result.should_merge) { + continue; + } + + // remove any paths that have been dominated + if (!merge_result.to_boot.empty()) { + auto insert_iter = best_paths_at_node.begin(); + auto should_skip_iter = merge_result.to_boot.begin(); + for (auto select_iter = best_paths_at_node.begin(); + select_iter != best_paths_at_node.end(); select_iter++) { + if (should_skip_iter != merge_result.to_boot.end()) { + if (*should_skip_iter == + std::distance(best_paths_at_node.begin(), select_iter)) { + should_skip_iter++; + continue; + } + } + *insert_iter = std::move(*select_iter); + insert_iter++; + } + best_paths_at_node.resize(best_paths_at_node.size() - merge_result.to_boot.size()); + } + + + // Create new paths from neighbors and queue. + for (int other_node_id = 0; other_node_id < static_cast(road_map.points.size()); + other_node_id++) { + if (road_map.adj(current_node_id, other_node_id)) { + std::vector new_path(in_progress.path_to_goal.begin(), + in_progress.path_to_goal.end()); + new_path.push_back(other_node_id); + const auto prop_result = + rev_propagator(other_node_id, current_node_id, in_progress.info_lower_bound); + + std::cout << "Considering neighbor: " << other_node_id << " info_lb: " << prop_result.info_lower_bound << " edge_cost " << prop_result.edge_cost << std::endl; + if (!std::isfinite(prop_result.info_lower_bound)) { + continue; + } + open_list.emplace(detail::InProgressPath{ + .info_lower_bound = prop_result.info_lower_bound, + .cost_to_go = prop_result.edge_cost + in_progress.cost_to_go, + .path_to_goal = std::move(new_path), + }); + } + } + + best_paths_at_node.emplace_back(std::move(in_progress)); + } + + return {}; +} +} // namespace robot::experimental::beacon_sim diff --git a/experimental/beacon_sim/information_lower_bound_search.hh b/experimental/beacon_sim/information_lower_bound_search.hh new file mode 100644 index 00000000..da86afc7 --- /dev/null +++ b/experimental/beacon_sim/information_lower_bound_search.hh @@ -0,0 +1,47 @@ + +#include + +#include "Eigen/Core" +#include "planning/probabilistic_road_map.hh" + +namespace robot::experimental::beacon_sim { + +struct PropagationResult { + double info_lower_bound; + double edge_cost; +}; + +// Propagate an information lower bound backwards in time from the end idx to the start idx. +// Note that these indices come from the road map +using LowerBoundReversePropagator = std::function; + +struct InformationLowerBoundResult { + double info_lower_bound; + double cost_to_go; + std::vector path_to_goal; +}; + +InformationLowerBoundResult information_lower_bound_search( + const planning::RoadMap &road_map, const int start_idx, const int end_idx, + const double start_information, const double end_information_lower_bound, + const LowerBoundReversePropagator &propagator); + +namespace detail { +struct InProgressPath { + double info_lower_bound; + double cost_to_go; + std::vector path_to_goal; + + bool operator>(const InProgressPath &other) const { return cost_to_go > other.cost_to_go; } +}; + +struct MergeResult { + bool should_merge; + std::vector to_boot; +}; + +MergeResult should_merge(const std::vector &existing, + const InProgressPath &new_path); +} // namespace detail +} // namespace robot::experimental::beacon_sim diff --git a/experimental/beacon_sim/information_lower_bound_search_test.cc b/experimental/beacon_sim/information_lower_bound_search_test.cc new file mode 100644 index 00000000..254f9207 --- /dev/null +++ b/experimental/beacon_sim/information_lower_bound_search_test.cc @@ -0,0 +1,212 @@ + +#include "experimental/beacon_sim/information_lower_bound_search.hh" + +#include "gtest/gtest.h" + +namespace robot::experimental::beacon_sim { +namespace { + +constexpr int GOAL_IDX = 5; +constexpr int START_IDX = 0; +constexpr int MEASUREMENT_IDX = 3; +constexpr int FAR_NODE_IDX = 4; + +struct TestEnvironment { + int start_idx; + int end_idx; + planning::RoadMap road_map; + LowerBoundReversePropagator rev_prop; +}; + +planning::RoadMap create_triangle_road_map() { + return { + .points = {{0, 0}, {1, 0}, {2, 0}, {1.5, 1}, {1.5, -10}, {3, 0}}, + // clang-format off + .adj = (Eigen::MatrixXd(6, 6) << 0, 1, 0, 0, 0, 0, + 1, 0, 1, 1, 1, 0, + 0, 1, 0, 1, 1, 1, + 0, 1, 1, 0, 0, 0, + 0, 1, 1, 0, 0, 0, + 0, 0, 1, 0, 0, 0).finished() + // clang-format on + }; +} + +LowerBoundReversePropagator create_triangle_rev_prop() { + return [](const int start, const int end, const double bound_at_end) -> PropagationResult { + const double Q = 0.2; + const double M = start == MEASUREMENT_IDX || end == MEASUREMENT_IDX ? 0.4 : 0; + const double info_lower_bound = (bound_at_end - M) / (1 - Q * (bound_at_end - M)); + return { + .info_lower_bound = info_lower_bound, + .edge_cost = (start == FAR_NODE_IDX || end == FAR_NODE_IDX) ? 10.0 : 1.0, + }; + }; +} + +TestEnvironment create_triangle_test_env() { + // Create the triangle test environment below + // We start on the left and want to get to the right. The path through 1-2 + // does not have any beacons where as the 1-3-2 path does. The optimal behavior + // is if the belief has more information than some amount, to proceed directly + // across the 1-2 path. However, if there is less information than this threshold + // amount, the 1-3-2 path should be desirable. The 1-4-2 path has no beacons and + // is significantly longer than 1-2 or 1-3-2, so it should never be preferred. + // + // B + // + // 3 + // ┌───X───┐ + // │ │ + // c=1 │ │ c=1 + // │1 2│ + // Start ────X───────X──── Goal + // │ c=1 │ + // │ │ + // c=10 │ │ c=10 + // └───X───┘ + // 4 + // + + return { + .start_idx = START_IDX, + .end_idx = GOAL_IDX, + .road_map = create_triangle_road_map(), + .rev_prop = create_triangle_rev_prop(), + }; +} +} // namespace + +TEST(InformationLowerBoundSearch, grid_environment_search_with_low_info_start) { + // Setup + TestEnvironment env = create_triangle_test_env(); + constexpr double GOAL_INFO_LOWER_BOUND = 1; + constexpr double START_INFO = 1; + + // Action + const auto result = information_lower_bound_search( + env.road_map, env.start_idx, env.end_idx, START_INFO, GOAL_INFO_LOWER_BOUND, env.rev_prop); + + // Verification + EXPECT_TRUE(false); +} + +TEST(InformationLowerBoundSearch, grid_environment_search_with_high_info_start) { + // Setup + TestEnvironment env = create_triangle_test_env(); + constexpr double GOAL_INFO_LOWER_BOUND = 1; + constexpr double START_INFO = 10; + + // Action + const auto result = information_lower_bound_search( + env.road_map, env.start_idx, env.end_idx, START_INFO, GOAL_INFO_LOWER_BOUND, env.rev_prop); + + // Verification + EXPECT_TRUE(false); +} + +TEST(InformationLowerBoundSearch, should_merge_with_empty) { + // Setup + const std::vector existing; + detail::InProgressPath new_path = { + .info_lower_bound = 20, .cost_to_go = 10, .path_to_goal = {}}; + + // Action + const detail::MergeResult result = should_merge(existing, new_path); + + // Verification + EXPECT_TRUE(result.should_merge); + EXPECT_TRUE(result.to_boot.empty()); +} + +TEST(InformationLowerBoundSearch, dominated_paths_are_booted) { + // Setup + const std::vector existing = { + { + .info_lower_bound = 30, + .cost_to_go = 40, + .path_to_goal = {}, + }, + { + .info_lower_bound = 40, + .cost_to_go = 15, + .path_to_goal = {}, + }, + { + .info_lower_bound = 10, + .cost_to_go = 50, + .path_to_goal = {}, + }, + }; + detail::InProgressPath new_path = { + .info_lower_bound = 20, .cost_to_go = 10, .path_to_goal = {}}; + + // Action + const detail::MergeResult result = should_merge(existing, new_path); + + // Verification + EXPECT_TRUE(result.should_merge); + EXPECT_EQ(result.to_boot.size(), 2); + EXPECT_EQ(result.to_boot.at(0), 0); + EXPECT_EQ(result.to_boot.at(1), 1); +} + +TEST(InformationLowerBoundSearch, dominated_new_path_is_dropped) { + // Setup + const std::vector existing = { + { + .info_lower_bound = 30, + .cost_to_go = 40, + .path_to_goal = {}, + }, + { + .info_lower_bound = 40, + .cost_to_go = 15, + .path_to_goal = {}, + }, + { + .info_lower_bound = 10, + .cost_to_go = 50, + .path_to_goal = {}, + }, + }; + detail::InProgressPath new_path = { + .info_lower_bound = 15, .cost_to_go = 60, .path_to_goal = {}}; + + // Action + const detail::MergeResult result = should_merge(existing, new_path); + + // Verification + EXPECT_FALSE(result.should_merge); + EXPECT_TRUE(result.to_boot.empty()); +} + +TEST(InformationLowerBoundSearch, keep_all_paths_if_different) { + // Setup + const std::vector existing = { + { + .info_lower_bound = 30, + .cost_to_go = 40, + .path_to_goal = {}, + }, + { + .info_lower_bound = 40, + .cost_to_go = 15, + .path_to_goal = {}, + }, + { + .info_lower_bound = 10, + .cost_to_go = 50, + .path_to_goal = {}, + }, + }; + detail::InProgressPath new_path = {.info_lower_bound = 5, .cost_to_go = 60, .path_to_goal = {}}; + + // Action + const detail::MergeResult result = should_merge(existing, new_path); + + // Verification + EXPECT_TRUE(result.should_merge); + EXPECT_TRUE(result.to_boot.empty()); +} +} // namespace robot::experimental::beacon_sim