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/python/graph_codegen.py b/y2023/control_loops/python/graph_codegen.py
index 6f3bc0d..289913f 100644
--- a/y2023/control_loops/python/graph_codegen.py
+++ b/y2023/control_loops/python/graph_codegen.py
@@ -37,12 +37,13 @@
cc_file.append(" %s," % (alpha_unitizer))
if reverse:
cc_file.append(
- " Trajectory(dynamics, &hybrid_roll_joint_loop->plant(), Path::Reversed(%s()), 0.005));"
+ " Trajectory(dynamics, &hybrid_roll_joint_loop->plant(), Path::Reversed(%s()), 0.005), "
% (path_function_name(str(name))))
else:
cc_file.append(
- " Trajectory(dynamics, &hybrid_roll_joint_loop->plant(), %s(), 0.005));"
+ " Trajectory(dynamics, &hybrid_roll_joint_loop->plant(), %s(), 0.005), "
% (path_function_name(str(name))))
+ cc_file.append(f"\"{path_function_name(str(name))}\");")
start_index = None
end_index = None
@@ -116,6 +117,9 @@
"#include \"y2023/control_loops/superstructure/arm/arm_constants.h\"")
h_file.append(
"#include \"y2023/control_loops/superstructure/arm/trajectory.h\"")
+ h_file.append(
+ "#include \"y2023/control_loops/superstructure/arm/arm_trajectories_generated.h\""
+ )
h_file.append("")
h_file.append("namespace y2023 {")
@@ -130,19 +134,39 @@
h_file.append(
"using y2023::control_loops::superstructure::arm::kArmConstants;")
+ h_file.append(
+ "using y2023::control_loops::superstructure::arm::TrajectoryAndParamsFbs;"
+ )
+
h_file.append("")
h_file.append("struct TrajectoryAndParams {")
h_file.append(" TrajectoryAndParams(double new_vmax,")
h_file.append(
" const ::Eigen::Matrix<double, 3, 3> &new_alpha_unitizer,"
)
- h_file.append(" Trajectory &&new_trajectory)")
+ h_file.append(
+ " Trajectory &&new_trajectory, std::string_view new_name)"
+ )
h_file.append(" : vmax(new_vmax),")
h_file.append(" alpha_unitizer(new_alpha_unitizer),")
- h_file.append(" trajectory(::std::move(new_trajectory)) {}")
+ h_file.append(" trajectory(::std::move(new_trajectory)),")
+ h_file.append(" name(new_name) {}")
+ h_file.append(
+ "TrajectoryAndParams(const frc971::control_loops::arm::Dynamics *dynamics, const StateFeedbackHybridPlant<3, 1, 1> *roll, const TrajectoryAndParamsFbs &trajectory_and_params_fbs)"
+ )
+ h_file.append(": vmax(trajectory_and_params_fbs.vmax()),")
+ h_file.append(
+ "alpha_unitizer((trajectory_and_params_fbs.alpha_unitizer()->data(),")
+ h_file.append(" trajectory_and_params_fbs.alpha_unitizer()->data() +")
+ h_file.append(
+ " trajectory_and_params_fbs.alpha_unitizer()->size())),")
+ h_file.append(
+ "trajectory(dynamics, roll, *trajectory_and_params_fbs.trajectory()),")
+ h_file.append("name(trajectory_and_params_fbs.name()->string_view()) {}")
h_file.append(" double vmax;")
h_file.append(" ::Eigen::Matrix<double, 3, 3> alpha_unitizer;")
h_file.append(" Trajectory trajectory;")
+ h_file.append(" std::string_view name;")
h_file.append("};")
h_file.append("")
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index 5da5250..e6f14aa 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -87,6 +87,9 @@
hdrs = [
"superstructure.h",
],
+ data = [
+ "//y2023/control_loops/superstructure/arm:arm_trajectories_generated.bfbs",
+ ],
deps = [
":end_effector",
":superstructure_goal_fbs",
@@ -100,6 +103,7 @@
"//y2023:constants",
"//y2023/control_loops/drivetrain:drivetrain_can_position_fbs",
"//y2023/control_loops/superstructure/arm",
+ "//y2023/control_loops/superstructure/arm:arm_trajectories_fbs",
],
)
@@ -129,6 +133,7 @@
":superstructure_output_fbs",
":superstructure_position_fbs",
":superstructure_status_fbs",
+ "//aos:json_to_flatbuffer",
"//aos:math",
"//aos/events/logging:log_writer",
"//aos/testing:googletest",
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_
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index f59626c..9b9b119 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -4,6 +4,7 @@
#include "aos/flatbuffer_merge.h"
#include "aos/network/team_number.h"
#include "frc971/zeroing/wrap.h"
+#include "y2023/control_loops/superstructure/arm/arm_trajectories_generated.h"
DEFINE_bool(ignore_distance, false,
"If true, ignore distance when shooting and obay joystick_reader");
@@ -18,9 +19,11 @@
using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::RelativeEncoderProfiledJointStatus;
-Superstructure::Superstructure(::aos::EventLoop *event_loop,
- std::shared_ptr<const constants::Values> values,
- const ::std::string &name)
+Superstructure::Superstructure(
+ ::aos::EventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values,
+ const aos::FlatbufferVector<ArmTrajectories> &arm_trajectories,
+ const ::std::string &name)
: frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
values_(values),
@@ -29,7 +32,7 @@
"/drivetrain")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
- arm_(values_),
+ arm_(values_, arm_trajectories.message()),
end_effector_(),
wrist_(values->wrist.subsystem_params) {
event_loop->SetRuntimeRealtimePriority(30);
diff --git a/y2023/control_loops/superstructure/superstructure.h b/y2023/control_loops/superstructure/superstructure.h
index 87a5395..0c8b2c0 100644
--- a/y2023/control_loops/superstructure/superstructure.h
+++ b/y2023/control_loops/superstructure/superstructure.h
@@ -2,17 +2,22 @@
#define Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
#include "aos/events/event_loop.h"
+#include "aos/json_to_flatbuffer.h"
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2023/constants.h"
#include "y2023/control_loops/drivetrain/drivetrain_can_position_generated.h"
#include "y2023/control_loops/superstructure/arm/arm.h"
+#include "y2023/control_loops/superstructure/arm/arm_trajectories_generated.h"
#include "y2023/control_loops/superstructure/end_effector.h"
#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
+using y2023::control_loops::superstructure::arm::ArmTrajectories;
+using y2023::control_loops::superstructure::arm::TrajectoryAndParams;
+
namespace y2023 {
namespace control_loops {
namespace superstructure {
@@ -35,9 +40,11 @@
::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
- explicit Superstructure(::aos::EventLoop *event_loop,
- std::shared_ptr<const constants::Values> values,
- const ::std::string &name = "/superstructure");
+ explicit Superstructure(
+ ::aos::EventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values,
+ const aos::FlatbufferVector<ArmTrajectories> &arm_trajectories,
+ const ::std::string &name = "/superstructure");
double robot_velocity() const;
@@ -45,6 +52,11 @@
inline const EndEffector &end_effector() const { return end_effector_; }
inline const AbsoluteEncoderSubsystem &wrist() const { return wrist_; }
+ static const aos::FlatbufferVector<ArmTrajectories> GetArmTrajectories(
+ const std::string &filename) {
+ return aos::FileToFlatbuffer<arm::ArmTrajectories>(filename);
+ }
+
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 2a67adf..4eeb0f6 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -276,8 +276,12 @@
values_(std::make_shared<constants::Values>(constants::MakeValues())),
roborio_(aos::configuration::GetNode(configuration(), "roborio")),
logger_pi_(aos::configuration::GetNode(configuration(), "logger")),
+ arm_trajectories_(superstructure::Superstructure::GetArmTrajectories(
+ "y2023/control_loops/superstructure/arm/"
+ "arm_trajectories_generated.bfbs")),
superstructure_event_loop(MakeEventLoop("Superstructure", roborio_)),
- superstructure_(superstructure_event_loop.get(), values_),
+ superstructure_(superstructure_event_loop.get(), values_,
+ arm_trajectories_),
test_event_loop_(MakeEventLoop("test", roborio_)),
superstructure_goal_fetcher_(
test_event_loop_->MakeFetcher<Goal>("/superstructure")),
@@ -410,6 +414,8 @@
const aos::Node *const roborio_;
const aos::Node *const logger_pi_;
+ const ::aos::FlatbufferVector<ArmTrajectories> arm_trajectories_;
+
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
::y2023::control_loops::superstructure::Superstructure superstructure_;
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
diff --git a/y2023/control_loops/superstructure/superstructure_main.cc b/y2023/control_loops/superstructure/superstructure_main.cc
index bb25ad8..10a9ca9 100644
--- a/y2023/control_loops/superstructure/superstructure_main.cc
+++ b/y2023/control_loops/superstructure/superstructure_main.cc
@@ -2,6 +2,12 @@
#include "aos/init.h"
#include "y2023/control_loops/superstructure/superstructure.h"
+DEFINE_string(arm_trajectories, "arm_trajectories_generated.bfbs",
+ "The path to the generated arm trajectories bfbs file.");
+
+using y2023::control_loops::superstructure::Superstructure;
+using y2023::control_loops::superstructure::arm::ArmTrajectories;
+
int main(int argc, char **argv) {
::aos::InitGoogle(&argc, &argv);
@@ -9,11 +15,15 @@
aos::configuration::ReadConfig("aos_config.json");
::aos::ShmEventLoop event_loop(&config.message());
+
+ auto trajectories =
+ y2023::control_loops::superstructure::Superstructure::GetArmTrajectories(
+ FLAGS_arm_trajectories);
+
std::shared_ptr<const y2023::constants::Values> values =
std::make_shared<const y2023::constants::Values>(
y2023::constants::MakeValues());
- ::y2023::control_loops::superstructure::Superstructure superstructure(
- &event_loop, values);
+ Superstructure superstructure(&event_loop, values, trajectories);
event_loop.Run();
diff --git a/y2023/control_loops/superstructure/superstructure_replay.cc b/y2023/control_loops/superstructure/superstructure_replay.cc
index ed97966..9bd3ae2 100644
--- a/y2023/control_loops/superstructure/superstructure_replay.cc
+++ b/y2023/control_loops/superstructure/superstructure_replay.cc
@@ -17,6 +17,8 @@
DEFINE_int32(team, 971, "Team number to use for logfile replay.");
DEFINE_string(output_folder, "/tmp/superstructure_replay/",
"Logs all channels to the provided logfile.");
+DEFINE_string(arm_trajectories, "arm/arm_trajectories_generated.bfbs",
+ "The path to the generated arm trajectories bfbs file.");
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
@@ -45,10 +47,16 @@
auto logger = std::make_unique<aos::logger::Logger>(logger_event_loop.get());
logger->StartLoggingOnRun(FLAGS_output_folder);
- roborio->OnStartup([roborio]() {
+ auto trajectories =
+ y2023::control_loops::superstructure::Superstructure::GetArmTrajectories(
+ FLAGS_arm_trajectories);
+
+ roborio->OnStartup([roborio, &trajectories]() {
roborio->AlwaysStart<y2023::control_loops::superstructure::Superstructure>(
- "superstructure", std::make_shared<y2023::constants::Values>(
- y2023::constants::MakeValues()));
+ "superstructure",
+ std::make_shared<y2023::constants::Values>(
+ y2023::constants::MakeValues()),
+ trajectories);
});
std::unique_ptr<aos::EventLoop> print_loop = roborio->MakeEventLoop("print");