Tune arm loops
Slow it waaay down and add more knobs to plot the poles.
Change-Id: I488ab7b0b999b3bb032c39523ffc8193363e7003
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/control_loops/python/roll.py b/y2023/control_loops/python/roll.py
index 7e342b2..ce0fac0 100644
--- a/y2023/control_loops/python/roll.py
+++ b/y2023/control_loops/python/roll.py
@@ -39,6 +39,7 @@
R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
angular_system.PlotKick(kRoll, R)
angular_system.PlotMotion(kRoll, R)
+ return
# Write the generated constants out to a file.
if len(argv) != 5:
diff --git a/y2023/control_loops/python/wrist.py b/y2023/control_loops/python/wrist.py
index 26d3a16..1970c5b 100644
--- a/y2023/control_loops/python/wrist.py
+++ b/y2023/control_loops/python/wrist.py
@@ -36,8 +36,9 @@
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
- angular_system.PlotKick(kIntake, R)
- angular_system.PlotMotion(kIntake, R)
+ angular_system.PlotKick(kWrist, R)
+ angular_system.PlotMotion(kWrist, R)
+ return
# Write the generated constants out to a file.
if len(argv) != 5:
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index 032c654..ce226d1 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -116,6 +116,7 @@
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
":arm_constants",
+ ":generated_graph",
":trajectory",
"//frc971/analysis:in_process_plotter",
"//frc971/control_loops:binomial",
diff --git a/y2023/control_loops/superstructure/arm/trajectory.cc b/y2023/control_loops/superstructure/arm/trajectory.cc
index ad6720f..565f497 100644
--- a/y2023/control_loops/superstructure/arm/trajectory.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory.cc
@@ -8,10 +8,10 @@
#include "frc971/control_loops/runge_kutta.h"
#include "gflags/gflags.h"
-DEFINE_double(lqr_proximal_pos, 0.15, "Position LQR gain");
-DEFINE_double(lqr_proximal_vel, 4.0, "Velocity LQR gain");
-DEFINE_double(lqr_distal_pos, 0.20, "Position LQR gain");
-DEFINE_double(lqr_distal_vel, 4.0, "Velocity LQR gain");
+DEFINE_double(lqr_proximal_pos, 0.5, "Position LQR gain");
+DEFINE_double(lqr_proximal_vel, 5, "Velocity LQR gain");
+DEFINE_double(lqr_distal_pos, 0.5, "Position LQR gain");
+DEFINE_double(lqr_distal_vel, 5, "Velocity LQR gain");
namespace y2023 {
namespace control_loops {
@@ -347,7 +347,7 @@
const ::Eigen::Matrix<double, 3, 1> U =
k_constant + k_scalar * voltage_accel;
- VLOG(1) << "Trying, U is " << U.transpose() << ", accel "
+ VLOG(2) << "Trying, U is " << U.transpose() << ", accel "
<< voltage_accel;
if ((U.array().abs() <= plan_vmax + 1e-6).all()) {
min_goal_acceleration = std::min(voltage_accel, min_goal_acceleration);
@@ -377,7 +377,7 @@
if (min_goal_acceleration < constraint_goal_acceleration &&
constraint_goal_acceleration < max_goal_acceleration) {
- VLOG(1)
+ VLOG(2)
<< "Solved valid accel at distance " << goal_distance << ", voltage "
<< (k_constant + k_scalar * constraint_goal_acceleration).transpose()
<< ", accel " << constraint_goal_acceleration
@@ -394,7 +394,7 @@
return constraint_goal_acceleration;
}
- VLOG(1) << "Solved valid accel at distance " << goal_distance << ", voltage "
+ VLOG(2) << "Solved valid accel at distance " << goal_distance << ", voltage "
<< (k_constant + k_scalar * goal_acceleration).transpose()
<< ", decel limits [" << min_goal_acceleration << ", "
<< max_goal_acceleration << "], picked " << goal_acceleration
@@ -480,7 +480,7 @@
// TODO(austin): This doesn't always give a valid solution. It really
// should. Figure out why.
- VLOG(1) << "Trying, U is " << U.transpose() << ", accel "
+ VLOG(2) << "Trying, U is " << U.transpose() << ", accel "
<< voltage_accel;
if ((U.array().abs() <= plan_vmax + 1e-6).all()) {
min_goal_acceleration = std::min(-voltage_accel, min_goal_acceleration);
@@ -502,7 +502,7 @@
if (min_goal_acceleration < constraint_goal_acceleration &&
constraint_goal_acceleration < max_goal_acceleration) {
- VLOG(1)
+ VLOG(2)
<< "Solved valid decel at distance " << goal_distance << ", voltage "
<< (k_constant - k_scalar * constraint_goal_acceleration).transpose()
<< ", decel " << min_goal_acceleration
@@ -525,7 +525,7 @@
? max_goal_acceleration
: min_goal_acceleration;
- VLOG(1) << "Solved valid decel at distance " << goal_distance << ", voltage "
+ VLOG(2) << "Solved valid decel at distance " << goal_distance << ", voltage "
<< (k_constant - k_scalar * goal_acceleration).transpose()
<< ", decel limits [" << min_goal_acceleration << ", "
<< max_goal_acceleration << "], picked " << goal_acceleration
@@ -703,8 +703,12 @@
::Eigen::Matrix<double, 4, 4> S;
::Eigen::Matrix<double, 2, 4> sub_K;
if (::frc971::controls::dlqr<4, 2>(final_A, final_B, Q, R, &sub_K, &S) == 0) {
- ::Eigen::EigenSolver<::Eigen::Matrix<double, 4, 4>> eigensolver(
- final_A - final_B * sub_K);
+ if (VLOG_IS_ON(1)) {
+ ::Eigen::EigenSolver<::Eigen::Matrix<double, 4, 4>> eigensolver(
+ final_A - final_B * sub_K);
+ LOG(INFO) << eigensolver.eigenvalues().transpose();
+ LOG(INFO) << sub_K;
+ }
::Eigen::Matrix<double, 2, 6> K;
K.setZero();
diff --git a/y2023/control_loops/superstructure/arm/trajectory_plot.cc b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
index 2b05a30..c56ced9 100644
--- a/y2023/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
@@ -6,6 +6,7 @@
#include "frc971/control_loops/fixed_quadrature.h"
#include "gflags/gflags.h"
#include "y2023/control_loops/superstructure/arm/arm_constants.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"
#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
@@ -14,6 +15,13 @@
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(vmax_battery, 12.0, "Max battery voltage.");
+DEFINE_double(time, 2.0, "Simulation time.");
+
namespace y2023 {
namespace control_loops {
namespace superstructure {
@@ -26,12 +34,12 @@
HybridKalman<3, 1, 1>>
hybrid_roll = superstructure::roll::MakeIntegralHybridRollLoop();
- Eigen::Matrix<double, 3, 4> spline_params;
+ Eigen::Matrix<double, 2, 4> spline_params;
spline_params << 0.30426338, 0.42813912, 0.64902386, 0.55127045, -1.73611082,
- -1.64478944, -1.44763868, -1.22624244, 0.1, 0.2, 0.3, 0.5;
+ -1.64478944, -1.04763868, -0.82624244;
LOG(INFO) << "Spline " << spline_params;
- NSpline<4, 2> spline(spline_params.block<2, 4>(0, 0));
+ 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}});
Path distance_spline(cos_spline, 100);
@@ -39,17 +47,17 @@
Trajectory trajectory(&dynamics, &hybrid_roll.plant(),
std::make_unique<Path>(cos_spline), 0.001);
- constexpr double kAlpha0Max = 20.0;
- constexpr double kAlpha1Max = 30.0;
- constexpr double kAlpha2Max = 40.0;
- constexpr double vmax = 9.0;
constexpr double sim_dt = 0.00505;
+ LOG(INFO) << "Planning with kAlpha0Max=" << FLAGS_alpha0_max
+ << ", kAlpha1Max=" << FLAGS_alpha1_max
+ << ", kAlpha2Max=" << FLAGS_alpha2_max;
+
const ::Eigen::DiagonalMatrix<double, 3> alpha_unitizer(
- (::Eigen::DiagonalMatrix<double, 3>().diagonal() << (1.0 / kAlpha0Max),
- (1.0 / kAlpha1Max), (1.0 / kAlpha2Max))
+ (::Eigen::DiagonalMatrix<double, 3>().diagonal() << (1.0 / FLAGS_alpha0_max),
+ (1.0 / FLAGS_alpha1_max), (1.0 / FLAGS_alpha2_max))
.finished());
- trajectory.OptimizeTrajectory(alpha_unitizer, vmax);
+ trajectory.OptimizeTrajectory(alpha_unitizer, FLAGS_vmax_plan);
const ::std::vector<double> distance_array = trajectory.DistanceArray();
@@ -286,7 +294,7 @@
::std::cout << "Really stabilized P: " << arm_ekf.P_converged()
<< ::std::endl;
- while (t < 1.2) {
+ while (t < FLAGS_time) {
t_array.push_back(t);
arm_ekf.Correct(
(::Eigen::Matrix<double, 2, 1>() << arm_X(0), arm_X(2)).finished(),
@@ -295,7 +303,7 @@
follower.Update(
(Eigen::Matrix<double, 9, 1>() << arm_ekf.X_hat(), roll.X_hat())
.finished(),
- false, sim_dt, vmax, 12.0);
+ false, sim_dt, FLAGS_vmax_plan, FLAGS_vmax_battery);
const ::Eigen::Matrix<double, 3, 1> theta_t =
trajectory.ThetaT(follower.goal()(0));