diff --git a/constants/navgrid.json b/constants/navgrid.json index 0c0475ea..31f0f90a 100644 --- a/constants/navgrid.json +++ b/constants/navgrid.json @@ -7,14 +7,6 @@ "grid": [ [ true, - false, - false, - false, - false, - false, - false, - false, - false, true, true, true, @@ -28,18 +20,6 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, true, true, true, @@ -52,27 +32,39 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, true, true ], [ true, - false, - false, - false, - false, - false, - false, - false, - false, true, true, true, @@ -86,18 +78,6 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, true, true, true, @@ -110,20 +90,40 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, true, true ], [ true, - false, + true, false, false, false, @@ -180,6 +180,7 @@ true ], [ + true, true, false, false, @@ -189,18 +190,19 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -221,11 +223,9 @@ true, true, true, - true, - true, - true, - true, - true, + false, + false, + false, false, false, false, @@ -238,6 +238,7 @@ true ], [ + true, true, false, false, @@ -248,8 +249,7 @@ false, false, false, - true, - true, + false, true, true, true, @@ -274,15 +274,15 @@ false, false, false, + false, true, true, true, true, true, true, - true, - true, - true, + false, + false, false, false, false, @@ -296,6 +296,7 @@ true ], [ + true, true, false, false, @@ -306,9 +307,7 @@ false, false, false, - true, - true, - true, + false, true, true, true, @@ -333,14 +332,15 @@ false, false, false, + false, true, true, true, true, true, true, - true, - true, + false, + false, false, false, false, @@ -354,6 +354,7 @@ true ], [ + true, true, false, false, @@ -372,9 +373,6 @@ true, true, true, - true, - true, - false, false, false, false, @@ -391,13 +389,15 @@ false, false, false, - true, - false, - false, false, false, true, true, + true, + true, + true, + true, + false, false, false, false, @@ -412,6 +412,7 @@ true ], [ + true, true, false, false, @@ -430,10 +431,6 @@ true, true, true, - true, - true, - false, - false, false, false, false, @@ -449,13 +446,16 @@ false, false, false, - true, - false, false, false, false, true, true, + true, + true, + true, + true, + false, false, false, false, @@ -470,6 +470,7 @@ true ], [ + true, true, false, false, @@ -488,8 +489,6 @@ true, true, true, - true, - true, false, false, false, @@ -507,13 +506,14 @@ false, false, false, - true, - false, - false, - false, false, true, true, + true, + true, + true, + true, + false, false, false, false, @@ -539,15 +539,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, - true, - true, false, false, false, @@ -565,13 +556,22 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -1003,14 +1003,14 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -1035,7 +1035,7 @@ true, true, true, - true, + false, false, false, false, @@ -1061,7 +1061,7 @@ false, false, false, - true, + false, true, true, true, @@ -1087,13 +1087,13 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -1119,7 +1119,7 @@ false, false, false, - true, + false, true, true, true, @@ -1151,7 +1151,7 @@ true, true, true, - true, + false, false, false, false, @@ -1177,7 +1177,7 @@ false, false, false, - true, + false, true, true, true, @@ -1209,7 +1209,7 @@ true, true, true, - true, + false, false, false, false, @@ -1235,7 +1235,7 @@ false, false, false, - true, + false, true, true, true, @@ -1267,8 +1267,8 @@ true, true, true, - true, - true, + false, + false, false, false, false, @@ -1292,9 +1292,8 @@ false, false, false, - true, - true, - true, + false, + false, true, true, true, @@ -1318,15 +1317,16 @@ false, false, false, + false, + false, true, true, true, true, true, true, - true, - true, - true, + false, + false, false, false, false, @@ -1340,6 +1340,7 @@ true ], [ + true, true, false, false, @@ -1349,11 +1350,8 @@ false, false, false, - true, - true, - true, - true, - true, + false, + false, true, true, true, @@ -1375,10 +1373,10 @@ false, false, false, - true, - true, - true, - true, + false, + false, + false, + false, true, true, true, @@ -1394,12 +1392,14 @@ false, false, false, + false, + false, true, true ], [ true, - false, + true, false, false, false, @@ -1457,14 +1457,6 @@ ], [ true, - false, - false, - false, - false, - false, - false, - false, - false, true, true, true, @@ -1478,18 +1470,6 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, true, true, true, @@ -1502,27 +1482,39 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, true, true ], [ true, - false, - false, - false, - false, - false, - false, - false, - false, true, true, true, @@ -1536,18 +1528,6 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, true, true, true, @@ -1560,14 +1540,34 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, true, true ] diff --git a/src/camera/cv_camera.cc b/src/camera/cv_camera.cc index 59f23f08..dfb71a11 100644 --- a/src/camera/cv_camera.cc +++ b/src/camera/cv_camera.cc @@ -33,7 +33,13 @@ CVCamera::CVCamera(const CameraConstant& c, std::optional log_path) LOG(INFO) << c.pipeline.value(); backup_image_ = cv::imread("/bos/constants/dont_worry_about_it.jpg"); - if (c.frame_height.has_value() && c.frame_width.has_value()) { + if (backup_image_.empty()) { + LOG(WARNING) << "Backup image /bos/constants/dont_worry_about_it.jpg not " + "found; using black placeholder"; + int w = c.frame_width.value_or(640); + int h = c.frame_height.value_or(480); + backup_image_ = cv::Mat::zeros(h, w, CV_8UC3); + } else if (c.frame_height.has_value() && c.frame_width.has_value()) { cv::resize(backup_image_, backup_image_, cv::Size(c.frame_width.value(), c.frame_height.value())); } diff --git a/src/pathing/CMakeLists.txt b/src/pathing/CMakeLists.txt index 5d057b96..605cee8c 100644 --- a/src/pathing/CMakeLists.txt +++ b/src/pathing/CMakeLists.txt @@ -1,2 +1,5 @@ -add_library(pathing pathfinding.cc splines.cc controller.cc) +add_library(pathing pathfinding.cc splines.cc velocity_profile.cc controller.cc) target_link_libraries(pathing PUBLIC wpilibc wpiutil ntcore nlohmann_json::nlohmann_json utils localization ${OpenCV_LIBS}) + +add_executable(pathing_simulator simulator.cc) +target_link_libraries(pathing_simulator PRIVATE pathing utils) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index 22e86f39..899a4106 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -1,29 +1,22 @@ #include #include -#include #include -#include #include #include -#include -#include -#include #include "splines.h" #include "src/localization/position_receiver.h" -#include "src/pathing/splines.h" #include "src/utils/log.h" +#include "src/utils/pch.h" +#include "velocity_profile.h" namespace pathing { auto RunController( const std::string& navgrid_path = "/root/bos/constants/navgrid.json", bool verbose = false) -> void { - const int lookahead_offset_ = 50; - const double speed_ = 1.0; std::ifstream file(navgrid_path); if (!file.is_open()) { - LOG(FATAL) << "Failed to open navgrid.json" << std::endl; LOG(FATAL) << "Failed to open navgrid.json"; return; } @@ -53,17 +46,17 @@ auto RunController( auto vx_pub = inst.GetDoubleTopic("/pathing/vx").Publish(); auto vy_pub = inst.GetDoubleTopic("/pathing/vy").Publish(); - auto next_pose_sub = - inst.GetStructTopic("/pathing/nextPose").Publish(); - - std::vector spline_points; + SplineResult result; + std::vector> velocity_profile; + int prev_closest_idx = -1; while (true) { if (!enabled_sub.Get()) { vx_pub.Set(0.0); vy_pub.Set(0.0); - spline_points.clear(); + result.points.clear(); + velocity_profile.clear(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } @@ -90,25 +83,25 @@ auto RunController( if (current_pose.Translation().Distance(t2d).value() < nodeSizeMeters) { vx_pub.Set(0.0); vy_pub.Set(0.0); - spline_points.clear(); + result.points.clear(); + velocity_profile.clear(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } - if (spline_points.empty()) { - std::vector new_spline = - createSpline(grid, start_pt, target_pt, nodeSizeMeters); - if (!new_spline.empty()) { - spline_points = new_spline; + if (result.points.empty()) { + result = CreateSpline(grid, start_pt, target_pt, nodeSizeMeters); + velocity_profile = CreateVelocityProfile(result); + if (!result.points.empty()) { if (verbose) { - for (const auto& p : spline_points) { + for (const auto& p : result.points) { LOG(INFO) << "spline pt: " << p.X().value() << " " << p.Y().value(); } } } } - if (spline_points.empty()) { + if (result.points.empty() || velocity_profile.empty()) { vx_pub.Set(0.0); vy_pub.Set(0.0); std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -119,10 +112,10 @@ auto RunController( int closest_idx = 0; // TODO: this can be optimizeed by only searching from the previous closest index instead of the entire spline - frc::Translation2d first2d(spline_points[0].X(), spline_points[0].Y()); + frc::Translation2d first2d(result.points[0].X(), result.points[0].Y()); double best_dist = current_pose.Translation().Distance(first2d).value(); - for (int i = 1; i < (int)spline_points.size(); ++i) { - frc::Translation2d t2d(spline_points[i].X(), spline_points[i].Y()); + for (int i = 1; i < (int)result.points.size(); ++i) { + frc::Translation2d t2d(result.points[i].X(), result.points[i].Y()); double d = current_pose.Translation().Distance(t2d).value(); if (d < best_dist) { best_dist = d; @@ -135,44 +128,57 @@ auto RunController( if (verbose) { LOG(INFO) << "Closeset idx: " << closest_idx - << " Spline size: " << spline_points.size(); + << " Spline size: " << result.points.size(); } - int lookahead_idx = std::min(closest_idx + lookahead_offset_, - (int)spline_points.size() - 1); - - frc::Pose2d lookahead = spline_points[lookahead_idx]; - - if (verbose) { - LOG(INFO) << "current " << current_pose.X().value() << " " - << current_pose.Y().value(); - } - - double dx = lookahead.X().value() - current_pose.X().value(); - double dy = lookahead.Y().value() - current_pose.Y().value(); - if (verbose) { - LOG(INFO) << "dx " << dx << " vy " << dy; - LOG(INFO) << "looakhead " << lookahead.X().value() << " " - << lookahead.Y().value(); + if (prev_closest_idx >= 0 && closest_idx < prev_closest_idx) { + vx_pub.Set(0.0); + vy_pub.Set(0.0); + inst.Flush(); + result.points.clear(); + velocity_profile.clear(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + continue; } + if (prev_closest_idx >= 0 && closest_idx == prev_closest_idx && + closest_idx >= static_cast(result.params.size()) - 5) { + double dx = target_pose.X().value() - current_pose.X().value(); + double dy = target_pose.Y().value() - current_pose.Y().value(); + double dist = std::hypot(dx, dy); + + while (dist > 0.001) { + double vx = (dx / dist) * 1.0; + double vy = (dy / dist) * 1.0; + + vx_pub.Set(vx); + vy_pub.Set(vy); + if (verbose) { + LOG(INFO) << "linear approach vx " << vx << " vy " << vy; + } + inst.Flush(); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); - next_pose_sub.Set(lookahead); - - double dist = std::hypot(dx, dy); - if (verbose) { - LOG(INFO) << "dist " << dist; - } - if (dist < 1e-6) { + current_pose = current_sub.Get(); + dx = target_pose.X().value() - current_pose.X().value(); + dy = target_pose.Y().value() - current_pose.Y().value(); + dist = std::hypot(dx, dy); + } vx_pub.Set(0.0); vy_pub.Set(0.0); + inst.Flush(); + result.points.clear(); + velocity_profile.clear(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } + prev_closest_idx = closest_idx; + + if (closest_idx >= static_cast(result.params.size())) { + closest_idx = static_cast(result.params.size()) - 1; + } - double vx = dx * speed_; - double vy = dy * speed_; + const auto [vx, vy] = velocity_profile[closest_idx]; - // NOTE: we need to test whether to divide vx and vy by dist to normalize speeds or not, vx_pub.Set(vx); vy_pub.Set(vy); if (verbose) { diff --git a/src/pathing/pathfinding.cc b/src/pathing/pathfinding.cc index 8b0b2bf3..7bb526cf 100644 --- a/src/pathing/pathfinding.cc +++ b/src/pathing/pathfinding.cc @@ -9,13 +9,59 @@ namespace pathing { +auto BFSFirstFreeCell(std::vector>& field, Point start_point) + -> Node { + int sx = start_point.x; + int sy = start_point.y; + + field[sy][sx].visited = true; + + std::deque queue; + queue.push_back(start_point); + + std::vector> dirs = {{-1, -1}, {-1, 0}, {-1, 1}, {0, -1}, + {0, 1}, {1, -1}, {1, 0}, {1, 1}}; + + while (!queue.empty()) { + const Point current_point = queue.front(); + queue.pop_front(); + + Node& current = field[current_point.y][current_point.x]; + + for (auto [dy, dx] : dirs) { + int nx = current.x + dx; + int ny = current.y + dy; + + if (nx >= 0 && nx < (int)field[0].size() && ny >= 0 && + ny < (int)field.size()) { + if (!field[ny][nx].visited) { + field[ny][nx].visited = true; + if (!field[ny][nx].obstacle) { + return field[ny][nx]; + } + queue.push_back({.x = (uint)nx, .y = (uint)ny}); + } + } + } + } + + return field[sy][sx]; +} + auto BFS(std::vector>& field, Point start_point, Point end_point) -> std::vector { int sx = start_point.x; int sy = start_point.y; + if (field[sy][sx].obstacle) { + Node adjusted = BFSFirstFreeCell(field, start_point); + sx = adjusted.x; + sy = adjusted.y; + } Node* start = &field[sy][sx]; + start_point.x = sx; + start_point.y = sy; start->visited = true; start->cost = 0; @@ -85,6 +131,9 @@ auto BFS(std::vector>& field, Point start_point, field[rcurrent->y][rcurrent->x].path = true; } } + if (rcurrent != nullptr) { + rpath.push_back(*rcurrent); + } std::reverse(rpath.begin(), rpath.end()); std::cout << "BFS path (" << start_point.x << "," << start_point.y << ") -> (" diff --git a/src/pathing/pathfinding.h b/src/pathing/pathfinding.h index 626051bf..e619d553 100644 --- a/src/pathing/pathfinding.h +++ b/src/pathing/pathfinding.h @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -9,7 +10,7 @@ namespace pathing { struct Node { uint x, y; - double cost = DBL_MAX; + double cost = INFINITY; bool visited = false; bool obstacle = false; char readable; @@ -25,6 +26,9 @@ struct Point { uint x, y; }; +auto BFSFirstFreeCell(std::vector>& field, Point start_point) + -> Node; + auto BFS(std::vector>& field, Point start_point, Point end_point) -> std::vector; diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index 128248cc..dcd083bc 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -1,147 +1,156 @@ #include +#include +#include #include #include #include #include #include #include -#include #include -#include "src/pathing/pathing.h" +#include "src/pathing/splines.h" +#include "src/pathing/velocity_profile.h" +#include "src/utils/log.h" auto main() -> int { - wpi::log::DataLogBackgroundWriter log{"/root/bos/logs", "sim.wpilog"}; - - wpi::log::StructLogEntry poseLog(log, "/sim/Pose2d"); - wpi::log::DoubleLogEntry accelXLog(log, "/sim/AccelX"); - wpi::log::DoubleLogEntry accelYLog(log, "/sim/AccelY"); - wpi::log::DoubleLogEntry accelMagLog(log, "/sim/AccelMagnitude"); - wpi::log::DoubleLogEntry velXLog(log, "/sim/VelX"); - wpi::log::DoubleLogEntry velYLog(log, "/sim/VelY"); + const int64_t dt_us = 20000; + const double dt_sec = 0.020; std::ifstream file("/root/bos/constants/navgrid.json"); if (!file.is_open()) { return 1; } - nlohmann::json data = nlohmann::json::parse(file); file.close(); - const int GRID_W = data["grid"][0].size(); - const int GRID_H = data["grid"].size(); - double nodeSizeMeters = data["nodeSizeMeters"]; - - std::vector> gridData(GRID_H, std::vector(GRID_W)); - for (int y = 0; y < GRID_H; ++y) { - for (int x = 0; x < GRID_W; ++x) { - gridData[y][x] = data["grid"][y][x]; + const int grid_h = data["grid"].size(); + const int grid_w = data["grid"][0].size(); + const double node_size_meters = data["nodeSizeMeters"]; + + std::vector> grid( + grid_h, std::vector(grid_w)); + for (int y = 0; y < grid_h; ++y) { + for (int x = 0; x < grid_w; ++x) { + grid[y][x].x = x; + grid[y][x].y = y; + grid[y][x].obstacle = data["grid"][y][x]; } } - cv::Mat grid = initializeGrid(gridData); - - auto poses = createSpline(grid, 10, 5, 45, 22, nodeSizeMeters); - if (poses.empty()) { - return 1; - } - - constexpr int64_t kDtUs = 20'000; - constexpr double kDtSec = kDtUs / 1'000'000.0; - constexpr double kMaxAccel = 3.0; - constexpr double kMaxDecel = 3.0; - constexpr double kMaxModuleSpeed = 5.0; + int start_x = 10; + int start_y = 6; + int target_x = 44; + int target_y = 12; + start_x = std::clamp(start_x, 0, grid_w - 1); + start_y = std::clamp(start_y, 0, grid_h - 1); + target_x = std::clamp(target_x, 0, grid_w - 1); + target_y = std::clamp(target_y, 0, grid_h - 1); + + frc::Pose2d current_pose(units::meter_t{start_x * node_size_meters}, + units::meter_t{start_y * node_size_meters}, + units::radian_t{0.0}); + const frc::Pose2d target_pose(units::meter_t{target_x * node_size_meters}, + units::meter_t{target_y * node_size_meters}, + units::radian_t{0.0}); + + wpi::log::DataLogBackgroundWriter log{"logs", "sim.wpilog"}; + log.AddStructSchema(); + wpi::log::StructLogEntry pose_log(log, "/sim/Pose2d"); + wpi::log::StructLogEntry target_log(log, "/sim/TargetPose2d"); + wpi::log::DoubleLogEntry vx_log(log, "/sim/pathing/vx"); + wpi::log::DoubleLogEntry vy_log(log, "/sim/pathing/vy"); + wpi::log::DoubleLogEntry speed_log(log, "/sim/speed"); + wpi::log::IntegerLogEntry closest_idx_log(log, "/sim/closest_idx"); + wpi::log::DoubleLogEntry v_profile_zero_x(log, "/sim/v_profile_zero_x"); + wpi::log::DoubleLogEntry v_profile_zero_y(log, "/sim/v_profile_zero_y"); + target_log.Append(target_pose, 1); + + pathing::SplineResult result; + std::vector> velocity_profile; + const int max_steps = 10000; int64_t t = 0; + int prev_closest_idx = 0; + for (int step = 0; step < max_steps; ++step) { + pathing::Point start_pt{ + .x = static_cast(current_pose.X().value() / node_size_meters), + .y = static_cast(current_pose.Y().value() / node_size_meters)}; + pathing::Point target_pt{ + .x = static_cast(target_pose.X().value() / node_size_meters), + .y = static_cast(target_pose.Y().value() / node_size_meters)}; + start_pt.x = std::clamp(static_cast(start_pt.x), 0, grid_w - 1); + start_pt.y = std::clamp(static_cast(start_pt.y), 0, grid_h - 1); + target_pt.x = std::clamp(static_cast(target_pt.x), 0, grid_w - 1); + target_pt.y = std::clamp(static_cast(target_pt.y), 0, grid_h - 1); + + frc::Translation2d t2d(target_pose.X(), target_pose.Y()); + double dist_to_target = current_pose.Translation().Distance(t2d).value(); + if (dist_to_target < node_size_meters) { + vx_log.Append(0.0, t); + vy_log.Append(0.0, t); + pose_log.Append(current_pose, t); + break; + } - std::vector pathDist(poses.size(), 0.0); - for (size_t i = 1; i < poses.size(); ++i) { - double dx = poses[i].X().value() - poses[i - 1].X().value(); - double dy = poses[i].Y().value() - poses[i - 1].Y().value(); - pathDist[i] = pathDist[i - 1] + std::sqrt(dx * dx + dy * dy); - } - - double totalDist = pathDist.back(); - - std::vector targetSpeed(poses.size()); - - for (size_t i = 0; i < poses.size(); ++i) { - double distFromStart = pathDist[i]; - double distToEnd = totalDist - pathDist[i]; + if (result.points.empty()) { + result = + pathing::CreateSpline(grid, start_pt, target_pt, node_size_meters); + velocity_profile = pathing::CreateVelocityProfile(result); + prev_closest_idx = -1; + } - double accelLimitedSpeed = std::sqrt(2.0 * kMaxAccel * distFromStart); - double decelLimitedSpeed = std::sqrt(2.0 * kMaxDecel * distToEnd); + if (result.points.empty() || velocity_profile.empty()) { + vx_log.Append(0.0, t); + vy_log.Append(0.0, t); + pose_log.Append(current_pose, t); + break; + } - double maxVelThisPose = kMaxModuleSpeed; - if (i > 0) { - double dx = poses[i].X().value() - poses[i - 1].X().value(); - double dy = poses[i].Y().value() - poses[i - 1].Y().value(); - double vx = dx / kDtSec; - double vy = dy / kDtSec; - double maxComponentSpeed = std::max(std::abs(vx), std::abs(vy)); - if (maxComponentSpeed > 0.001) { - double speedRatio = std::sqrt(vx * vx + vy * vy) / maxComponentSpeed; - maxVelThisPose = kMaxModuleSpeed / speedRatio; + const int search_window = 1000; + + // Start searching from 1 because index 0 of velocity profile is always 0, + // it needs an initial speed. + int start_search = std::max(1, prev_closest_idx); + int end_search = std::min(static_cast(result.points.size()) - 1, + start_search + search_window); + + int closest_idx = start_search; + frc::Translation2d first2d(result.points[start_search].X(), + result.points[start_search].Y()); + double best_dist = current_pose.Translation().Distance(first2d).value(); + for (int i = start_search + 1; i <= end_search; ++i) { + frc::Translation2d p(result.points[i].X(), result.points[i].Y()); + double d = current_pose.Translation().Distance(p).value(); + if (d < best_dist) { + best_dist = d; + closest_idx = i; } } - targetSpeed[i] = - std::min({accelLimitedSpeed, decelLimitedSpeed, maxVelThisPose}); - } - - double currentVx = 0.0; - double currentVy = 0.0; - double currentX = poses[0].X().value(); - double currentY = poses[0].Y().value(); - double currentSpeed = 0.0; - - for (size_t i = 0; i < poses.size(); ++i) { - frc::Pose2d actualPose{units::meter_t{currentX}, units::meter_t{currentY}, - poses[i].Rotation()}; - poseLog.Append(actualPose, t); - - double desiredSpeed = targetSpeed[i]; - - double dvMag = desiredSpeed - currentSpeed; - double accelMag = 0.0; - - if (dvMag > 0) { - accelMag = std::min(dvMag / kDtSec, kMaxAccel); - } else { - accelMag = std::max(dvMag / kDtSec, -kMaxDecel); + if (prev_closest_idx >= 0 && closest_idx < prev_closest_idx) { + closest_idx = prev_closest_idx; } - currentSpeed += accelMag * kDtSec; - currentSpeed = std::max(0.0, currentSpeed); - - if (i > 0) { - double dx = poses[i].X().value() - poses[i - 1].X().value(); - double dy = poses[i].Y().value() - poses[i - 1].Y().value(); - double segDist = std::sqrt(dx * dx + dy * dy); - - double dirX = segDist > 0.001 ? dx / segDist : 0.0; - double dirY = segDist > 0.001 ? dy / segDist : 0.0; - - double newVx = dirX * currentSpeed; - double newVy = dirY * currentSpeed; - - double ax = (newVx - currentVx) / kDtSec; - double ay = (newVy - currentVy) / kDtSec; - - currentVx = newVx; - currentVy = newVy; - - currentX += currentVx * kDtSec; - currentY += currentVy * kDtSec; - - double accelMagTotal = std::sqrt(ax * ax + ay * ay); + prev_closest_idx = closest_idx; - accelXLog.Append(ax, t); - accelYLog.Append(ay, t); - accelMagLog.Append(accelMagTotal, t); - velXLog.Append(currentVx, t); - velYLog.Append(currentVy, t); + if (closest_idx >= static_cast(result.params.size())) { + closest_idx = static_cast(result.params.size()) - 1; } - t += kDtUs; + auto [vx, vy] = velocity_profile[closest_idx]; + + vx_log.Append(vx, t); + vy_log.Append(vy, t); + speed_log.Append(std::hypot(vx, vy), t); + closest_idx_log.Append(closest_idx, t); + pose_log.Append(current_pose, t); + v_profile_zero_x.Append(velocity_profile[0].first, t); + v_profile_zero_y.Append(velocity_profile[0].second, t); + + current_pose = + frc::Pose2d(units::meter_t{current_pose.X().value() + vx * dt_sec}, + units::meter_t{current_pose.Y().value() + vy * dt_sec}, + units::radian_t{0.0}); + t += dt_us; } log.Flush(); diff --git a/src/pathing/splines.cc b/src/pathing/splines.cc index 24e457d6..39df2cf6 100644 --- a/src/pathing/splines.cc +++ b/src/pathing/splines.cc @@ -25,7 +25,38 @@ auto KnotVector(int n, int p) -> std::vector { return knots; } -auto basis(int i, int p, double t, const std::vector& knots) -> double { +auto FiniteDifferences(const std::vector>& controls, + const std::vector& knots, int p, int k) + -> std::vector> { + if (k == 0) { + return controls; + } + + if (controls.size() <= 1 || k >= static_cast(controls.size())) { + return {}; + } + + std::vector> ret; + ret.reserve(controls.size() - 1); + + std::vector> prev = + FiniteDifferences(controls, knots, p, k - 1); + + for (int i = 0; i < (int)prev.size() - 1; ++i) { + double denom = knots[i + p + 1] - knots[i + k]; + if (denom != 0) { + double x = (prev[i + 1].first - prev[i].first) / denom; + double y = (prev[i + 1].second - prev[i].second) / denom; + ret.emplace_back(x, y); + } else { + ret.emplace_back(0.0, 0.0); + } + } + + return ret; +} + +auto Basis(int i, int p, double t, const std::vector& knots) -> double { if (p == 0) { if (knots[i] <= t && t < knots[i + 1]) { return 1.0; @@ -37,20 +68,21 @@ auto basis(int i, int p, double t, const std::vector& knots) -> double { double left = 0.0; double denoml = knots[i + p] - knots[i]; if (denoml != 0) { - left = (t - knots[i]) / denoml * basis(i, p - 1, t, knots); + left = (t - knots[i]) / denoml * Basis(i, p - 1, t, knots); } double right = 0.0; double denomr = knots[i + p + 1] - knots[i + 1]; if (denomr != 0) { - right = (knots[i + p + 1] - t) / denomr * basis(i + 1, p - 1, t, knots); + right = (knots[i + p + 1] - t) / denomr * Basis(i + 1, p - 1, t, knots); } return left + right; } -auto evaluate(double t, const std::vector>& controls, - const std::vector& knots, int p) +auto EvaluatePosition(double t, + const std::vector>& controls, + const std::vector& knots, int p) -> std::pair { if (t >= knots.back()) { return controls.back(); @@ -59,16 +91,35 @@ auto evaluate(double t, const std::vector>& controls, int n = static_cast(controls.size()); double x = 0.0, y = 0.0; for (int i = 0; i < n; ++i) { - double b = basis(i, p, t, knots); + double b = Basis(i, p, t, knots); x += b * controls[i].first; y += b * controls[i].second; } return {x, y}; } -auto createSpline(const std::vector>& grid, +auto EvaluateDerivative(double t, + const std::vector>& controls, + const std::vector& knots, int p, int k) + -> std::pair { + if (k < 0 || k > p || controls.size() <= static_cast(k)) { + return {0.0, 0.0}; + } + + std::vector> first_diffs = + FiniteDifferences(controls, knots, p, k); + auto [x, y] = EvaluatePosition(t, first_diffs, knots, p - k); + + int n = 1; + for (int i = p; i >= p - k + 1; i--) { + n *= i; + } + return {x * n, y * n}; +} + +auto CreateSpline(const std::vector>& grid, Point start_point, Point target_point, double nodeSizeMeters) - -> std::vector { + -> SplineResult { std::vector> gridCopy = grid; std::vector path = BFS(gridCopy, start_point, target_point); @@ -79,6 +130,11 @@ auto createSpline(const std::vector>& grid, } std::vector> control_points; + std::vector knots; + std::vector spline_points; + std::vector spline_params; + uint p; + control_points.reserve(path.size()); for (const pathing::Node& node : path) { control_points.emplace_back(node.x * nodeSizeMeters, @@ -86,26 +142,25 @@ auto createSpline(const std::vector>& grid, } uint numControls = control_points.size(); - if (numControls < 4) { return {}; } - uint p = 3; + p = 3; if (numControls <= p) { p = numControls - 1; } - std::vector knots = KnotVector(numControls, p); + knots = KnotVector(numControls, p); - std::vector spline_points; for (int t = 0; t <= 1000; t += 1) { double t_real = t / 1000.0; - auto [x, y] = evaluate(t_real, control_points, knots, p); + auto [x, y] = EvaluatePosition(t_real, control_points, knots, p); spline_points.emplace_back(units::meter_t{x}, units::meter_t{y}, 0_rad); + spline_params.emplace_back(t_real); } - - return spline_points; + auto first_deriv_controls = FiniteDifferences(control_points, knots, p, 1); + return {spline_points, control_points, first_deriv_controls, + knots, spline_params, p}; } - } // namespace pathing diff --git a/src/pathing/splines.h b/src/pathing/splines.h index f265c6b7..426b5075 100644 --- a/src/pathing/splines.h +++ b/src/pathing/splines.h @@ -2,21 +2,39 @@ #include #include +#include #include #include "pathfinding.h" namespace pathing { +struct SplineResult { + std::vector points; + std::vector> controls; + std::vector> first_deriv_controls; + std::vector knots; + std::vector params; + uint p; +}; + auto KnotVector(int n, int p) -> std::vector; -auto basis(int i, int p, double t, const std::vector& knots) -> double; +auto FiniteDifferences(const std::vector>& controls, + const std::vector& knots, int p, int k) + -> std::vector>; + +auto Basis(int i, int p, double t, const std::vector& knots) -> double; -auto evaluate(double t, const std::vector>& controls, - const std::vector& knots, int p) +auto EvaluatePosition(double t, + const std::vector>& controls, + const std::vector& knots, int p) + -> std::pair; +auto EvaluateDerivative(double t, + const std::vector>& controls, + const std::vector& knots, int p, int k) -> std::pair; -auto createSpline(const std::vector>& grid, Point start_point, - Point target_point, double nodeSizeMeters) - -> std::vector; +auto CreateSpline(const std::vector>& grid, Point start_point, + Point target_point, double nodeSizeMeters) -> SplineResult; } // namespace pathing diff --git a/src/pathing/velocity_profile.cc b/src/pathing/velocity_profile.cc new file mode 100644 index 00000000..2f39515e --- /dev/null +++ b/src/pathing/velocity_profile.cc @@ -0,0 +1,73 @@ +#include "velocity_profile.h" +#include +#include + +namespace pathing { + +auto _CreateVelocityProfile(const SplineResult& result, double maxVelocity, + double maxAcceleration) + -> std::vector> { + if (result.points.empty() || result.params.empty() || maxVelocity <= 0.0 || + maxAcceleration <= 0.0) { + return {}; + } + + std::vector distances; + distances.reserve(result.points.size()); + distances.emplace_back(0.0); + + for (int i = 1; i < static_cast(result.points.size()); ++i) { + frc::Translation2d prev(result.points[i - 1].X(), result.points[i - 1].Y()); + frc::Translation2d current(result.points[i].X(), result.points[i].Y()); + double distance = current.Distance(prev).value(); + distances.emplace_back(distances.back() + distance); + } + + double total_distance = distances.back(); + if (total_distance <= 0.0) { + return {}; + } + + double ramp_distance = maxVelocity * maxVelocity / (2.0 * maxAcceleration); + double profile_velocity = maxVelocity; + if (ramp_distance * 2.0 > total_distance) { + ramp_distance = total_distance / 2.0; + profile_velocity = std::sqrt(total_distance * maxAcceleration); + } + + std::vector> velocities; + velocities.reserve(result.params.size()); + + for (int i = 0; i < static_cast(result.params.size()); ++i) { + double distance = + distances[std::min(i, static_cast(distances.size()) - 1)]; + double end_distance = total_distance - distance; + double speed = profile_velocity; + + if (distance < ramp_distance) { + speed = std::sqrt(2.0 * maxAcceleration * distance); + } + if (end_distance < ramp_distance) { + speed = std::min(speed, std::sqrt(2.0 * maxAcceleration * end_distance)); + } + + auto [dx, dy] = EvaluateDerivative(result.params[i], result.controls, + result.knots, result.p, 1); + double derivative = std::hypot(dx, dy); + if (derivative <= 1e-6) { + velocities.emplace_back(0.0, 0.0); + } else { + velocities.emplace_back((dx / derivative) * speed, + (dy / derivative) * speed); + } + } + + return velocities; +} + +auto CreateVelocityProfile(const SplineResult& result) + -> std::vector> { + return _CreateVelocityProfile(result, max_velocity, max_accel); +} + +} // namespace pathing diff --git a/src/pathing/velocity_profile.h b/src/pathing/velocity_profile.h new file mode 100644 index 00000000..6206f247 --- /dev/null +++ b/src/pathing/velocity_profile.h @@ -0,0 +1,20 @@ +#pragma once + +#include +#include +#include "splines.h" + +namespace pathing { + +// Gotten from current pathplanner config of team +const double max_velocity = 3.5; +const double max_accel = 3.2; + +auto _CreateVelocityProfile(const SplineResult& result, double maxVelocity, + double maxAcceleration) + -> std::vector>; + +auto CreateVelocityProfile(const SplineResult& result) + -> std::vector>; + +} // namespace pathing