Allow pre-compiling arm trajectories
The robot takes a while before it's usable because it has to re-optimize
the arm trajectories when it runs. This commit takes the outputted c++
from graph_codegen and optimizes the trajectories before the robot
runs. It then puts them into a bfbs which is able to be placed on the
robot and read.
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ic75f9d590d6b89bfd5eed8b804e73c8c84ecec6b
diff --git a/y2023/control_loops/superstructure/arm/arm.h b/y2023/control_loops/superstructure/arm/arm.h
index 216b89d..1f97d80 100644
--- a/y2023/control_loops/superstructure/arm/arm.h
+++ b/y2023/control_loops/superstructure/arm/arm.h
@@ -21,7 +21,8 @@
class Arm {
public:
- Arm(std::shared_ptr<const constants::Values> values);
+ Arm(std::shared_ptr<const constants::Values> values,
+ const ArmTrajectories &arm_trajectories);
flatbuffers::Offset<superstructure::ArmStatus> Iterate(
const ::aos::monotonic_clock::time_point /*monotonic_now*/,
@@ -52,6 +53,23 @@
follower_.path_distance_to_go() < 1e-3;
}
+ static SearchGraph GetSearchGraph(const ArmTrajectories &arm_trajectories) {
+ // Creating edges from fbs
+ std::vector<SearchGraph::Edge> edges;
+
+ for (const auto *edge : *arm_trajectories.edges()) {
+ SearchGraph::Edge edge_data{
+ .start = static_cast<size_t>(edge->start()),
+ .end = static_cast<size_t>(edge->end()),
+ .cost = edge->cost(),
+ };
+
+ edges.emplace_back(edge_data);
+ }
+
+ return SearchGraph(edges.size(), std::move(edges));
+ }
+
std::shared_ptr<const constants::Values> values_;
ArmState state_;