Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Implement RRT* #26

Open
wants to merge 14 commits into
base: main
Choose a base branch
from
Open

Implement RRT* #26

Changes from 1 commit
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
db11527
added ao_benchmark.py and formatted with yapf
pranavjad Sep 3, 2024
6221d2e
added rrt_star.hh and formatted, also gitignored .vscode
pranavjad Sep 3, 2024
70e5449
change ! to not
pranavjad Sep 3, 2024
59122fb
added rrt_star_settings
pranavjad Sep 3, 2024
327287a
added common.hh
pranavjad Sep 3, 2024
3f2d0f0
added rrt_star_settings to settings.cc
pranavjad Sep 3, 2024
b13cf54
added rrt_star to __init__.py
pranavjad Sep 3, 2024
958ea41
added CL params to ao_benchmark.py ; added .vscode to gitignore
pranavjad Sep 3, 2024
99c16d9
minor fixes to ao_benchmark and rrt_star.hh
pranavjad Sep 5, 2024
af41d75
adding const and some minor changes
pranavjad Sep 5, 2024
6091525
changed loop variable to auto
pranavjad Sep 5, 2024
0704a6b
added spaces, consts, and moved build final path into a function
pranavjad Sep 9, 2024
599f53b
Merge pull request #2 from pranavjad/rrt_star
pranavjad Sep 9, 2024
a56d002
clang-format on rrt_star.hh
pranavjad Sep 9, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
clang-format on rrt_star.hh
  • Loading branch information
@pranavjad
pranavjad committed Sep 9, 2024
commit a56d00256340751331d4bf897622e89813447944
16 changes: 10 additions & 6 deletions src/impl/vamp/planning/rrt_star.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,12 +87,14 @@ namespace vamp::planning
}
};

const auto build_path = [&result, &buffer_index, &parent](const Configuration &goal, const std::size_t last_config_idx)
const auto build_path = [&result, &buffer_index, &parent](
const Configuration &goal, const std::size_t last_config_idx)
{
result.path.emplace_back(goal);
result.path.emplace_back(buffer_index(last_config_idx));
auto current = last_config_idx;
while (parent[current] != current) {
while (parent[current] != current)
{
auto parent_idx = parent[current];
result.path.emplace_back(buffer_index(parent_idx));
current = parent_idx;
Expand Down Expand Up @@ -127,7 +129,7 @@ namespace vamp::planning
std::vector<std::pair<std::size_t, Configuration>> goal_motions;
float best_path_cost = std::numeric_limits<float>::max();
std::pair<std::size_t, Configuration> best_goal_motion;

// main loop
std::size_t iter = 0;
std::vector<std::pair<NNNode<dimension>, float>> near;
Expand Down Expand Up @@ -175,7 +177,8 @@ namespace vamp::planning
settings.range);
tree.nearest(near, NNFloatArray<dimension>{new_configuration_ptr}, card, rrt_radius);

// initialize variables to keep track of the min cost neighbor and the cost of that neighbor
// initialize variables to keep track of the min cost neighbor and the cost of that
// neighbor
auto min_neighbor = nearest_node;
float min_cost = cost[nearest_node.index] + nearest_distance;

Expand Down Expand Up @@ -252,7 +255,8 @@ namespace vamp::planning
// loop through goal nodes to find the best solution
for (const auto &[end_node_idx, goal] : goal_motions)
{
const float cur_cost = cost[end_node_idx] + goal.distance(buffer_index(end_node_idx));
const float cur_cost =
cost[end_node_idx] + goal.distance(buffer_index(end_node_idx));
if (cur_cost < best_path_cost)
{
best_path_cost = cur_cost;
Expand All @@ -262,7 +266,7 @@ namespace vamp::planning
}
}
}

if (not settings.force_max_iters)
{
result.nanoseconds = vamp::utils::get_elapsed_nanoseconds(start_time);
Expand Down