Fix arm freakout
There was an uninitialized variable which James found. Manually setting
it to a bad value triggers the arm to freak out when disabled, and the
code below didn't clear it. I confirmed that initializing it to 0 fixes
it...
I have no clue how to write a test for this reasonably. We'd need
sanitizers working.
Change-Id: I53881cf2e83ea9e610e8ac86de40ec8c4cdd3bb1
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/control_loops/superstructure/arm/trajectory.cc b/y2023/control_loops/superstructure/arm/trajectory.cc
index d216467..88e3d1b 100644
--- a/y2023/control_loops/superstructure/arm/trajectory.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory.cc
@@ -974,8 +974,8 @@
}
// Pull us back to the previous point until we aren't saturated anymore.
- double saturation_goal_velocity;
- double saturation_goal_acceleration;
+ double saturation_goal_velocity = 0.0;
+ double saturation_goal_acceleration = 0.0;
while (step_size > 0.01) {
USaturationSearch(goal_(0), last_goal_(0), goal_(1), last_goal_(1),
saturation_fraction_along_path_, arm_K, X, *trajectory_,
diff --git a/y2023/control_loops/superstructure/arm/trajectory_plot.cc b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
index c56ced9..24bc8dd 100644
--- a/y2023/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
@@ -15,10 +15,10 @@
DEFINE_bool(plot, true, "If true, plot");
DEFINE_bool(plot_thetas, true, "If true, plot the angles");
-DEFINE_double(alpha0_max, 20.0, "Max acceleration on joint 0.");
-DEFINE_double(alpha1_max, 30.0, "Max acceleration on joint 1.");
-DEFINE_double(alpha2_max, 60.0, "Max acceleration on joint 2.");
-DEFINE_double(vmax_plan, 10.0, "Max voltage to plan.");
+DEFINE_double(alpha0_max, 15.0, "Max acceleration on joint 0.");
+DEFINE_double(alpha1_max, 10.0, "Max acceleration on joint 1.");
+DEFINE_double(alpha2_max, 90.0, "Max acceleration on joint 2.");
+DEFINE_double(vmax_plan, 9.5, "Max voltage to plan.");
DEFINE_double(vmax_battery, 12.0, "Max battery voltage.");
DEFINE_double(time, 2.0, "Simulation time.");
@@ -36,12 +36,20 @@
Eigen::Matrix<double, 2, 4> spline_params;
- spline_params << 0.30426338, 0.42813912, 0.64902386, 0.55127045, -1.73611082,
- -1.64478944, -1.04763868, -0.82624244;
+ spline_params << 3.227752818257, 3.032002509469, 3.131082488348,
+ 3.141592653590, 0.914286433787, 0.436747899287, 0.235917057271,
+ 0.000000000000;
LOG(INFO) << "Spline " << spline_params;
NSpline<4, 2> spline(spline_params);
CosSpline cos_spline(spline,
- {{0.0, 0.1}, {0.3, 0.1}, {0.7, 0.2}, {1.0, 0.2}});
+ {
+ CosSpline::AlphaTheta{.alpha = 0.000000000000,
+ .theta = 1.570796326795},
+ CosSpline::AlphaTheta{.alpha = 0.050000000000,
+ .theta = 1.570796326795},
+ CosSpline::AlphaTheta{.alpha = 1.000000000000,
+ .theta = 0.000000000000},
+ });
Path distance_spline(cos_spline, 100);
Trajectory trajectory(&dynamics, &hybrid_roll.plant(),
@@ -300,10 +308,14 @@
(::Eigen::Matrix<double, 2, 1>() << arm_X(0), arm_X(2)).finished(),
sim_dt);
roll.Correct((::Eigen::Matrix<double, 1, 1>() << roll_X(0)).finished());
+ bool disabled = false;
+ if (t > 0.40 && t < 0.46) {
+ disabled = true;
+ }
follower.Update(
(Eigen::Matrix<double, 9, 1>() << arm_ekf.X_hat(), roll.X_hat())
.finished(),
- false, sim_dt, FLAGS_vmax_plan, FLAGS_vmax_battery);
+ disabled, sim_dt, FLAGS_vmax_plan, FLAGS_vmax_battery);
const ::Eigen::Matrix<double, 3, 1> theta_t =
trajectory.ThetaT(follower.goal()(0));