Add 2023 arm design code
The C++ was better for design than the python...
Change-Id: Ic606a8a728527668b7335eb7a94cbfd69e272774
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Signed-off-by: Xander Yee <xander.yee@gmail.com>
diff --git a/frc971/control_loops/double_jointed_arm/dynamics.cc b/frc971/control_loops/double_jointed_arm/dynamics.cc
index b5b6c8d..2842ad4 100644
--- a/frc971/control_loops/double_jointed_arm/dynamics.cc
+++ b/frc971/control_loops/double_jointed_arm/dynamics.cc
@@ -9,29 +9,29 @@
Dynamics::Dynamics(ArmConstants arm_constants)
: arm_constants_(arm_constants),
- K3_((::Eigen::Matrix<double, 2, 2>() << arm_constants_.g1 *
+ K3_((::Eigen::Matrix<double, 2, 2>() << arm_constants_.g0 *
arm_constants_.Kt /
arm_constants_.resistance,
0.0, 0.0,
- arm_constants_.g2 * arm_constants_.num_distal_motors *
+ arm_constants_.g1 * arm_constants_.num_distal_motors *
arm_constants_.Kt / arm_constants_.resistance)
.finished()),
K3_inverse_(K3_.inverse()),
K4_((::Eigen::Matrix<double, 2, 2>()
- << arm_constants_.g1 * arm_constants_.g1 * arm_constants_.Kt /
+ << arm_constants_.g0 * arm_constants_.g0 * arm_constants_.Kt /
(arm_constants_.Kv * arm_constants_.resistance),
0.0, 0.0,
- arm_constants_.g2 * arm_constants_.g2 * arm_constants_.Kt *
+ arm_constants_.g1 * arm_constants_.g1 * arm_constants_.Kt *
arm_constants_.num_distal_motors /
(arm_constants_.Kv * arm_constants_.resistance))
.finished()),
- alpha_(arm_constants_.j1 +
- arm_constants_.r1 * arm_constants_.r1 * arm_constants_.m1 +
- arm_constants_.l1 * arm_constants_.l1 * arm_constants_.m2),
- beta_(arm_constants_.l1 * arm_constants_.r2 * arm_constants_.m2),
- gamma_(arm_constants_.j2 +
- arm_constants_.r2 * arm_constants_.r2 * arm_constants_.m2) {}
+ alpha_(arm_constants_.j0 +
+ arm_constants_.r0 * arm_constants_.r0 * arm_constants_.m0 +
+ arm_constants_.l0 * arm_constants_.l0 * arm_constants_.m1),
+ beta_(arm_constants_.l0 * arm_constants_.r1 * arm_constants_.m1),
+ gamma_(arm_constants_.j1 +
+ arm_constants_.r1 * arm_constants_.r1 * arm_constants_.m1) {}
} // namespace arm
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/double_jointed_arm/dynamics.h b/frc971/control_loops/double_jointed_arm/dynamics.h
index 1e934bc..924588b 100644
--- a/frc971/control_loops/double_jointed_arm/dynamics.h
+++ b/frc971/control_loops/double_jointed_arm/dynamics.h
@@ -12,26 +12,26 @@
namespace arm {
struct ArmConstants {
- // Below, 1 refers to the proximal joint, and 2 refers to the distal joint.
+ // Below, 0 refers to the proximal joint, and 1 refers to the distal joint.
// Length of the joints in meters.
+ double l0;
double l1;
- double l2;
// Mass of the joints in kilograms.
+ double m0;
double m1;
- double m2;
// Moment of inertia of the joints in kg m^2
+ double j0;
double j1;
- double j2;
// Radius of the center of mass of the joints in meters.
+ double r0;
double r1;
- double r2;
// Gear ratios for the two joints.
+ double g0;
double g1;
- double g2;
// motor constants.
double efficiency_tweak;
@@ -48,6 +48,8 @@
// This class captures the dynamics of our system. It doesn't actually need to
// store state yet, so everything can be constexpr and/or static.
+//
+// 0, 0 is straight up.
class Dynamics {
public:
Dynamics(ArmConstants arm_constants);
@@ -55,9 +57,10 @@
// K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
// These matricies are missing the velocity factor for K2[1, 0], and K2[0, 1].
// You probbaly want MatriciesForState.
- void NormilizedMatriciesForState(const ::Eigen::Matrix<double, 4, 1> &X,
- ::Eigen::Matrix<double, 2, 2> *K1_result,
- ::Eigen::Matrix<double, 2, 2> *K2_result) const {
+ void NormilizedMatriciesForState(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ ::Eigen::Matrix<double, 2, 2> *K1_result,
+ ::Eigen::Matrix<double, 2, 2> *K2_result) const {
const double angle = X(0, 0) - X(2, 0);
const double s = ::std::sin(angle);
const double c = ::std::cos(angle);
@@ -75,6 +78,50 @@
(*K2_result)(0, 1) *= X(3, 0);
}
+ // Calculates the joint torques as a function of the state and command.
+ const ::Eigen::Matrix<double, 2, 1> TorqueFromCommand(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ const ::Eigen::Matrix<double, 2, 1> velocity =
+ (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
+
+ return K3_ * U - K4_ * velocity;
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> CurrentFromTorque(
+ const ::Eigen::Matrix<double, 2, 1> &torque) {
+ return ::Eigen::DiagonalMatrix<double, 2>(
+ 1.0 / (arm_constants_.Kt * arm_constants_.g0),
+ 1.0 / (arm_constants_.Kt * arm_constants_.g1 *
+ arm_constants_.num_distal_motors)) *
+ torque;
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> CurrentFromCommand(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ return CurrentFromTorque(TorqueFromCommand(X, U));
+ }
+
+ // Computes the two joint torques given the state and the external force in
+ // x, y.
+ const ::Eigen::Matrix<double, 2, 1> TorqueFromForce(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &F) {
+ const ::Eigen::Matrix<double, 2, 1> L0(std::sin(X(0)) * arm_constants_.l0,
+ std::cos(X(0)) * arm_constants_.l0);
+ const ::Eigen::Matrix<double, 2, 1> L1(std::sin(X(2)) * arm_constants_.l1,
+ std::cos(X(2)) * arm_constants_.l1);
+
+ const Eigen::Matrix<double, 2, 1> Fn1 =
+ F - L0.normalized().dot(F) * L0.normalized();
+
+ const double torque1 = L0.x() * Fn1.y() - L0.y() * Fn1.x();
+ const double torque2 = L1.x() * F.y() - L1.y() * F.x();
+
+ return ::Eigen::Matrix<double, 2, 1>(torque1, torque2);
+ }
+
// TODO(austin): We may want a way to provide K1 and K2 to save CPU cycles.
// Calculates the acceleration given the current state and control input.
@@ -150,10 +197,10 @@
const ::Eigen::Matrix<double, 4, 1> &X) const {
const double accel_due_to_gravity = 9.8 * arm_constants_.efficiency_tweak;
return (::Eigen::Matrix<double, 2, 1>()
- << (arm_constants_.r1 * arm_constants_.m1 +
- arm_constants_.l1 * arm_constants_.m2) *
+ << (arm_constants_.r0 * arm_constants_.m0 +
+ arm_constants_.l0 * arm_constants_.m1) *
::std::sin(X(0)) * accel_due_to_gravity,
- arm_constants_.r2 * arm_constants_.m2 * ::std::sin(X(2)) *
+ arm_constants_.r1 * arm_constants_.m1 * ::std::sin(X(2)) *
accel_due_to_gravity)
.finished() *
(FLAGS_gravity ? 1.0 : 0.0);
diff --git a/frc971/control_loops/double_jointed_arm/test_constants.h b/frc971/control_loops/double_jointed_arm/test_constants.h
index ea1d3b7..4885d14 100644
--- a/frc971/control_loops/double_jointed_arm/test_constants.h
+++ b/frc971/control_loops/double_jointed_arm/test_constants.h
@@ -14,22 +14,22 @@
constexpr double kStallCurrent = 89.0;
constexpr ArmConstants kArmConstants = {
- .l1 = 46.25 * 0.0254,
- .l2 = 41.80 * 0.0254,
- .m1 = 9.34 / 2.2,
- .m2 = 9.77 / 2.2,
+ .l0 = 46.25 * 0.0254,
+ .l1 = 41.80 * 0.0254,
+ .m0 = 9.34 / 2.2,
+ .m1 = 9.77 / 2.2,
// Moment of inertia of the joints in kg m^2
- .j1 = 2957.05 * 0.0002932545454545454,
- .j2 = 2824.70 * 0.0002932545454545454,
+ .j0 = 2957.05 * 0.0002932545454545454,
+ .j1 = 2824.70 * 0.0002932545454545454,
// Radius of the center of mass of the joints in meters.
- .r1 = 21.64 * 0.0254,
- .r2 = 26.70 * 0.0254,
+ .r0 = 21.64 * 0.0254,
+ .r1 = 26.70 * 0.0254,
// Gear ratios for the two joints.
- .g1 = 140.0,
- .g2 = 90.0,
+ .g0 = 140.0,
+ .g1 = 90.0,
// MiniCIM motor constants.
.efficiency_tweak = kEfficiencyTweak,
diff --git a/y2018/constants.h b/y2018/constants.h
index 2a6f0ac..6adcb8a 100644
--- a/y2018/constants.h
+++ b/y2018/constants.h
@@ -51,7 +51,7 @@
}
static constexpr double kMaxProximalEncoderPulsesPerSecond() {
return control_loops::superstructure::arm::kArmConstants.free_speed /
- (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g1 /
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g0 /
kProximalEncoderRatio() * kProximalEncoderCountsPerRevolution();
}
static constexpr double kProximalPotRatio() { return (12.0 / 60.0); }
@@ -60,7 +60,7 @@
static constexpr double kDistalEncoderRatio() { return (12.0 / 60.0); }
static constexpr double kMaxDistalEncoderPulsesPerSecond() {
return control_loops::superstructure::arm::kArmConstants.free_speed /
- (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g2 /
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g1 /
kDistalEncoderRatio() * kProximalEncoderCountsPerRevolution();
}
static constexpr double kDistalPotRatio() {
diff --git a/y2018/control_loops/superstructure/arm/arm_constants.h b/y2018/control_loops/superstructure/arm/arm_constants.h
index 9ac7020..1697a8e 100644
--- a/y2018/control_loops/superstructure/arm/arm_constants.h
+++ b/y2018/control_loops/superstructure/arm/arm_constants.h
@@ -14,22 +14,22 @@
constexpr double kStallCurrent = 89.0;
constexpr ::frc971::control_loops::arm::ArmConstants kArmConstants = {
- .l1 = 46.25 * 0.0254,
- .l2 = 41.80 * 0.0254,
- .m1 = 9.34 / 2.2,
- .m2 = 9.77 / 2.2,
+ .l0 = 46.25 * 0.0254,
+ .l1 = 41.80 * 0.0254,
+ .m0 = 9.34 / 2.2,
+ .m1 = 9.77 / 2.2,
// Moment of inertia of the joints in kg m^2
- .j1 = 2957.05 * 0.0002932545454545454,
- .j2 = 2824.70 * 0.0002932545454545454,
+ .j0 = 2957.05 * 0.0002932545454545454,
+ .j1 = 2824.70 * 0.0002932545454545454,
// Radius of the center of mass of the joints in meters.
- .r1 = 21.64 * 0.0254,
- .r2 = 26.70 * 0.0254,
+ .r0 = 21.64 * 0.0254,
+ .r1 = 26.70 * 0.0254,
// Gear ratios for the two joints.
- .g1 = 140.0,
- .g2 = 90.0,
+ .g0 = 140.0,
+ .g1 = 90.0,
// MiniCIM motor constants.
.efficiency_tweak = kEfficiencyTweak,
diff --git a/y2023/constants.h b/y2023/constants.h
index b5e61a1..0475242 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -60,7 +60,7 @@
static constexpr double kProximalEncoderRatio() { return (15.0 / 95.0); }
static constexpr double kMaxProximalEncoderPulsesPerSecond() {
return control_loops::superstructure::arm::kArmConstants.free_speed /
- (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g1 /
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g0 /
kProximalEncoderRatio() * kProximalEncoderCountsPerRevolution();
}
static constexpr double kProximalPotRatio() {
@@ -76,8 +76,8 @@
static constexpr double kDistalEncoderRatio() { return (15.0 / 95.0); }
static constexpr double kMaxDistalEncoderPulsesPerSecond() {
return control_loops::superstructure::arm::kArmConstants.free_speed /
- (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g2 /
- kDistalEncoderRatio() * kDistalEncoderCountsPerRevolution();
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g1 /
+ kDistalEncoderRatio() * kProximalEncoderCountsPerRevolution();
}
static constexpr double kDistalPotRatio() {
return (24.0 / 36.0) * (18.0 / 66.0) * (15.0 / 95.0);
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index dc52f02..c125fac 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -62,3 +62,19 @@
"//frc971/control_loops/double_jointed_arm:dynamics",
],
)
+
+cc_binary(
+ name = "arm_design",
+ srcs = [
+ "arm_design.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":arm_constants",
+ "//aos:init",
+ "//frc971/analysis:in_process_plotter",
+ "//frc971/control_loops:dlqr",
+ "//frc971/control_loops:jacobian",
+ "//frc971/control_loops/double_jointed_arm:dynamics",
+ ],
+)
diff --git a/y2023/control_loops/superstructure/arm/arm_constants.h b/y2023/control_loops/superstructure/arm/arm_constants.h
index 5611eaa..39e146f 100644
--- a/y2023/control_loops/superstructure/arm/arm_constants.h
+++ b/y2023/control_loops/superstructure/arm/arm_constants.h
@@ -14,23 +14,23 @@
constexpr double kStallCurrent = 257.0;
constexpr ::frc971::control_loops::arm::ArmConstants kArmConstants = {
- .l1 = 20 * 0.0254,
- .l2 = 38 * 0.0254,
+ .l0 = 20 * 0.0254,
+ .l1 = 38 * 0.0254,
- .m1 = 9.34 / 2.2,
- .m2 = 9.77 / 2.2,
+ .m0 = 9.34 / 2.2,
+ .m1 = 9.77 / 2.2,
// Moment of inertia of the joints in kg m^2
- .j1 = 2957.05 * 0.0002932545454545454,
- .j2 = 2824.70 * 0.0002932545454545454,
+ .j0 = 2957.05 * 0.0002932545454545454,
+ .j1 = 2824.70 * 0.0002932545454545454,
// Radius of the center of mass of the joints in meters.
- .r1 = 21.64 * 0.0254,
- .r2 = 26.70 * 0.0254,
+ .r0 = 21.64 * 0.0254,
+ .r1 = 26.70 * 0.0254,
// Gear ratios for the two joints.
- .g1 = 55.0,
- .g2 = 106.0,
+ .g0 = 55.0,
+ .g1 = 106.0,
// Falcon motor constants.
.efficiency_tweak = kEfficiencyTweak,
diff --git a/y2023/control_loops/superstructure/arm/arm_design.cc b/y2023/control_loops/superstructure/arm/arm_design.cc
new file mode 100644
index 0000000..99ffa2c
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/arm_design.cc
@@ -0,0 +1,193 @@
+#include "aos/init.h"
+#include "frc971/analysis/in_process_plotter.h"
+#include "frc971/control_loops/dlqr.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/jacobian.h"
+#include "y2023/control_loops/superstructure/arm/arm_constants.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(fx, 0.0, "X force");
+DEFINE_double(fy, 0.0, "y force");
+
+DEFINE_double(start0, 0.0, "starting position on proximal");
+DEFINE_double(start1, 0.0, "starting position on distal");
+DEFINE_double(goal0, 0.0, "goal position on proximal");
+DEFINE_double(goal1, 0.0, "goal position on distal");
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+int Main() {
+ frc971::analysis::Plotter plotter;
+
+ frc971::control_loops::arm::Dynamics dynamics(kArmConstants);
+
+ ::Eigen::Matrix<double, 4, 1> X = ::Eigen::Matrix<double, 4, 1>::Zero();
+ ::Eigen::Matrix<double, 4, 1> goal = ::Eigen::Matrix<double, 4, 1>::Zero();
+ goal(0, 0) = FLAGS_goal0;
+ goal(1, 0) = 0;
+ goal(2, 0) = FLAGS_goal1;
+ goal(3, 0) = 0;
+
+ X(0, 0) = FLAGS_start0;
+ X(1, 0) = 0;
+ X(2, 0) = FLAGS_start1;
+ X(3, 0) = 0;
+ ::Eigen::Matrix<double, 2, 1> U = ::Eigen::Matrix<double, 2, 1>::Zero();
+
+ constexpr double kDt = 0.00505;
+
+ std::vector<double> t;
+ std::vector<double> x0;
+ std::vector<double> x1;
+ std::vector<double> x2;
+ std::vector<double> x3;
+ std::vector<double> u0;
+ std::vector<double> u1;
+
+ std::vector<double> current0;
+ std::vector<double> current1;
+ std::vector<double> torque0;
+ std::vector<double> torque1;
+
+ const double kProximalPosLQR = FLAGS_lqr_proximal_pos;
+ const double kProximalVelLQR = FLAGS_lqr_proximal_vel;
+ const double kDistalPosLQR = FLAGS_lqr_distal_pos;
+ const double kDistalVelLQR = FLAGS_lqr_distal_vel;
+ const ::Eigen::DiagonalMatrix<double, 4> Q =
+ (::Eigen::DiagonalMatrix<double, 4>().diagonal()
+ << 1.0 / ::std::pow(kProximalPosLQR, 2),
+ 1.0 / ::std::pow(kProximalVelLQR, 2), 1.0 / ::std::pow(kDistalPosLQR, 2),
+ 1.0 / ::std::pow(kDistalVelLQR, 2))
+ .finished()
+ .asDiagonal();
+
+ const ::Eigen::DiagonalMatrix<double, 2> R =
+ (::Eigen::DiagonalMatrix<double, 2>().diagonal()
+ << 1.0 / ::std::pow(12.0, 2),
+ 1.0 / ::std::pow(12.0, 2))
+ .finished()
+ .asDiagonal();
+
+ {
+ const ::Eigen::Matrix<double, 2, 1> torque =
+ dynamics
+ .TorqueFromForce(X,
+ ::Eigen::Matrix<double, 2, 1>(FLAGS_fx, FLAGS_fy))
+ .transpose();
+ LOG(INFO) << "Torque (N m): " << torque.transpose();
+ const ::Eigen::Matrix<double, 2, 1> current =
+ dynamics.CurrentFromTorque(torque);
+
+ LOG(INFO) << "Current (Amps): " << current.transpose();
+
+ ::Eigen::Matrix<double, 2, 1> battery_current;
+ battery_current(0) =
+ current(0) * current(0) * kArmConstants.resistance / 12.0;
+ battery_current(1) =
+ current(1) * current(1) * kArmConstants.resistance / 12.0;
+
+ LOG(INFO) << "Battery current (Amps): " << battery_current.transpose();
+ }
+
+ ::Eigen::Matrix<double, 2, 4> last_K = ::Eigen::Matrix<double, 2, 4>::Zero();
+ for (int i = 0; i < 400; ++i) {
+ t.push_back(i * kDt);
+ x0.push_back(X(0));
+ x1.push_back(X(1));
+ x2.push_back(X(2));
+ x3.push_back(X(3));
+
+ const auto x_blocked = X.block<4, 1>(0, 0);
+
+ const ::Eigen::Matrix<double, 4, 4> final_A =
+ ::frc971::control_loops::NumericalJacobianX<4, 2>(
+ [dynamics](const auto &x_blocked, const auto &U, double kDt) {
+ return dynamics.UnboundedDiscreteDynamics(x_blocked, U, kDt);
+ },
+ x_blocked, U, 0.00505);
+ const ::Eigen::Matrix<double, 4, 2> final_B =
+ ::frc971::control_loops::NumericalJacobianU<4, 2>(
+ [dynamics](const auto &x_blocked, const auto &U, double kDt) {
+ return dynamics.UnboundedDiscreteDynamics(x_blocked, U, kDt);
+ },
+ x_blocked, U, 0.00505);
+
+ ::Eigen::Matrix<double, 2, 1> U_ff =
+ dynamics.FF_U(x_blocked, ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero());
+
+ ::Eigen::Matrix<double, 4, 4> S;
+ ::Eigen::Matrix<double, 2, 4> K;
+ if (::frc971::controls::dlqr<4, 2>(final_A, final_B, Q, R, &K, &S) == 0) {
+ ::Eigen::EigenSolver<::Eigen::Matrix<double, 4, 4>> eigensolver(
+ final_A - final_B * K);
+
+ last_K = K;
+ } else {
+ LOG(INFO) << "Failed to solve for K at " << i;
+ }
+ U = U_ff + last_K * (goal - X);
+ if (std::abs(U(0)) > 12.0) {
+ U /= std::abs(U(0)) / 12.0;
+ }
+ if (std::abs(U(1)) > 12.0) {
+ U /= std::abs(U(1)) / 12.0;
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> torque =
+ dynamics.TorqueFromCommand(X, U);
+ const ::Eigen::Matrix<double, 2, 1> current_per_motor =
+ dynamics.CurrentFromCommand(X, U);
+
+ current0.push_back(current_per_motor(0));
+ current1.push_back(current_per_motor(1));
+ torque0.push_back(torque(0));
+ torque1.push_back(torque(1));
+
+ u0.push_back(U(0));
+ u1.push_back(U(1));
+
+ X = dynamics.UnboundedDiscreteDynamics(X, U, kDt);
+ }
+
+ plotter.Title("Arm motion");
+ plotter.AddFigure("State");
+ plotter.AddLine(t, x0, "X 0");
+ plotter.AddLine(t, x1, "X 1");
+ plotter.AddLine(t, x2, "X 2");
+ plotter.AddLine(t, x3, "X 3");
+
+ plotter.AddLine(t, u0, "U 0");
+ plotter.AddLine(t, u1, "U 1");
+ plotter.Publish();
+
+ plotter.AddFigure("Command");
+ plotter.AddLine(t, u0, "U 0");
+ plotter.AddLine(t, u1, "U 1");
+
+ plotter.AddLine(t, current0, "current 0");
+ plotter.AddLine(t, current1, "current 1");
+ plotter.AddLine(t, torque0, "torque 0");
+ plotter.AddLine(t, torque1, "torque 1");
+ plotter.Publish();
+
+ plotter.Spin();
+
+ return 0;
+}
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+ return y2023::control_loops::superstructure::arm::Main();
+}