Move 2018 arm code to frc971 and make dynamics adjustable
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I16e7bbb0d293aca461abc931f560fb9066b19aaf
diff --git a/frc971/control_loops/double_jointed_arm/BUILD b/frc971/control_loops/double_jointed_arm/BUILD
new file mode 100644
index 0000000..c7fcefc
--- /dev/null
+++ b/frc971/control_loops/double_jointed_arm/BUILD
@@ -0,0 +1,121 @@
+cc_library(
+ name = "trajectory",
+ srcs = [
+ "trajectory.cc",
+ ],
+ hdrs = [
+ "trajectory.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":dynamics",
+ "//aos/logging",
+ "//frc971/control_loops:dlqr",
+ "//frc971/control_loops:jacobian",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_test(
+ name = "trajectory_test",
+ srcs = [
+ "trajectory_test.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":demo_path",
+ ":dynamics",
+ ":ekf",
+ ":test_constants",
+ ":trajectory",
+ "//aos/testing:googletest",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "dynamics",
+ srcs = [
+ "dynamics.cc",
+ ],
+ hdrs = [
+ "dynamics.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:runge_kutta",
+ "@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "demo_path",
+ srcs = [
+ "demo_path.cc",
+ ],
+ hdrs = ["demo_path.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [":trajectory"],
+)
+
+cc_test(
+ name = "dynamics_test",
+ srcs = [
+ "dynamics_test.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":dynamics",
+ ":test_constants",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "ekf",
+ srcs = [
+ "ekf.cc",
+ ],
+ hdrs = [
+ "ekf.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":dynamics",
+ "//frc971/control_loops:jacobian",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "graph",
+ srcs = ["graph.cc"],
+ hdrs = ["graph.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+cc_test(
+ name = "graph_test",
+ srcs = ["graph_test.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":graph",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "test_constants",
+ hdrs = ["test_constants.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":dynamics",
+ ],
+)
diff --git a/y2018/control_loops/superstructure/arm/demo_path.cc b/frc971/control_loops/double_jointed_arm/demo_path.cc
similarity index 98%
rename from y2018/control_loops/superstructure/arm/demo_path.cc
rename to frc971/control_loops/double_jointed_arm/demo_path.cc
index 03856c9..8d549d1 100644
--- a/y2018/control_loops/superstructure/arm/demo_path.cc
+++ b/frc971/control_loops/double_jointed_arm/demo_path.cc
@@ -1,12 +1,11 @@
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
+#include "frc971/control_loops/double_jointed_arm/demo_path.h"
#include <array>
#include <initializer_list>
#include <memory>
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
::std::vector<::std::array<double, 6>> FlipPath(
@@ -25,7 +24,6 @@
return result;
}
-
::std::unique_ptr<Path> MakeDemoPath() {
return ::std::unique_ptr<Path>(new Path(FlipPath(
{{{1.3583511559969876, 0.99753029519739866, 0.63708920330895369,
@@ -213,6 +211,5 @@
}
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
diff --git a/frc971/control_loops/double_jointed_arm/demo_path.h b/frc971/control_loops/double_jointed_arm/demo_path.h
new file mode 100644
index 0000000..9a9d393
--- /dev/null
+++ b/frc971/control_loops/double_jointed_arm/demo_path.h
@@ -0,0 +1,19 @@
+#ifndef FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DEMO_PATH_H_
+#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DEMO_PATH_H_
+
+#include <memory>
+
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace arm {
+
+::std::unique_ptr<Path> MakeDemoPath();
+::std::unique_ptr<Path> MakeReversedDemoPath();
+
+} // namespace arm
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DEMO_PATH_H_
diff --git a/frc971/control_loops/double_jointed_arm/dynamics.cc b/frc971/control_loops/double_jointed_arm/dynamics.cc
new file mode 100644
index 0000000..b5b6c8d
--- /dev/null
+++ b/frc971/control_loops/double_jointed_arm/dynamics.cc
@@ -0,0 +1,37 @@
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+
+DEFINE_bool(gravity, true, "If true, enable gravity.");
+
+namespace frc971 {
+namespace control_loops {
+namespace arm {
+
+Dynamics::Dynamics(ArmConstants arm_constants)
+ : arm_constants_(arm_constants),
+
+ K3_((::Eigen::Matrix<double, 2, 2>() << arm_constants_.g1 *
+ arm_constants_.Kt /
+ arm_constants_.resistance,
+ 0.0, 0.0,
+ arm_constants_.g2 * 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_.Kv * arm_constants_.resistance),
+ 0.0, 0.0,
+ arm_constants_.g2 * arm_constants_.g2 * 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) {}
+} // 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
new file mode 100644
index 0000000..1e934bc
--- /dev/null
+++ b/frc971/control_loops/double_jointed_arm/dynamics.h
@@ -0,0 +1,195 @@
+#ifndef FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DYNAMICS_H_
+#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DYNAMICS_H_
+
+#include "Eigen/Dense"
+#include "frc971/control_loops/runge_kutta.h"
+#include "gflags/gflags.h"
+
+DECLARE_bool(gravity);
+
+namespace frc971 {
+namespace control_loops {
+namespace arm {
+
+struct ArmConstants {
+ // Below, 1 refers to the proximal joint, and 2 refers to the distal joint.
+ // Length of the joints in meters.
+ double l1;
+ double l2;
+
+ // Mass of the joints in kilograms.
+ double m1;
+ double m2;
+
+ // Moment of inertia of the joints in kg m^2
+ double j1;
+ double j2;
+
+ // Radius of the center of mass of the joints in meters.
+ double r1;
+ double r2;
+
+ // Gear ratios for the two joints.
+ double g1;
+ double g2;
+
+ // motor constants.
+ double efficiency_tweak;
+ double stall_torque;
+ double free_speed;
+ double stall_current;
+ double resistance;
+ double Kv;
+ double Kt;
+
+ // Number of motors on the distal joint.
+ double num_distal_motors;
+};
+
+// 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.
+class Dynamics {
+ public:
+ Dynamics(ArmConstants arm_constants);
+ // Generates K1-2 for the arm ODE.
+ // 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 {
+ const double angle = X(0, 0) - X(2, 0);
+ const double s = ::std::sin(angle);
+ const double c = ::std::cos(angle);
+ *K1_result << alpha_, c * beta_, c * beta_, gamma_;
+ *K2_result << 0.0, s * beta_, -s * beta_, 0.0;
+ }
+
+ // Generates K1-2 for the arm ODE.
+ // K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
+ void MatriciesForState(const ::Eigen::Matrix<double, 4, 1> &X,
+ ::Eigen::Matrix<double, 2, 2> *K1_result,
+ ::Eigen::Matrix<double, 2, 2> *K2_result) const {
+ NormilizedMatriciesForState(X, K1_result, K2_result);
+ (*K2_result)(1, 0) *= X(1, 0);
+ (*K2_result)(0, 1) *= X(3, 0);
+ }
+
+ // 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.
+ const ::Eigen::Matrix<double, 4, 1> Acceleration(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) const {
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2;
+
+ MatriciesForState(X, &K1, &K2);
+
+ const ::Eigen::Matrix<double, 2, 1> velocity =
+ (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
+
+ const ::Eigen::Matrix<double, 2, 1> torque = K3_ * U - K4_ * velocity;
+ const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
+
+ const ::Eigen::Matrix<double, 2, 1> accel =
+ K1.inverse() * (torque + gravity_torque - K2 * velocity);
+
+ return (::Eigen::Matrix<double, 4, 1>() << X(1, 0), accel(0, 0), X(3, 0),
+ accel(1, 0))
+ .finished();
+ }
+
+ // Calculates the acceleration given the current augmented kalman filter state
+ // and control input.
+ const ::Eigen::Matrix<double, 6, 1> EKFAcceleration(
+ const ::Eigen::Matrix<double, 6, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) const {
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2;
+
+ MatriciesForState(X.block<4, 1>(0, 0), &K1, &K2);
+
+ const ::Eigen::Matrix<double, 2, 1> velocity =
+ (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
+
+ const ::Eigen::Matrix<double, 2, 1> torque =
+ K3_ *
+ (U +
+ (::Eigen::Matrix<double, 2, 1>() << X(4, 0), X(5, 0)).finished()) -
+ K4_ * velocity;
+ const ::Eigen::Matrix<double, 2, 1> gravity_torque =
+ GravityTorque(X.block<4, 1>(0, 0));
+
+ const ::Eigen::Matrix<double, 2, 1> accel =
+ K1.inverse() * (torque + gravity_torque - K2 * velocity);
+
+ return (::Eigen::Matrix<double, 6, 1>() << X(1, 0), accel(0, 0), X(3, 0),
+ accel(1, 0), 0.0, 0.0)
+ .finished();
+ }
+
+ // Calculates the voltage required to follow the trajectory. This requires
+ // knowing the current state, desired angular velocity and acceleration.
+ const ::Eigen::Matrix<double, 2, 1> FF_U(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &omega_t,
+ const ::Eigen::Matrix<double, 2, 1> &alpha_t) const {
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2;
+
+ MatriciesForState(X, &K1, &K2);
+
+ const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
+
+ return K3_inverse_ *
+ (K1 * alpha_t + K2 * omega_t + K4_ * omega_t - gravity_torque);
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> GravityTorque(
+ 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) *
+ ::std::sin(X(0)) * accel_due_to_gravity,
+ arm_constants_.r2 * arm_constants_.m2 * ::std::sin(X(2)) *
+ accel_due_to_gravity)
+ .finished() *
+ (FLAGS_gravity ? 1.0 : 0.0);
+ }
+
+ const ::Eigen::Matrix<double, 4, 1> UnboundedDiscreteDynamics(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U, double dt) const {
+ return ::frc971::control_loops::RungeKuttaU(
+ [this](const auto &X, const auto &U) { return Acceleration(X, U); }, X,
+ U, dt);
+ }
+
+ const ::Eigen::Matrix<double, 6, 1> UnboundedEKFDiscreteDynamics(
+ const ::Eigen::Matrix<double, 6, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U, double dt) const {
+ return ::frc971::control_loops::RungeKuttaU(
+ [this](const auto &X, const auto &U) { return EKFAcceleration(X, U); },
+ X, U, dt);
+ }
+
+ const ArmConstants arm_constants_;
+
+ // K3, K4 matricies described above.
+ const ::Eigen::Matrix<double, 2, 2> K3_;
+ const ::Eigen::Matrix<double, 2, 2> K3_inverse_;
+ const ::Eigen::Matrix<double, 2, 2> K4_;
+
+ private:
+ const double alpha_;
+ const double beta_;
+ const double gamma_;
+};
+
+} // namespace arm
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DYNAMICS_H_
diff --git a/frc971/control_loops/double_jointed_arm/dynamics_test.cc b/frc971/control_loops/double_jointed_arm/dynamics_test.cc
new file mode 100644
index 0000000..6e12f75
--- /dev/null
+++ b/frc971/control_loops/double_jointed_arm/dynamics_test.cc
@@ -0,0 +1,53 @@
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/test_constants.h"
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace arm {
+namespace testing {
+
+// Tests that zero inputs result in no acceleration and no motion.
+// This isn't all that rigerous, but it's a good start.
+TEST(DynamicsTest, Acceleration) {
+ Dynamics dynamics(kArmConstants);
+
+ EXPECT_TRUE(dynamics
+ .Acceleration(::Eigen::Matrix<double, 4, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero())
+ .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
+
+ EXPECT_TRUE(
+ dynamics
+ .UnboundedDiscreteDynamics(::Eigen::Matrix<double, 4, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero(), 0.1)
+ .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
+
+ const ::Eigen::Matrix<double, 4, 1> X =
+ (::Eigen::Matrix<double, 4, 1>() << M_PI / 2.0, 0.0, 0.0, 0.0).finished();
+
+ ::std::cout << dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero())
+ << ::std::endl;
+
+ ::std::cout << dynamics.UnboundedDiscreteDynamics(
+ X,
+ dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero()),
+ 0.01)
+ << ::std::endl;
+
+ EXPECT_TRUE(dynamics
+ .UnboundedDiscreteDynamics(
+ X,
+ dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero()),
+ 0.01)
+ .isApprox(X));
+}
+
+} // namespace testing
+} // namespace arm
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2018/control_loops/superstructure/arm/ekf.cc b/frc971/control_loops/double_jointed_arm/ekf.cc
similarity index 87%
rename from y2018/control_loops/superstructure/arm/ekf.cc
rename to frc971/control_loops/double_jointed_arm/ekf.cc
index 48a71f4..a70e4a6 100644
--- a/y2018/control_loops/superstructure/arm/ekf.cc
+++ b/frc971/control_loops/double_jointed_arm/ekf.cc
@@ -1,19 +1,18 @@
-#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
-#include "Eigen/Dense"
#include <iostream>
+#include "Eigen/Dense"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/jacobian.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
DEFINE_double(proximal_voltage_error_uncertainty, 8.0,
"Proximal joint voltage error uncertainty.");
DEFINE_double(distal_voltage_error_uncertainty, 2.0,
"Distal joint voltage error uncertainty.");
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
namespace {
@@ -29,7 +28,7 @@
.asDiagonal());
} // namespace
-EKF::EKF() {
+EKF::EKF(const Dynamics *dynamics) : dynamics_(dynamics) {
X_hat_.setZero();
Q_covariance =
((::Eigen::DiagonalMatrix<double, 6>().diagonal() << ::std::pow(0.1, 2),
@@ -69,9 +68,12 @@
void EKF::Predict(const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
const ::Eigen::Matrix<double, 6, 6> A =
::frc971::control_loops::NumericalJacobianX<6, 2>(
- Dynamics::UnboundedEKFDiscreteDynamics, X_hat_, U, dt);
+ [this](const auto &X_hat_, const auto &U, double dt) {
+ return dynamics_->UnboundedEKFDiscreteDynamics(X_hat_, U, dt);
+ },
+ X_hat_, U, dt);
- X_hat_ = Dynamics::UnboundedEKFDiscreteDynamics(X_hat_, U, dt);
+ X_hat_ = dynamics_->UnboundedEKFDiscreteDynamics(X_hat_, U, dt);
P_ = A * P_ * A.transpose() + Q_covariance;
}
@@ -83,8 +85,8 @@
.asDiagonal());
// H is the jacobian of the h(x) measurement prediction function
const ::Eigen::Matrix<double, 2, 6> H_jacobian =
- (::Eigen::Matrix<double, 2, 6>() << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 1.0, 0.0, 0.0, 0.0)
+ (::Eigen::Matrix<double, 2, 6>() << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 1.0, 0.0, 0.0, 0.0)
.finished();
// Update step Measurement residual error of proximal and distal joint
@@ -106,6 +108,5 @@
}
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
diff --git a/y2018/control_loops/superstructure/arm/ekf.h b/frc971/control_loops/double_jointed_arm/ekf.h
similarity index 81%
rename from y2018/control_loops/superstructure/arm/ekf.h
rename to frc971/control_loops/double_jointed_arm/ekf.h
index 9ae8b47..d969e82 100644
--- a/y2018/control_loops/superstructure/arm/ekf.h
+++ b/frc971/control_loops/double_jointed_arm/ekf.h
@@ -1,11 +1,11 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
+#ifndef FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_EKF_H_
+#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_EKF_H_
#include "Eigen/Dense"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
// An extended kalman filter for the Arm.
@@ -13,7 +13,7 @@
// [theta0, omega0, theta1, omega1, voltage error0, voltage error1]
class EKF {
public:
- EKF();
+ EKF(const Dynamics *dynamics);
// Resets the internal state back to X. Resets the torque disturbance to 0.
void Reset(const ::Eigen::Matrix<double, 4, 1> &X);
@@ -43,11 +43,12 @@
::Eigen::Matrix<double, 6, 6> P_half_converged_;
::Eigen::Matrix<double, 6, 6> P_converged_;
::Eigen::Matrix<double, 6, 6> P_reset_;
+
+ const Dynamics *dynamics_;
};
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_EKF_H_
diff --git a/y2018/control_loops/superstructure/arm/graph.cc b/frc971/control_loops/double_jointed_arm/graph.cc
similarity index 92%
rename from y2018/control_loops/superstructure/arm/graph.cc
rename to frc971/control_loops/double_jointed_arm/graph.cc
index ad6f982..b5a70ab 100644
--- a/y2018/control_loops/superstructure/arm/graph.cc
+++ b/frc971/control_loops/double_jointed_arm/graph.cc
@@ -1,11 +1,10 @@
-#include "y2018/control_loops/superstructure/arm/graph.h"
+#include "frc971/control_loops/double_jointed_arm/graph.h"
#include <algorithm>
#include <cassert>
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
SearchGraph::SearchGraph(size_t num_vertexes, std::initializer_list<Edge> edges)
@@ -68,6 +67,5 @@
}
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
diff --git a/y2018/control_loops/superstructure/arm/graph.h b/frc971/control_loops/double_jointed_arm/graph.h
similarity index 94%
rename from y2018/control_loops/superstructure/arm/graph.h
rename to frc971/control_loops/double_jointed_arm/graph.h
index 2300d6a..06d396c 100644
--- a/y2018/control_loops/superstructure/arm/graph.h
+++ b/frc971/control_loops/double_jointed_arm/graph.h
@@ -1,5 +1,5 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
+#ifndef FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_GRAPH_H_
+#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_GRAPH_H_
#include <algorithm>
#include <cassert>
@@ -8,9 +8,8 @@
#include <memory>
#include <vector>
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
// Grr... normal priority queues don't allow modifying the node cost.
@@ -183,8 +182,7 @@
};
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_GRAPH_H_
diff --git a/y2018/control_loops/superstructure/arm/graph_test.cc b/frc971/control_loops/double_jointed_arm/graph_test.cc
similarity index 90%
rename from y2018/control_loops/superstructure/arm/graph_test.cc
rename to frc971/control_loops/double_jointed_arm/graph_test.cc
index 8f246a5..7b14ced 100644
--- a/y2018/control_loops/superstructure/arm/graph_test.cc
+++ b/frc971/control_loops/double_jointed_arm/graph_test.cc
@@ -1,10 +1,9 @@
-#include "y2018/control_loops/superstructure/arm/graph.h"
+#include "frc971/control_loops/double_jointed_arm/graph.h"
#include "gtest/gtest.h"
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
namespace testing {
@@ -68,6 +67,5 @@
} // namespace testing
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
diff --git a/frc971/control_loops/double_jointed_arm/test_constants.h b/frc971/control_loops/double_jointed_arm/test_constants.h
new file mode 100644
index 0000000..ea1d3b7
--- /dev/null
+++ b/frc971/control_loops/double_jointed_arm/test_constants.h
@@ -0,0 +1,53 @@
+#ifndef FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TEST_CONSTANTS_H_
+#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TEST_CONSTANTS_H_
+
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace arm {
+namespace testing {
+
+constexpr double kEfficiencyTweak = 0.95;
+constexpr double kStallTorque = 1.41 * kEfficiencyTweak;
+constexpr double kFreeSpeed = (5840.0 / 60.0) * 2.0 * M_PI;
+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,
+
+ // Moment of inertia of the joints in kg m^2
+ .j1 = 2957.05 * 0.0002932545454545454,
+ .j2 = 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,
+
+ // Gear ratios for the two joints.
+ .g1 = 140.0,
+ .g2 = 90.0,
+
+ // MiniCIM motor constants.
+ .efficiency_tweak = kEfficiencyTweak,
+ .stall_torque = kStallTorque,
+ .free_speed = kFreeSpeed,
+ .stall_current = kStallCurrent,
+ .resistance = 12.0 / kStallCurrent,
+ .Kv = kFreeSpeed / 12.0,
+ .Kt = kStallTorque / kStallCurrent,
+
+ // Number of motors on the distal joint.
+ .num_distal_motors = 2.0,
+};
+
+} // namespace testing
+} // namespace arm
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TEST_CONSTANTS_H_
+
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/frc971/control_loops/double_jointed_arm/trajectory.cc
similarity index 92%
rename from y2018/control_loops/superstructure/arm/trajectory.cc
rename to frc971/control_loops/double_jointed_arm/trajectory.cc
index c6e09dd..d389eee 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.cc
+++ b/frc971/control_loops/double_jointed_arm/trajectory.cc
@@ -1,20 +1,19 @@
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
#include "Eigen/Dense"
#include "aos/logging/logging.h"
#include "frc971/control_loops/dlqr.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/jacobian.h"
#include "gflags/gflags.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.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");
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
Path Path::Reversed(const Path &p) {
@@ -106,9 +105,9 @@
::Eigen::Matrix<double, 2, 2> K2;
const ::Eigen::Matrix<double, 2, 1> gravity_volts =
- Dynamics::K3_inverse * Dynamics::GravityTorque(X);
+ dynamics_->K3_inverse_ * dynamics_->GravityTorque(X);
- Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
+ dynamics_->NormilizedMatriciesForState(X, &K1, &K2);
const ::Eigen::Matrix<double, 2, 2> omega_square =
(::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
@@ -123,18 +122,18 @@
::std::sqrt(1.0 / ::std::max(0.001, (alpha_unitizer * alpha).norm()));
const ::Eigen::Matrix<double, 2, 1> vk1 =
- Dynamics::K3_inverse * (K1 * alpha + K2 * omega_square * omega);
+ dynamics_->K3_inverse_ * (K1 * alpha + K2 * omega_square * omega);
const ::Eigen::Matrix<double, 2, 1> vk2 =
- Dynamics::K3_inverse * Dynamics::K4 * omega;
+ dynamics_->K3_inverse_ * dynamics_->K4_ * omega;
// Loop through all the various vmin, plan_vmax combinations.
for (const double c : {-plan_vmax, plan_vmax}) {
// Also loop through saturating theta0 and theta1
for (const ::std::tuple<double, double, double> &abgravity :
{::std::tuple<double, double, double>{vk1(0), vk2(0),
- gravity_volts(0)},
+ gravity_volts(0)},
::std::tuple<double, double, double>{vk1(1), vk2(1),
- gravity_volts(1)}}) {
+ gravity_volts(1)}}) {
const double a = ::std::get<0>(abgravity);
const double b = ::std::get<1>(abgravity);
const double gravity = ::std::get<2>(abgravity);
@@ -178,19 +177,19 @@
::Eigen::Matrix<double, 2, 2> K1;
::Eigen::Matrix<double, 2, 2> K2;
- Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
+ dynamics_->NormilizedMatriciesForState(X, &K1, &K2);
const ::Eigen::Matrix<double, 2, 2> omega_square =
(::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
.finished();
const ::Eigen::Matrix<double, 2, 1> k_constant =
- Dynamics::K3_inverse *
+ dynamics_->K3_inverse_ *
((K1 * alpha + K2 * omega_square * omega) * goal_velocity *
goal_velocity +
- Dynamics::K4 * omega * goal_velocity - Dynamics::GravityTorque(X));
+ dynamics_->K4_ * omega * goal_velocity - dynamics_->GravityTorque(X));
const ::Eigen::Matrix<double, 2, 1> k_scalar =
- Dynamics::K3_inverse * K1 * omega;
+ dynamics_->K3_inverse_ * K1 * omega;
const double constraint_goal_acceleration =
::std::sqrt(
@@ -236,19 +235,19 @@
::Eigen::Matrix<double, 2, 2> K1;
::Eigen::Matrix<double, 2, 2> K2;
- Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
+ dynamics_->NormilizedMatriciesForState(X, &K1, &K2);
const ::Eigen::Matrix<double, 2, 2> omega_square =
(::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
.finished();
const ::Eigen::Matrix<double, 2, 1> k_constant =
- Dynamics::K3_inverse *
+ dynamics_->K3_inverse_ *
((K1 * alpha + K2 * omega_square * omega) * goal_velocity *
goal_velocity +
- Dynamics::K4 * omega * goal_velocity - Dynamics::GravityTorque(X));
+ dynamics_->K4_ * omega * goal_velocity - dynamics_->GravityTorque(X));
const ::Eigen::Matrix<double, 2, 1> k_scalar =
- Dynamics::K3_inverse * K1 * omega;
+ dynamics_->K3_inverse_ * K1 * omega;
const double constraint_goal_acceleration =
::std::sqrt(
@@ -390,12 +389,22 @@
.finished()
.asDiagonal();
+ 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::UnboundedDiscreteDynamics, X.block<4, 1>(0, 0), U, 0.00505);
+ [this](const auto &x_blocked, const auto &U, double dt) {
+ return this->dynamics_->UnboundedDiscreteDynamics(
+ x_blocked, U, dt);
+ },
+ x_blocked, U, 0.00505);
+
const ::Eigen::Matrix<double, 4, 2> final_B =
::frc971::control_loops::NumericalJacobianU<4, 2>(
- Dynamics::UnboundedDiscreteDynamics, X.block<4, 1>(0, 0), U, 0.00505);
+ [this](const auto &x_blocked, const auto &U, double dt) {
+ return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
+ },
+ x_blocked, U, 0.00505);
::Eigen::Matrix<double, 4, 4> S;
::Eigen::Matrix<double, 2, 4> sub_K;
@@ -441,7 +450,7 @@
*saturation_goal_acceleration);
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U_ff =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
+ dynamics_->FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
*U = U_ff + K * (R - X);
}
@@ -494,11 +503,11 @@
U_unsaturated_.setZero();
} else {
const ::Eigen::Matrix<double, 6, 1> R =
- trajectory_->R(theta_, ::Eigen::Matrix<double, 2, 1>::Zero());
+ Trajectory::R(theta_, ::Eigen::Matrix<double, 2, 1>::Zero());
- U_ff_ = Dynamics::FF_U(X.block<4, 1>(0, 0),
- ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero());
+ U_ff_ = dynamics_->FF_U(
+ X.block<4, 1>(0, 0), ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero());
const ::Eigen::Matrix<double, 2, 6> K = K_at_state(X, U_ff_);
U_ = U_unsaturated_ = U_ff_ + K * (R - X);
@@ -558,7 +567,7 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory_->R(theta_t, omega_t);
- U_ff_ = Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
+ U_ff_ = dynamics_->FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
const ::Eigen::Matrix<double, 2, 6> K = K_at_state(X, U_ff_);
U_ = U_unsaturated_ = U_ff_ + K * (R - X);
@@ -613,6 +622,5 @@
}
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
diff --git a/y2018/control_loops/superstructure/arm/trajectory.h b/frc971/control_loops/double_jointed_arm/trajectory.h
similarity index 93%
rename from y2018/control_loops/superstructure/arm/trajectory.h
rename to frc971/control_loops/double_jointed_arm/trajectory.h
index a3a4246..a194a03 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.h
+++ b/frc971/control_loops/double_jointed_arm/trajectory.h
@@ -1,5 +1,5 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
+#ifndef FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TRAJECTORY_H_
+#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TRAJECTORY_H_
#include <array>
#include <initializer_list>
@@ -7,10 +7,10 @@
#include <vector>
#include "Eigen/Dense"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
// This class represents a path in theta0, theta1 space. It also returns the
@@ -90,8 +90,10 @@
public:
// Constructs a trajectory (but doesn't calculate it) given a path and a step
// size.
- Trajectory(::std::unique_ptr<const Path> path, double gridsize)
- : path_(::std::move(path)),
+ Trajectory(const Dynamics *dynamics, ::std::unique_ptr<const Path> path,
+ double gridsize)
+ : dynamics_(dynamics),
+ path_(::std::move(path)),
num_plan_points_(
static_cast<size_t>(::std::ceil(path_->length() / gridsize) + 1)),
step_size_(path_->length() /
@@ -220,7 +222,8 @@
double GetDAcceleration(double distance) const {
return GetDAcceleration(distance, max_dvelocity_);
}
- double GetDAcceleration(double distance, const ::std::vector<double> &plan) const {
+ double GetDAcceleration(double distance,
+ const ::std::vector<double> &plan) const {
::std::pair<size_t, size_t> indices = IndicesForDistance(distance);
const double v0 = plan[indices.first];
const double v1 = plan[indices.second];
@@ -276,6 +279,7 @@
const Path &path() const { return *path_; }
+
private:
friend class testing::TrajectoryTest_IndicesForDistanceTest_Test;
@@ -298,6 +302,8 @@
return ::std::pair<size_t, size_t>(lower_index, lower_index + 1);
}
+ const Dynamics *dynamics_;
+
// The path to follow.
::std::unique_ptr<const Path> path_;
// The number of points in the plan.
@@ -315,14 +321,16 @@
// This class tracks the current goal along trajectories and paths.
class TrajectoryFollower {
public:
- TrajectoryFollower(const ::Eigen::Matrix<double, 2, 1> &theta)
- : trajectory_(nullptr), theta_(theta) {
+ TrajectoryFollower(const Dynamics *dynamics,
+ const ::Eigen::Matrix<double, 2, 1> &theta)
+ : dynamics_(dynamics), trajectory_(nullptr), theta_(theta) {
omega_.setZero();
last_K_.setZero();
Reset();
}
- TrajectoryFollower(Trajectory *const trajectory) : trajectory_(trajectory) {
+ TrajectoryFollower(const Dynamics *dynamics, Trajectory *const trajectory)
+ : dynamics_(dynamics), trajectory_(trajectory) {
last_K_.setZero();
Reset();
}
@@ -405,6 +413,7 @@
int failed_solutions() const { return failed_solutions_; }
private:
+ const Dynamics *dynamics_;
// The trajectory plan.
const Trajectory *trajectory_ = nullptr;
@@ -431,8 +440,7 @@
};
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TRAJECTORY_H_
diff --git a/y2018/control_loops/superstructure/arm/trajectory_test.cc b/frc971/control_loops/double_jointed_arm/trajectory_test.cc
similarity index 86%
rename from y2018/control_loops/superstructure/arm/trajectory_test.cc
rename to frc971/control_loops/double_jointed_arm/trajectory_test.cc
index b91c5a4..e700282 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_test.cc
+++ b/frc971/control_loops/double_jointed_arm/trajectory_test.cc
@@ -1,13 +1,13 @@
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
+#include "frc971/control_loops/double_jointed_arm/test_constants.h"
+#include "frc971/control_loops/double_jointed_arm/demo_path.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
#include "gtest/gtest.h"
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-#include "y2018/control_loops/superstructure/arm/ekf.h"
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
namespace testing {
@@ -75,20 +75,22 @@
(::Eigen::Matrix<double, 2, 1>() << 3.0, 0.0).finished()));
}
-
-// Tests that we can compute the indices of the plan for a given distance correctly.
+// Tests that we can compute the indices of the plan for a given distance
+// correctly.
TEST(TrajectoryTest, IndicesForDistanceTest) {
// Start with a stupid simple plan.
Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
{{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
{{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
{{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
- Trajectory t(::std::unique_ptr<Path>(new Path(p)), 0.1);
+ Dynamics dynamics(kArmConstants);
+ Trajectory t(&dynamics, ::std::unique_ptr<Path>(new Path(p)), 0.1);
// 0 - 3.0 every 0.1 should be 31 points.
EXPECT_EQ(t.num_plan_points(), 31);
- // Verify that something centered in a grid cell returns the points on either side.
+ // Verify that something centered in a grid cell returns the points on either
+ // side.
EXPECT_EQ(::std::make_pair(static_cast<size_t>(0), static_cast<size_t>(1)),
t.IndicesForDistance(0.05));
EXPECT_EQ(::std::make_pair(static_cast<size_t>(1), static_cast<size_t>(2)),
@@ -145,17 +147,21 @@
EXPECT_NEAR(path->length(), reversed_path->length(), 1e-6);
for (double d = 0; d < path->length(); d += 0.01) {
- EXPECT_TRUE(path->Theta(d).isApprox(reversed_path->Theta(path->length() - d)));
- EXPECT_TRUE(path->Omega(d).isApprox(-reversed_path->Omega(path->length() - d)));
- EXPECT_TRUE(path->Alpha(d).isApprox(reversed_path->Alpha(path->length() - d)));
+ EXPECT_TRUE(
+ path->Theta(d).isApprox(reversed_path->Theta(path->length() - d)));
+ EXPECT_TRUE(
+ path->Omega(d).isApprox(-reversed_path->Omega(path->length() - d)));
+ EXPECT_TRUE(
+ path->Alpha(d).isApprox(reversed_path->Alpha(path->length() - d)));
}
}
// Tests that we can follow a path. Look at :trajectory_plot if you want to see
// the path.
TEST(TrajectoryTest, RunTrajectory) {
+ Dynamics dynamics(kArmConstants);
::std::unique_ptr<Path> path = MakeDemoPath();
- Trajectory trajectory(::std::move(path), 0.001);
+ Trajectory trajectory(&dynamics, ::std::move(path), 0.001);
constexpr double kAlpha0Max = 40.0;
constexpr double kAlpha1Max = 60.0;
@@ -174,16 +180,17 @@
X << theta_t(0), 0.0, theta_t(1), 0.0;
}
- EKF arm_ekf;
+ EKF arm_ekf(&dynamics);
arm_ekf.Reset(X);
- TrajectoryFollower follower(&trajectory);
+ TrajectoryFollower follower(&dynamics, &trajectory);
constexpr double sim_dt = 0.00505;
while (t < 1.0) {
arm_ekf.Correct((::Eigen::Matrix<double, 2, 1>() << X(0), X(2)).finished(),
sim_dt);
follower.Update(arm_ekf.X_hat(), false, sim_dt, vmax, 12.0);
- X = Dynamics::UnboundedDiscreteDynamics(X, follower.U(), sim_dt);
+
+ X = dynamics.UnboundedDiscreteDynamics(X, follower.U(), sim_dt);
arm_ekf.Predict(follower.U(), sim_dt);
t += sim_dt;
}
@@ -204,6 +211,5 @@
} // namespace testing
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
diff --git a/y2018/BUILD b/y2018/BUILD
index ac59493..8feaac4 100644
--- a/y2018/BUILD
+++ b/y2018/BUILD
@@ -57,9 +57,10 @@
"//aos/network:team_number",
"//aos/stl_mutex",
"//frc971:constants",
+ "//frc971/control_loops/double_jointed_arm:dynamics",
"//frc971/shooter_interpolation:interpolation",
"//y2018/control_loops/drivetrain:polydrivetrain_plants",
- "//y2018/control_loops/superstructure/arm:dynamics",
+ "//y2018/control_loops/superstructure/arm:arm_constants",
"//y2018/control_loops/superstructure/intake:intake_plants",
"@com_github_google_glog//:glog",
],
diff --git a/y2018/constants.h b/y2018/constants.h
index 16f1f8c..2a6f0ac 100644
--- a/y2018/constants.h
+++ b/y2018/constants.h
@@ -6,7 +6,8 @@
#include "frc971/constants.h"
#include "y2018/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
namespace y2018 {
@@ -49,8 +50,8 @@
return (12.0 / 60.0) * (18.0 / 84.0);
}
static constexpr double kMaxProximalEncoderPulsesPerSecond() {
- return control_loops::superstructure::arm::Dynamics::kFreeSpeed /
- (2.0 * M_PI) / control_loops::superstructure::arm::Dynamics::kG1 /
+ return control_loops::superstructure::arm::kArmConstants.free_speed /
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g1 /
kProximalEncoderRatio() * kProximalEncoderCountsPerRevolution();
}
static constexpr double kProximalPotRatio() { return (12.0 / 60.0); }
@@ -58,8 +59,8 @@
static constexpr double kDistalEncoderCountsPerRevolution() { return 4096.0; }
static constexpr double kDistalEncoderRatio() { return (12.0 / 60.0); }
static constexpr double kMaxDistalEncoderPulsesPerSecond() {
- return control_loops::superstructure::arm::Dynamics::kFreeSpeed /
- (2.0 * M_PI) / control_loops::superstructure::arm::Dynamics::kG2 /
+ return control_loops::superstructure::arm::kArmConstants.free_speed /
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g2 /
kDistalEncoderRatio() * kProximalEncoderCountsPerRevolution();
}
static constexpr double kDistalPotRatio() {
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index e7f8ce1..be267de 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -27,12 +27,12 @@
cc_file.append(" %s," % (alpha_unitizer))
if reverse:
cc_file.append(
- " Trajectory(Path::Reversed(%s()), 0.005));"
+ " Trajectory(dynamics, Path::Reversed(%s()), 0.005));"
% (path_function_name(str(name))))
else:
cc_file.append(
- " Trajectory(%s(), 0.005));" %
- (path_function_name(str(name))))
+ " Trajectory(dynamics, %s(), 0.005));"
+ % (path_function_name(str(name))))
start_index = None
end_index = None
@@ -68,9 +68,14 @@
cc_file.append("#include <memory>")
cc_file.append("")
cc_file.append(
- "#include \"y2018/control_loops/superstructure/arm/trajectory.h\"")
+ "#include \"frc971/control_loops/double_jointed_arm/trajectory.h\"")
cc_file.append(
- "#include \"y2018/control_loops/superstructure/arm/graph.h\"")
+ "#include \"frc971/control_loops/double_jointed_arm/graph.h\"")
+
+ cc_file.append("using frc971::control_loops::arm::Trajectory;")
+ cc_file.append("using frc971::control_loops::arm::Path;")
+ cc_file.append("using frc971::control_loops::arm::SearchGraph;")
+
cc_file.append("")
cc_file.append("namespace y2018 {")
cc_file.append("namespace control_loops {")
@@ -86,15 +91,21 @@
h_file.append("#include <memory>")
h_file.append("")
h_file.append(
- "#include \"y2018/control_loops/superstructure/arm/trajectory.h\"")
+ "#include \"frc971/control_loops/double_jointed_arm/trajectory.h\"")
h_file.append(
- "#include \"y2018/control_loops/superstructure/arm/graph.h\"")
+ "#include \"frc971/control_loops/double_jointed_arm/graph.h\"")
+ h_file.append(
+ "#include \"frc971/control_loops/double_jointed_arm/dynamics.h\"")
h_file.append("")
h_file.append("namespace y2018 {")
h_file.append("namespace control_loops {")
h_file.append("namespace superstructure {")
h_file.append("namespace arm {")
+ h_file.append("using frc971::control_loops::arm::Trajectory;")
+ h_file.append("using frc971::control_loops::arm::Path;")
+ h_file.append("using frc971::control_loops::arm::SearchGraph;")
+
h_file.append("")
h_file.append("struct TrajectoryAndParams {")
h_file.append(" TrajectoryAndParams(double new_vmax,")
@@ -182,11 +193,13 @@
h_file.append("")
h_file.append("// Builds a search graph.")
h_file.append("SearchGraph MakeSearchGraph("
+ "const frc971::control_loops::arm::Dynamics *dynamics, "
"::std::vector<TrajectoryAndParams> *trajectories,")
h_file.append(" "
"const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
h_file.append(" double vmax);")
cc_file.append("SearchGraph MakeSearchGraph("
+ "const frc971::control_loops::arm::Dynamics *dynamics, "
"::std::vector<TrajectoryAndParams> *trajectories,")
cc_file.append(" "
"const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 567810d..a3cb808 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -94,6 +94,7 @@
"//frc971/control_loops:control_loop_test",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
+ "//y2018/control_loops/superstructure/arm:arm_constants",
"//y2018/control_loops/superstructure/intake:intake_plants",
],
)
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index f90cc92..9f969ff 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -1,128 +1,4 @@
cc_library(
- name = "trajectory",
- srcs = [
- "trajectory.cc",
- ],
- hdrs = [
- "trajectory.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- ":dynamics",
- "//aos/logging",
- "//frc971/control_loops:dlqr",
- "//frc971/control_loops:jacobian",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_test(
- name = "trajectory_test",
- srcs = [
- "trajectory_test.cc",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":demo_path",
- ":dynamics",
- ":ekf",
- ":trajectory",
- "//aos/testing:googletest",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
- name = "dynamics",
- srcs = [
- "dynamics.cc",
- ],
- hdrs = [
- "dynamics.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "//frc971/control_loops:runge_kutta",
- "@com_github_gflags_gflags//:gflags",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
- name = "demo_path",
- srcs = [
- "demo_path.cc",
- ],
- hdrs = ["demo_path.h"],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [":trajectory"],
-)
-
-cc_test(
- name = "dynamics_test",
- srcs = [
- "dynamics_test.cc",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":dynamics",
- "//aos/testing:googletest",
- ],
-)
-
-cc_binary(
- name = "trajectory_plot",
- srcs = [
- "trajectory_plot.cc",
- ],
- target_compatible_with = ["@platforms//cpu:x86_64"],
- deps = [
- ":ekf",
- ":generated_graph",
- ":trajectory",
- "//frc971/analysis:in_process_plotter",
- "@com_github_gflags_gflags//:gflags",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
- name = "ekf",
- srcs = [
- "ekf.cc",
- ],
- hdrs = [
- "ekf.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- ":dynamics",
- "//frc971/control_loops:jacobian",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
- name = "graph",
- srcs = ["graph.cc"],
- hdrs = ["graph.h"],
- target_compatible_with = ["@platforms//os:linux"],
-)
-
-cc_test(
- name = "graph_test",
- srcs = ["graph_test.cc"],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":graph",
- "//aos/testing:googletest",
- ],
-)
-
-cc_library(
name = "arm",
srcs = [
"arm.cc",
@@ -133,15 +9,16 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
- ":demo_path",
- ":ekf",
":generated_graph",
- ":graph",
- ":trajectory",
+ "//frc971/control_loops/double_jointed_arm:demo_path",
+ "//frc971/control_loops/double_jointed_arm:ekf",
+ "//frc971/control_loops/double_jointed_arm:graph",
+ "//frc971/control_loops/double_jointed_arm:trajectory",
"//frc971/zeroing",
"//y2018:constants",
"//y2018/control_loops/superstructure:superstructure_position_fbs",
"//y2018/control_loops/superstructure:superstructure_status_fbs",
+ "//y2018/control_loops/superstructure/arm:arm_constants",
],
)
@@ -170,7 +47,35 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
- ":graph",
- ":trajectory",
+ ":arm_constants",
+ "//frc971/control_loops/double_jointed_arm:graph",
+ "//frc971/control_loops/double_jointed_arm:trajectory",
+ ],
+)
+
+cc_library(
+ name = "arm_constants",
+ hdrs = ["arm_constants.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/double_jointed_arm:dynamics",
+ ],
+)
+
+cc_binary(
+ name = "trajectory_plot",
+ srcs = [
+ "trajectory_plot.cc",
+ ],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":arm_constants",
+ ":generated_graph",
+ "//frc971/analysis:in_process_plotter",
+ "//frc971/control_loops/double_jointed_arm:ekf",
+ "//frc971/control_loops/double_jointed_arm:trajectory",
+ "@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 1c29157..ab5deea 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -5,8 +5,9 @@
#include "aos/logging/logging.h"
#include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/demo_path.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
namespace y2018 {
@@ -29,9 +30,12 @@
alpha_unitizer_((::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max(),
0.0, 0.0, 1.0 / kAlpha1Max())
.finished()),
- search_graph_(MakeSearchGraph(&trajectories_, alpha_unitizer_, kVMax())),
+ dynamics_(kArmConstants),
+ search_graph_(MakeSearchGraph(&dynamics_, &trajectories_, alpha_unitizer_,
+ kVMax())),
+ arm_ekf_(&dynamics_),
// Go to the start of the first trajectory.
- follower_(ReadyAboveBoxPoint()),
+ follower_(&dynamics_, ReadyAboveBoxPoint()),
points_(PointList()) {
int i = 0;
for (const auto &trajectory : trajectories_) {
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 7ceb001..51d6c4d 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -4,14 +4,17 @@
#include "aos/time/time.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
-#include "y2018/control_loops/superstructure/arm/graph.h"
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
+#include "frc971/control_loops/double_jointed_arm/graph.h"
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
+using frc971::control_loops::arm::TrajectoryFollower;
+using frc971::control_loops::arm::EKF;
+
namespace y2018 {
namespace control_loops {
namespace superstructure {
@@ -114,6 +117,8 @@
double vmax_ = kVMax();
+ frc971::control_loops::arm::Dynamics dynamics_;
+
::std::vector<TrajectoryAndParams> trajectories_;
SearchGraph search_graph_;
diff --git a/y2018/control_loops/superstructure/arm/arm_constants.h b/y2018/control_loops/superstructure/arm/arm_constants.h
new file mode 100644
index 0000000..9ac7020
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/arm_constants.h
@@ -0,0 +1,53 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
+
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+constexpr double kEfficiencyTweak = 0.95;
+constexpr double kStallTorque = 1.41 * kEfficiencyTweak;
+constexpr double kFreeSpeed = (5840.0 / 60.0) * 2.0 * M_PI;
+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,
+
+ // Moment of inertia of the joints in kg m^2
+ .j1 = 2957.05 * 0.0002932545454545454,
+ .j2 = 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,
+
+ // Gear ratios for the two joints.
+ .g1 = 140.0,
+ .g2 = 90.0,
+
+ // MiniCIM motor constants.
+ .efficiency_tweak = kEfficiencyTweak,
+ .stall_torque = kStallTorque,
+ .free_speed = kFreeSpeed,
+ .stall_current = kStallCurrent,
+ .resistance = 12.0 / kStallCurrent,
+ .Kv = kFreeSpeed / 12.0,
+ .Kt = kStallTorque / kStallCurrent,
+
+ // Number of motors on the distal joint.
+ .num_distal_motors = 2.0,
+};
+
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
diff --git a/y2018/control_loops/superstructure/arm/demo_path.h b/y2018/control_loops/superstructure/arm/demo_path.h
deleted file mode 100644
index 0c18103..0000000
--- a/y2018/control_loops/superstructure/arm/demo_path.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
-
-#include <memory>
-
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-::std::unique_ptr<Path> MakeDemoPath();
-::std::unique_ptr<Path> MakeReversedDemoPath();
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
-
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
diff --git a/y2018/control_loops/superstructure/arm/dynamics.cc b/y2018/control_loops/superstructure/arm/dynamics.cc
deleted file mode 100644
index 02a2861..0000000
--- a/y2018/control_loops/superstructure/arm/dynamics.cc
+++ /dev/null
@@ -1,35 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
-DEFINE_bool(gravity, true, "If true, enable gravity.");
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-const ::Eigen::Matrix<double, 2, 2> Dynamics::K3 =
- (::Eigen::Matrix<double, 2, 2>()
- << Dynamics::kG1 * Dynamics::Kt / Dynamics::kResistance,
- 0.0, 0.0, Dynamics::kG2 *Dynamics::kNumDistalMotors *Dynamics::Kt /
- Dynamics::kResistance)
- .finished();
-
-const ::Eigen::Matrix<double, 2, 2> Dynamics::K3_inverse = K3.inverse();
-
-const ::Eigen::Matrix<double, 2, 2> Dynamics::K4 =
- (::Eigen::Matrix<double, 2, 2>()
- << Dynamics::kG1 * Dynamics::kG1 * Dynamics::Kt /
- (Dynamics::Kv * Dynamics::kResistance),
- 0.0, 0.0,
- Dynamics::kG2 *Dynamics::kG2 *Dynamics::Kt *Dynamics::kNumDistalMotors /
- (Dynamics::Kv * Dynamics::kResistance))
- .finished();
-
-constexpr double Dynamics::kAlpha;
-constexpr double Dynamics::kBeta;
-constexpr double Dynamics::kGamma;
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/dynamics.h b/y2018/control_loops/superstructure/arm/dynamics.h
deleted file mode 100644
index b1ebc97..0000000
--- a/y2018/control_loops/superstructure/arm/dynamics.h
+++ /dev/null
@@ -1,189 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
-
-#include "Eigen/Dense"
-
-#include "frc971/control_loops/runge_kutta.h"
-#include "gflags/gflags.h"
-
-DECLARE_bool(gravity);
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-// 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.
-class Dynamics {
- public:
- // Below, 1 refers to the proximal joint, and 2 refers to the distal joint.
- // Length of the joints in meters.
- static constexpr double kL1 = 46.25 * 0.0254;
- static constexpr double kL2 = 41.80 * 0.0254;
-
- // Mass of the joints in kilograms.
- static constexpr double kM1 = 9.34 / 2.2;
- static constexpr double kM2 = 9.77 / 2.2;
-
- // Moment of inertia of the joints in kg m^2
- static constexpr double kJ1 = 2957.05 * 0.0002932545454545454;
- static constexpr double kJ2 = 2824.70 * 0.0002932545454545454;
-
- // Radius of the center of mass of the joints in meters.
- static constexpr double r1 = 21.64 * 0.0254;
- static constexpr double r2 = 26.70 * 0.0254;
-
- // Gear ratios for the two joints.
- static constexpr double kG1 = 140.0;
- static constexpr double kG2 = 90.0;
-
- // MiniCIM motor constants.
- static constexpr double kEfficiencyTweak = 0.95;
- static constexpr double kStallTorque = 1.41 * kEfficiencyTweak;
- static constexpr double kFreeSpeed = (5840.0 / 60.0) * 2.0 * M_PI;
- static constexpr double kStallCurrent = 89.0;
- static constexpr double kResistance = 12.0 / kStallCurrent;
- static constexpr double Kv = kFreeSpeed / 12.0;
- static constexpr double Kt = kStallTorque / kStallCurrent;
-
- // Number of motors on the distal joint.
- static constexpr double kNumDistalMotors = 2.0;
-
- static constexpr double kAlpha = kJ1 + r1 * r1 * kM1 + kL1 * kL1 * kM2;
- static constexpr double kBeta = kL1 * r2 * kM2;
- static constexpr double kGamma = kJ2 + r2 * r2 * kM2;
-
- // K3, K4 matricies described below.
- static const ::Eigen::Matrix<double, 2, 2> K3;
- static const ::Eigen::Matrix<double, 2, 2> K3_inverse;
- static const ::Eigen::Matrix<double, 2, 2> K4;
-
- // Generates K1-2 for the arm ODE.
- // 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.
- static void NormilizedMatriciesForState(
- const ::Eigen::Matrix<double, 4, 1> &X,
- ::Eigen::Matrix<double, 2, 2> *K1_result,
- ::Eigen::Matrix<double, 2, 2> *K2_result) {
- const double angle = X(0, 0) - X(2, 0);
- const double s = ::std::sin(angle);
- const double c = ::std::cos(angle);
- *K1_result << kAlpha, c * kBeta, c * kBeta, kGamma;
- *K2_result << 0.0, s * kBeta, -s * kBeta, 0.0;
- }
-
- // Generates K1-2 for the arm ODE.
- // K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
- static void MatriciesForState(const ::Eigen::Matrix<double, 4, 1> &X,
- ::Eigen::Matrix<double, 2, 2> *K1_result,
- ::Eigen::Matrix<double, 2, 2> *K2_result) {
- NormilizedMatriciesForState(X, K1_result, K2_result);
- (*K2_result)(1, 0) *= X(1, 0);
- (*K2_result)(0, 1) *= X(3, 0);
- }
-
- // 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.
- static const ::Eigen::Matrix<double, 4, 1> Acceleration(
- const ::Eigen::Matrix<double, 4, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U) {
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
-
- MatriciesForState(X, &K1, &K2);
-
- const ::Eigen::Matrix<double, 2, 1> velocity =
- (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
-
- const ::Eigen::Matrix<double, 2, 1> torque = K3 * U - K4 * velocity;
- const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
-
- const ::Eigen::Matrix<double, 2, 1> accel =
- K1.inverse() * (torque + gravity_torque - K2 * velocity);
-
- return (::Eigen::Matrix<double, 4, 1>() << X(1, 0), accel(0, 0), X(3, 0),
- accel(1, 0))
- .finished();
- }
-
- // Calculates the acceleration given the current augmented kalman filter state
- // and control input.
- static const ::Eigen::Matrix<double, 6, 1> EKFAcceleration(
- const ::Eigen::Matrix<double, 6, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U) {
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
-
- MatriciesForState(X.block<4, 1>(0, 0), &K1, &K2);
-
- const ::Eigen::Matrix<double, 2, 1> velocity =
- (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
-
- const ::Eigen::Matrix<double, 2, 1> torque =
- K3 *
- (U +
- (::Eigen::Matrix<double, 2, 1>() << X(4, 0), X(5, 0)).finished()) -
- K4 * velocity;
- const ::Eigen::Matrix<double, 2, 1> gravity_torque =
- GravityTorque(X.block<4, 1>(0, 0));
-
- const ::Eigen::Matrix<double, 2, 1> accel =
- K1.inverse() * (torque + gravity_torque - K2 * velocity);
-
- return (::Eigen::Matrix<double, 6, 1>() << X(1, 0), accel(0, 0), X(3, 0),
- accel(1, 0), 0.0, 0.0)
- .finished();
- }
-
- // Calculates the voltage required to follow the trajectory. This requires
- // knowing the current state, desired angular velocity and acceleration.
- static const ::Eigen::Matrix<double, 2, 1> FF_U(
- const ::Eigen::Matrix<double, 4, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &omega_t,
- const ::Eigen::Matrix<double, 2, 1> &alpha_t) {
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
-
- MatriciesForState(X, &K1, &K2);
-
- const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
-
- return K3_inverse *
- (K1 * alpha_t + K2 * omega_t + K4 * omega_t - gravity_torque);
- }
-
- static ::Eigen::Matrix<double, 2, 1> GravityTorque(
- const ::Eigen::Matrix<double, 4, 1> &X) {
- constexpr double kAccelDueToGravity = 9.8 * kEfficiencyTweak;
- return (::Eigen::Matrix<double, 2, 1>() << (r1 * kM1 + kL1 * kM2) *
- ::std::sin(X(0)) *
- kAccelDueToGravity,
- r2 * kM2 * ::std::sin(X(2)) * kAccelDueToGravity)
- .finished() *
- (FLAGS_gravity ? 1.0 : 0.0);
- }
-
- static const ::Eigen::Matrix<double, 4, 1> UnboundedDiscreteDynamics(
- const ::Eigen::Matrix<double, 4, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
- return ::frc971::control_loops::RungeKuttaU(Dynamics::Acceleration, X, U,
- dt);
- }
-
- static const ::Eigen::Matrix<double, 6, 1> UnboundedEKFDiscreteDynamics(
- const ::Eigen::Matrix<double, 6, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
- return ::frc971::control_loops::RungeKuttaU(Dynamics::EKFAcceleration, X, U,
- dt);
- }
-};
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
-
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
diff --git a/y2018/control_loops/superstructure/arm/dynamics_test.cc b/y2018/control_loops/superstructure/arm/dynamics_test.cc
deleted file mode 100644
index 23e5adc..0000000
--- a/y2018/control_loops/superstructure/arm/dynamics_test.cc
+++ /dev/null
@@ -1,52 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
-#include "gtest/gtest.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-namespace testing {
-
-// Tests that zero inputs result in no acceleration and no motion.
-// This isn't all that rigerous, but it's a good start.
-TEST(DynamicsTest, Acceleration) {
- Dynamics dynamics;
-
- EXPECT_TRUE(dynamics
- .Acceleration(::Eigen::Matrix<double, 4, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero())
- .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
-
- EXPECT_TRUE(dynamics
- .UnboundedDiscreteDynamics(
- ::Eigen::Matrix<double, 4, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero(), 0.1)
- .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
-
- const ::Eigen::Matrix<double, 4, 1> X =
- (::Eigen::Matrix<double, 4, 1>() << M_PI / 2.0, 0.0, 0.0, 0.0).finished();
-
- ::std::cout << dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero())
- << ::std::endl;
-
- ::std::cout << dynamics.UnboundedDiscreteDynamics(
- X, dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero()),
- 0.01)
- << ::std::endl;
-
- EXPECT_TRUE(dynamics
- .UnboundedDiscreteDynamics(
- X, dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero()),
- 0.01)
- .isApprox(X));
-}
-
-} // namespace testing
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/trajectory_plot.cc b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
index 10b39bc..0cee664 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
@@ -1,10 +1,11 @@
#include "aos/init.h"
#include "frc971/analysis/in_process_plotter.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
#include "gflags/gflags.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
DEFINE_bool(forwards, true, "If true, run the forwards simulation.");
DEFINE_bool(plot, true, "If true, plot");
@@ -16,10 +17,12 @@
namespace arm {
void Main() {
- Trajectory trajectory(FLAGS_forwards
- ? MakeNeutralToFrontHighPath()
- : Path::Reversed(MakeNeutralToFrontHighPath()),
- 0.001);
+ frc971::control_loops::arm::Dynamics dynamics(kArmConstants);
+ frc971::control_loops::arm::Trajectory trajectory(
+ &dynamics,
+ FLAGS_forwards ? MakeNeutralToFrontHighPath()
+ : Path::Reversed(MakeNeutralToFrontHighPath()),
+ 0.001);
constexpr double kAlpha0Max = 30.0;
constexpr double kAlpha1Max = 50.0;
@@ -96,7 +99,7 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+ dynamics.FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
.array()
.max(-20)
.min(20);
@@ -118,7 +121,7 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+ dynamics.FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
.array()
.max(-20)
.min(20);
@@ -140,7 +143,7 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+ dynamics.FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
.array()
.max(-20)
.min(20);
@@ -156,7 +159,8 @@
X << theta_t(0), 0.0, theta_t(1), 0.0;
}
- TrajectoryFollower follower(&trajectory);
+ frc971::control_loops::arm::TrajectoryFollower follower(&dynamics,
+ &trajectory);
::std::vector<double> t_array;
::std::vector<double> theta0_goal_t_array;
@@ -187,7 +191,7 @@
::std::vector<double> torque0_hat_t_array;
::std::vector<double> torque1_hat_t_array;
- EKF arm_ekf;
+ frc971::control_loops::arm::EKF arm_ekf(&dynamics);
arm_ekf.Reset(X);
::std::cout << "Reset P: " << arm_ekf.P_reset() << ::std::endl;
::std::cout << "Stabilized P: " << arm_ekf.P_half_converged() << ::std::endl;
@@ -236,9 +240,9 @@
actual_U(0) += 1.0;
const ::Eigen::Matrix<double, 4, 1> xdot =
- Dynamics::Acceleration(X, actual_U);
+ dynamics.Acceleration(X, actual_U);
- X = Dynamics::UnboundedDiscreteDynamics(X, actual_U, sim_dt);
+ X = dynamics.UnboundedDiscreteDynamics(X, actual_U, sim_dt);
arm_ekf.Predict(follower.U(), sim_dt);
alpha0_t_array.push_back(xdot(1));
@@ -270,10 +274,14 @@
plotter.AddLine(distance_array, alpha0_array, "alpha0");
plotter.AddLine(distance_array, alpha1_array, "alpha1");
- plotter.AddLine(integrated_distance, integrated_theta0_array, "integrated theta0");
- plotter.AddLine(integrated_distance, integrated_theta1_array, "integrated theta1");
- plotter.AddLine(integrated_distance, integrated_omega0_array, "integrated omega0");
- plotter.AddLine(integrated_distance, integrated_omega1_array, "integrated omega1");
+ plotter.AddLine(integrated_distance, integrated_theta0_array,
+ "integrated theta0");
+ plotter.AddLine(integrated_distance, integrated_theta1_array,
+ "integrated theta1");
+ plotter.AddLine(integrated_distance, integrated_omega0_array,
+ "integrated omega0");
+ plotter.AddLine(integrated_distance, integrated_omega1_array,
+ "integrated omega1");
plotter.Publish();
plotter.AddFigure();
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 285c8c3..54342c5 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -8,7 +8,8 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
#include "y2018/control_loops/superstructure/superstructure.h"
@@ -129,7 +130,8 @@
constants::Values::kProximalEncoderRatio()),
distal_zeroing_constants_(distal_zeroing_constants),
distal_pot_encoder_(M_PI * 2.0 *
- constants::Values::kDistalEncoderRatio()) {
+ constants::Values::kDistalEncoderRatio()),
+ dynamics_(arm::kArmConstants) {
X_.setZero();
}
@@ -174,7 +176,7 @@
AOS_CHECK_LE(::std::abs(U(1)), voltage_check);
if (release_arm_brake) {
- X_ = arm::Dynamics::UnboundedDiscreteDynamics(X_, U, 0.00505);
+ X_ = dynamics_.UnboundedDiscreteDynamics(X_, U, 0.00505);
} else {
// Well, the brake shouldn't stop both joints, but this will get the tests
// to pass.
@@ -197,6 +199,8 @@
const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
distal_zeroing_constants_;
PositionSensorSimulator distal_pot_encoder_;
+
+ ::frc971::control_loops::arm::Dynamics dynamics_;
};
class SuperstructureSimulation {