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/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index e1e3392..44b2aac 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -1,3 +1,5 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
cc_library(
name = "arm",
srcs = [
@@ -24,6 +26,39 @@
)
genrule(
+ name = "generated_arm_trajectory_genrule",
+ outs = [
+ "arm_trajectories_generated.bfbs",
+ ],
+ cmd = "$(location //y2023/control_loops/superstructure/arm:arm_trajectory_generator) --output $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2023/control_loops/superstructure/arm:arm_trajectory_generator",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+cc_binary(
+ name = "arm_trajectory_generator",
+ srcs = [
+ "arm_trajectory_gen.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":arm_constants",
+ ":arm_trajectories_fbs",
+ "//aos:flatbuffers",
+ "//aos:json_to_flatbuffer",
+ "//frc971/control_loops/double_jointed_arm:graph",
+ "//y2023:constants",
+ "//y2023/control_loops/superstructure/arm:generated_graph",
+ "//y2023/control_loops/superstructure/arm:trajectory",
+ "//y2023/control_loops/superstructure/roll:roll_plants",
+ ],
+)
+
+genrule(
name = "generated_graph_genrule",
outs = [
"generated_graph.h",
@@ -36,6 +71,15 @@
],
)
+flatbuffer_cc_library(
+ name = "arm_trajectories_fbs",
+ srcs = [
+ "arm_trajectories.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
+
cc_library(
name = "generated_graph",
srcs = [
@@ -86,6 +130,7 @@
srcs = ["trajectory.cc"],
hdrs = ["trajectory.h"],
target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
deps = [
"//frc971/control_loops:binomial",
"//frc971/control_loops:dlqr",
@@ -93,6 +138,7 @@
"//frc971/control_loops:hybrid_state_feedback_loop",
"//frc971/control_loops/double_jointed_arm:dynamics",
"//frc971/control_loops/double_jointed_arm:ekf",
+ "//y2023/control_loops/superstructure/arm:arm_trajectories_fbs",
"@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/y2023/control_loops/superstructure/arm/arm.cc b/y2023/control_loops/superstructure/arm/arm.cc
index 27a1450..6cd8d0d 100644
--- a/y2023/control_loops/superstructure/arm/arm.cc
+++ b/y2023/control_loops/superstructure/arm/arm.cc
@@ -16,7 +16,8 @@
} // namespace
-Arm::Arm(std::shared_ptr<const constants::Values> values)
+Arm::Arm(std::shared_ptr<const constants::Values> values,
+ const ArmTrajectories &arm_trajectories)
: values_(values),
state_(ArmState::UNINITIALIZED),
proximal_zeroing_estimator_(values_->arm_proximal.zeroing),
@@ -36,13 +37,17 @@
roll_joint_loop_(roll::MakeIntegralRollLoop()),
hybrid_roll_joint_loop_(roll::MakeIntegralHybridRollLoop()),
arm_ekf_(&dynamics_),
- search_graph_(MakeSearchGraph(&dynamics_, &trajectories_, alpha_unitizer_,
- constants::Values::kArmVMax(),
- &hybrid_roll_joint_loop_)),
+ search_graph_(GetSearchGraph(arm_trajectories)),
// Go to the start of the first trajectory.
follower_(&dynamics_, &hybrid_roll_joint_loop_, NeutralPoint()),
points_(PointList()),
current_node_(0) {
+ // Creating trajectories from fbs
+ for (const auto *trajectory : *arm_trajectories.trajectories()) {
+ trajectories_.emplace_back(&dynamics_, &hybrid_roll_joint_loop_.plant(),
+ *trajectory);
+ }
+
int i = 0;
for (const auto &trajectory : trajectories_) {
AOS_LOG(INFO, "trajectory length for edge node %d: %f\n", i,
@@ -54,7 +59,6 @@
void Arm::Reset() { state_ = ArmState::UNINITIALIZED; }
namespace {
-
// Proximal joint center in xy space
constexpr std::pair<double, double> kJointCenter = {-0.203, 0.787};
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_;
diff --git a/y2023/control_loops/superstructure/arm/arm_trajectories.fbs b/y2023/control_loops/superstructure/arm/arm_trajectories.fbs
new file mode 100644
index 0000000..b0570f0
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/arm_trajectories.fbs
@@ -0,0 +1,56 @@
+namespace y2023.control_loops.superstructure.arm;
+
+table SplineFbs {
+ // 2x4 Eigen matrix
+ control_points: [double] (id: 0);
+}
+
+struct AlphaThetaFbs {
+ // Parameterized value from 0-1
+ alpha: double;
+ // Roll joint angle
+ theta: double;
+}
+
+table CosSplineFbs {
+ spline: SplineFbs (id: 0);
+ roll: [AlphaThetaFbs] (id: 1);
+}
+
+table PathFbs {
+ spline: CosSplineFbs (id: 0);
+ distances: [float] (id: 1);
+}
+
+table TrajectoryFbs {
+ path: PathFbs (id: 0);
+ num_plan_points: uint64 (id: 1);
+ step_size: double (id: 2);
+ max_dvelocity_unfiltered: [double] (id: 3);
+ max_dvelocity_backward_accel: [double] (id: 4);
+ max_dvelocity_forwards_accel: [double] (id: 5);
+ max_dvelocity_backward_voltage: [double] (id: 6);
+ max_dvelocity_forwards_voltage: [double] (id: 7);
+ alpha_unitizer: [double] (id: 8);
+}
+
+struct EdgeFbs {
+ start: uint64;
+ end: uint64;
+ cost: double;
+}
+
+table TrajectoryAndParamsFbs {
+ vmax: double (id: 0);
+ alpha_unitizer: [double] (id: 1);
+ trajectory: TrajectoryFbs (id: 2);
+ name: string (id: 3);
+}
+
+table ArmTrajectories {
+ trajectories: [TrajectoryAndParamsFbs] (id: 0);
+ edges: [EdgeFbs] (id: 1);
+ // TODO(milind/maxwell): add vertexes in too
+}
+
+root_type ArmTrajectories;
diff --git a/y2023/control_loops/superstructure/arm/arm_trajectory_gen.cc b/y2023/control_loops/superstructure/arm/arm_trajectory_gen.cc
new file mode 100644
index 0000000..578bf1f
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/arm_trajectory_gen.cc
@@ -0,0 +1,196 @@
+#include <iostream>
+#include <memory>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/flatbuffers.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "frc971/control_loops/double_jointed_arm/graph.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "y2023/constants.h"
+#include "y2023/control_loops/superstructure/arm/arm_constants.h"
+#include "y2023/control_loops/superstructure/arm/arm_trajectories_generated.h"
+#include "y2023/control_loops/superstructure/arm/generated_graph.h"
+#include "y2023/control_loops/superstructure/arm/trajectory.h"
+#include "y2023/control_loops/superstructure/roll/integral_hybrid_roll_plant.h"
+
+using frc971::control_loops::arm::SearchGraph;
+using y2023::control_loops::superstructure::arm::AlphaThetaFbs;
+using y2023::control_loops::superstructure::arm::CosSpline;
+using y2023::control_loops::superstructure::arm::kArmConstants;
+using y2023::control_loops::superstructure::arm::NSpline;
+using y2023::control_loops::superstructure::arm::Path;
+using y2023::control_loops::superstructure::arm::Trajectory;
+
+DEFINE_string(output, "", "Defines the location of the output file");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ const frc971::control_loops::arm::Dynamics dynamics(kArmConstants);
+ std::vector<y2023::control_loops::superstructure::arm::TrajectoryAndParams>
+ trajectories;
+ Eigen::DiagonalMatrix<double, 3> alpha_unitizer(
+ (::Eigen::DiagonalMatrix<double, 3>().diagonal()
+ << (1.0 / y2023::constants::Values::kArmAlpha0Max()),
+ (1.0 / y2023::constants::Values::kArmAlpha1Max()),
+ (1.0 / y2023::constants::Values::kArmAlpha2Max()))
+ .finished());
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>
+ hybrid_roll_joint_loop = y2023::control_loops::superstructure::roll::
+ MakeIntegralHybridRollLoop();
+
+ // Optimizes paths
+ auto search_graph =
+ y2023::control_loops::superstructure::arm::MakeSearchGraph(
+ &dynamics, &trajectories, alpha_unitizer,
+ y2023::constants::Values::kArmVMax(), &hybrid_roll_joint_loop);
+
+ auto edges = search_graph.edges();
+
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+
+ std::vector<flatbuffers::Offset<
+ y2023::control_loops::superstructure::arm::TrajectoryAndParamsFbs>>
+ trajectory_and_params_offsets;
+
+ std::vector<y2023::control_loops::superstructure::arm::EdgeFbs> fbs_edges;
+
+ // Generating flatbuffer
+
+ for (const auto &trajectory : trajectories) {
+ auto nspline = trajectory.trajectory.path().spline().spline();
+ auto cos_spline = trajectory.trajectory.path().spline();
+ auto path = trajectory.trajectory.path();
+
+ auto control_points_offset = fbb.CreateVector(
+ nspline.control_points().data(), nspline.control_points().size());
+
+ std::vector<AlphaThetaFbs> roll;
+
+ for (const auto &alpha_theta : cos_spline.roll()) {
+ roll.emplace_back(alpha_theta.alpha, alpha_theta.theta);
+ }
+ auto cos_spline_roll = fbb.CreateVectorOfStructs(roll);
+
+ auto path_distances =
+ fbb.CreateVector(path.distances().data(), path.distances().size());
+
+ auto trajectory_max_dvelocity_unfiltered = fbb.CreateVector(
+ trajectory.trajectory.max_dvelocity_unfiltered().data(),
+ trajectory.trajectory.max_dvelocity_unfiltered().size());
+
+ auto trajectory_max_dvelocity_backward_accel = fbb.CreateVector(
+ trajectory.trajectory.max_dvelocity_backward_accel().data(),
+ trajectory.trajectory.max_dvelocity_backward_accel().size());
+
+ auto trajectory_max_dvelocity_forwards_accel = fbb.CreateVector(
+ trajectory.trajectory.max_dvelocity_forwards_accel().data(),
+ trajectory.trajectory.max_dvelocity_forwards_accel().size());
+
+ auto trajectory_max_dvelocity_backward_voltage = fbb.CreateVector(
+ trajectory.trajectory.max_dvelocity_backward_voltage().data(),
+ trajectory.trajectory.max_dvelocity_backward_voltage().size());
+
+ auto trajectory_max_dvelocity_forwards_voltage = fbb.CreateVector(
+ trajectory.trajectory.max_dvelocity_forwards_voltage().data(),
+ trajectory.trajectory.max_dvelocity_forwards_voltage().size());
+
+ auto trajectory_alpha_unitizer =
+ fbb.CreateVector(trajectory.trajectory.alpha_unitizer().data(),
+ trajectory.trajectory.alpha_unitizer().size());
+
+ auto trajectory_and_params_name = fbb.CreateString(trajectory.name);
+
+ auto trajectory_and_params_alpha_unitizer = fbb.CreateVector(
+ trajectory.alpha_unitizer.data(), trajectory.alpha_unitizer.size());
+
+ y2023::control_loops::superstructure::arm::SplineFbs::Builder
+ spline_builder(fbb);
+
+ spline_builder.add_control_points(control_points_offset);
+
+ auto spline_offset = spline_builder.Finish();
+
+ y2023::control_loops::superstructure::arm::CosSplineFbs::Builder
+ cos_spline_builder(fbb);
+
+ cos_spline_builder.add_spline(spline_offset);
+
+ cos_spline_builder.add_roll(cos_spline_roll);
+
+ auto cos_spline_offset = cos_spline_builder.Finish();
+
+ y2023::control_loops::superstructure::arm::PathFbs::Builder path_builder(
+ fbb);
+
+ path_builder.add_spline(cos_spline_offset);
+ path_builder.add_distances(path_distances);
+
+ auto path_offset = path_builder.Finish();
+
+ y2023::control_loops::superstructure::arm::TrajectoryFbs::Builder
+ trajectory_builder(fbb);
+
+ trajectory_builder.add_path(path_offset);
+ trajectory_builder.add_num_plan_points(
+ trajectory.trajectory.num_plan_points());
+ trajectory_builder.add_step_size(trajectory.trajectory.step_size());
+ trajectory_builder.add_max_dvelocity_unfiltered(
+ trajectory_max_dvelocity_unfiltered);
+ trajectory_builder.add_max_dvelocity_backward_accel(
+ trajectory_max_dvelocity_backward_accel);
+ trajectory_builder.add_max_dvelocity_forwards_accel(
+ trajectory_max_dvelocity_forwards_accel);
+ trajectory_builder.add_max_dvelocity_backward_voltage(
+ trajectory_max_dvelocity_backward_voltage);
+ trajectory_builder.add_max_dvelocity_forwards_voltage(
+ trajectory_max_dvelocity_forwards_voltage);
+ trajectory_builder.add_alpha_unitizer(trajectory_alpha_unitizer);
+
+ auto trajectory_offset = trajectory_builder.Finish();
+
+ y2023::control_loops::superstructure::arm::TrajectoryAndParamsFbs::Builder
+ trajectory_and_params_builder(fbb);
+
+ trajectory_and_params_builder.add_vmax(trajectory.vmax);
+ trajectory_and_params_builder.add_alpha_unitizer(
+ trajectory_and_params_alpha_unitizer);
+ trajectory_and_params_builder.add_trajectory(trajectory_offset);
+ trajectory_and_params_builder.add_name(trajectory_and_params_name);
+
+ trajectory_and_params_offsets.emplace_back(
+ trajectory_and_params_builder.Finish());
+ }
+
+ for (const auto &edge : edges) {
+ fbs_edges.emplace_back(edge.start, edge.end, edge.cost);
+ }
+
+ auto arm_trajectories_trajctory_and_params_offsets =
+ fbb.CreateVector(trajectory_and_params_offsets.data(),
+ trajectory_and_params_offsets.size());
+
+ auto arm_trajectories_edges = fbb.CreateVectorOfStructs(fbs_edges);
+
+ y2023::control_loops::superstructure::arm::ArmTrajectories::Builder
+ arm_trajectories_builder(fbb);
+
+ arm_trajectories_builder.add_trajectories(
+ arm_trajectories_trajctory_and_params_offsets);
+ arm_trajectories_builder.add_edges(arm_trajectories_edges);
+
+ auto arm_trajectories_offset = arm_trajectories_builder.Finish();
+
+ fbb.Finish(arm_trajectories_offset);
+
+ auto detatched_buffer = aos::FlatbufferDetachedBuffer<
+ y2023::control_loops::superstructure::arm::ArmTrajectories>(
+ fbb.Release());
+
+ // This writes to a binary file so we can cache the optimization results
+ aos::WriteFlatbufferToFile(FLAGS_output, detatched_buffer);
+}
diff --git a/y2023/control_loops/superstructure/arm/trajectory.cc b/y2023/control_loops/superstructure/arm/trajectory.cc
index 717a29e..d216467 100644
--- a/y2023/control_loops/superstructure/arm/trajectory.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory.cc
@@ -7,6 +7,7 @@
#include "frc971/control_loops/jacobian.h"
#include "frc971/control_loops/runge_kutta.h"
#include "gflags/gflags.h"
+#include "y2023/control_loops/superstructure/arm/arm_trajectories_generated.h"
DEFINE_double(lqr_proximal_pos, 0.5, "Position LQR gain");
DEFINE_double(lqr_proximal_vel, 5, "Velocity LQR gain");
@@ -165,6 +166,56 @@
return ::std::make_unique<Path>(p->Reversed());
}
+Trajectory::Trajectory(const frc971::control_loops::arm::Dynamics *dynamics,
+ const StateFeedbackHybridPlant<3, 1, 1> *roll,
+ const TrajectoryFbs &trajectory_fbs)
+ : dynamics_(dynamics),
+ roll_(roll),
+ num_plan_points_(trajectory_fbs.num_plan_points()),
+ step_size_(trajectory_fbs.step_size()),
+ max_dvelocity_unfiltered_(
+ trajectory_fbs.max_dvelocity_unfiltered()->data(),
+ trajectory_fbs.max_dvelocity_unfiltered()->data() +
+ trajectory_fbs.max_dvelocity_unfiltered()->size()),
+ max_dvelocity_backwards_voltage_(
+ trajectory_fbs.max_dvelocity_backward_voltage()->data(),
+ trajectory_fbs.max_dvelocity_backward_voltage()->data() +
+ trajectory_fbs.max_dvelocity_backward_voltage()->size()),
+ max_dvelocity_backwards_accel_(
+ trajectory_fbs.max_dvelocity_backward_accel()->data(),
+ trajectory_fbs.max_dvelocity_backward_accel()->data() +
+ trajectory_fbs.max_dvelocity_backward_accel()->size()),
+ max_dvelocity_forwards_accel_(
+ trajectory_fbs.max_dvelocity_forwards_accel()->data(),
+ trajectory_fbs.max_dvelocity_forwards_accel()->data() +
+ trajectory_fbs.max_dvelocity_forwards_accel()->size()),
+ max_dvelocity_forwards_voltage_(
+ trajectory_fbs.max_dvelocity_forwards_voltage()->data(),
+ trajectory_fbs.max_dvelocity_forwards_voltage()->data() +
+ trajectory_fbs.max_dvelocity_forwards_voltage()->size()),
+ alpha_unitizer_(trajectory_fbs.alpha_unitizer()->data()) {
+ auto control_points = ::Eigen::Matrix<double, 2, 4>(
+ trajectory_fbs.path()->spline()->spline()->control_points()->data());
+ NSpline<4, 2> spline(control_points);
+
+ std::vector<CosSpline::AlphaTheta> alpha_roll;
+
+ for (const auto &alpha_theta : *trajectory_fbs.path()->spline()->roll()) {
+ CosSpline::AlphaTheta atheta = {
+ .alpha = alpha_theta->alpha(),
+ .theta = alpha_theta->theta(),
+ };
+
+ alpha_roll.emplace_back(atheta);
+ }
+
+ CosSpline cos_spline(spline, alpha_roll);
+
+ path_ = std::make_unique<Path>(cos_spline,
+ (trajectory_fbs.path()->distances()->data(),
+ trajectory_fbs.path()->distances()->size()));
+}
+
double Trajectory::MaxCurvatureSpeed(
double goal_distance, const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,
double plan_vmax) {
diff --git a/y2023/control_loops/superstructure/arm/trajectory.h b/y2023/control_loops/superstructure/arm/trajectory.h
index 17ec3aa..e498168 100644
--- a/y2023/control_loops/superstructure/arm/trajectory.h
+++ b/y2023/control_loops/superstructure/arm/trajectory.h
@@ -13,6 +13,7 @@
#include "frc971/control_loops/fixed_quadrature.h"
#include "frc971/control_loops/hybrid_state_feedback_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2023/control_loops/superstructure/arm/arm_trajectories_generated.h"
namespace y2023 {
namespace control_loops {
@@ -152,6 +153,10 @@
// Returns the d^2spline/dalpha^2 for a given alpha.
::Eigen::Matrix<double, 3, 1> Alpha(double alpha) const;
+ const NSpline<4, 2> &spline() const { return spline_; }
+
+ const std::vector<AlphaTheta> &roll() { return roll_; }
+
CosSpline Reversed() const;
private:
@@ -170,6 +175,9 @@
Path(const CosSpline &spline, int num_alpha = 100)
: spline_(spline), distances_(BuildDistances(num_alpha)) {}
+ Path(const CosSpline &spline, std::vector<float> distances)
+ : spline_(spline), distances_(distances) {}
+
virtual ~Path() {}
// Returns a point on the spline as a function of distance.
@@ -186,6 +194,8 @@
const absl::Span<const float> distances() const { return distances_; }
+ const CosSpline &spline() const { return spline_; }
+
Path Reversed() const;
static ::std::unique_ptr<Path> Reversed(::std::unique_ptr<Path> p);
@@ -211,7 +221,7 @@
// size.
Trajectory(const frc971::control_loops::arm::Dynamics *dynamics,
const StateFeedbackHybridPlant<3, 1, 1> *roll,
- ::std::unique_ptr<const Path> path, double gridsize)
+ std::unique_ptr<const Path> path, double gridsize)
: dynamics_(dynamics),
roll_(roll),
path_(::std::move(path)),
@@ -222,6 +232,10 @@
alpha_unitizer_.setZero();
}
+ Trajectory(const frc971::control_loops::arm::Dynamics *dynamics,
+ const StateFeedbackHybridPlant<3, 1, 1> *roll,
+ const TrajectoryFbs &trajectory_fbs);
+
// Optimizes the trajectory. The path will adhere to the constraints that
// || angular acceleration * alpha_unitizer || < 1, and the applied voltage <
// plan_vmax.
@@ -431,6 +445,8 @@
const Path &path() const { return *path_; }
+ double step_size() const { return step_size_; }
+
private:
friend class testing::TrajectoryTest_IndicesForDistanceTest_Test;
@@ -457,7 +473,7 @@
const StateFeedbackHybridPlant<3, 1, 1> *roll_;
// The path to follow.
- ::std::unique_ptr<const Path> path_;
+ std::unique_ptr<const Path> path_;
// The number of points in the plan.
const size_t num_plan_points_;
// A cached version of the step size since we need this a *lot*.
@@ -607,8 +623,8 @@
};
} // namespace arm
+} // namespace superstructure
} // namespace control_loops
-} // namespace frc971
} // namespace y2023
#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_