Merge "Add Flipper Arms Wpilib_Interface"
diff --git a/WORKSPACE b/WORKSPACE
index 8e84cd1..9c4529b 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -975,6 +975,22 @@
)
http_archive(
+ name = "osqp_amd64",
+ build_file = "@//debian:osqp_python.BUILD",
+ sha256 = "8003fc363f707daa46fef3af548e6a580372154d6cd49a7bf2f569ba5f807d15",
+ type = "zip",
+ url = "https://files.pythonhosted.org/packages/3f/e2/f1c40e890f00f8a566bc2481d0f215e52def3dfe8eea6b8ad4cc2d3cbca2/osqp-0.6.2.post5-cp39-cp39-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_17_x86_64.manylinux2014_x86_64.whl",
+)
+
+http_archive(
+ name = "qdldl_amd64",
+ build_file = "@//debian:qdldl_python.BUILD",
+ sha256 = "2c09f4b1a1c6f3a0579af004443417e084491e7c844ff9fb73170bb5d43f70b5",
+ type = "zip",
+ url = "https://files.pythonhosted.org/packages/9e/26/ccb4f065b40c1e9ff35ee66970d4fa97dd2fe221b846da2110eb8cd6c3f4/qdldl-0.1.5.post0-cp39-cp39-manylinux2014_x86_64.whl",
+)
+
+http_archive(
name = "gstreamer_k8",
build_file = "@//debian:gstreamer.BUILD",
sha256 = "4d74d4a82f7a73dc9fe9463d5fae409b17845eef7cd64ef9c4c4553816c53589",
diff --git a/debian/osqp_python.BUILD b/debian/osqp_python.BUILD
new file mode 100644
index 0000000..9d3260e
--- /dev/null
+++ b/debian/osqp_python.BUILD
@@ -0,0 +1,15 @@
+py_library(
+ name = "python_osqp",
+ srcs = glob(["**/*.py"]),
+ data = glob(
+ include = ["**/*"],
+ exclude = ["**/*.py"],
+ ),
+ imports = ["."],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@python_repo//:numpy",
+ "@python_repo//:scipy",
+ "@qdldl_amd64//:python_qdldl",
+ ],
+)
diff --git a/debian/qdldl_python.BUILD b/debian/qdldl_python.BUILD
new file mode 100644
index 0000000..76d647e
--- /dev/null
+++ b/debian/qdldl_python.BUILD
@@ -0,0 +1,14 @@
+py_library(
+ name = "python_qdldl",
+ srcs = glob(["**/*.py"]),
+ data = glob(
+ include = ["**/*"],
+ exclude = ["**/*.py"],
+ ),
+ imports = ["."],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@python_repo//:numpy",
+ "@python_repo//:scipy",
+ ],
+)
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 0170d7c..fd39359 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -28,7 +28,8 @@
C(other.C),
D(other.D),
U_min(other.U_min),
- U_max(other.U_max) {}
+ U_max(other.U_max),
+ delayed_u(other.delayed_u) {}
StateFeedbackHybridPlantCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
@@ -38,13 +39,14 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C,
const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
- const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min)
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min, bool delayed_u)
: A_continuous(A_continuous),
B_continuous(B_continuous),
C(C),
D(D),
U_min(U_min),
- U_max(U_max) {}
+ U_max(U_max),
+ delayed_u(delayed_u) {}
const Eigen::Matrix<Scalar, number_of_states, number_of_states> A_continuous;
const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B_continuous;
@@ -52,6 +54,8 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> D;
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
+
+ const bool delayed_u;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -226,16 +230,20 @@
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
P_steady_state;
+ const bool delayed_u;
+
HybridKalmanCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
&Q_continuous,
const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs>
&R_continuous,
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
- &P_steady_state)
+ &P_steady_state, bool delayed_u)
: Q_continuous(Q_continuous),
R_continuous(R_continuous),
- P_steady_state(P_steady_state) {}
+ P_steady_state(P_steady_state), delayed_u(delayed_u) {
+ CHECK(!delayed_u) << ": Delayed hybrid filters aren't supported yet.";
+ }
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index c866457..147a84f 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -34,7 +34,7 @@
B_continuous(1, 0) = 443.75;
B_continuous(2, 0) = 0.0;
return StateFeedbackHybridPlantCoefficients<3, 1, 1>(
- A_continuous, B_continuous, C, D, U_max, U_min);
+ A_continuous, B_continuous, C, D, U_max, U_min, false);
}
StateFeedbackControllerCoefficients<3, 1, 1>
@@ -74,7 +74,7 @@
P_steady_state(2, 1) = 0.015962850974712596;
P_steady_state(2, 2) = 0.0019821816120708254;
return HybridKalmanCoefficients<3, 1, 1>(Q_continuous, R_continuous,
- P_steady_state);
+ P_steady_state, false);
}
StateFeedbackHybridPlant<3, 1, 1> MakeIntegralShooterPlant() {
@@ -128,7 +128,7 @@
Eigen::Matrix<double, 7, 4>::Identity(),
Eigen::Matrix<double, 4, 1>::Constant(1),
Eigen::Matrix<double, 4, 1>::Constant(-1),
- frc971::controls::kLoopFrequency);
+ frc971::controls::kLoopFrequency, false);
// Build a plant.
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>>
@@ -153,7 +153,7 @@
v_observer.emplace_back(new StateFeedbackObserverCoefficients<2, 4, 7>(
Eigen::Matrix<double, 2, 7>::Identity(),
Eigen::Matrix<double, 2, 2>::Identity(),
- Eigen::Matrix<double, 7, 7>::Identity()));
+ Eigen::Matrix<double, 7, 7>::Identity(), false));
StateFeedbackObserver<2, 4, 7> observer(std::move(v_observer));
StateFeedbackLoop<2, 4, 7> test_loop(
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 86e608c..5a450ec 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -7,10 +7,9 @@
#include <utility>
#include "Eigen/Dense"
-
-#include "frc971/control_loops/control_loop.h"
#include "aos/util/trapezoid_profile.h"
#include "frc971/constants.h"
+#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
@@ -154,16 +153,23 @@
// Updates our estimator with the latest position.
void Correct(const typename ZeroingEstimator::Position &position);
- // Runs the controller and profile generator for a cycle.
- void Update(bool disabled);
+ // Runs the controller and profile generator for a cycle. This is equivilent
+ // to calling UpdateObserver(UpdateController()) with the rest of the syntax
+ // actually right.
+ double Update(bool disabled);
+ // Just computes the controller and pushes the feed forwards forwards 1 step.
+ double UpdateController(bool disabled);
+ // Updates the observer with the computed U.
+ // Note: if this is the only method called, ForceGoal should also be called to
+ // move the state to match.
+ void UpdateObserver(double voltage);
// Fills out the ProfiledJointStatus structure with the current state.
template <class StatusTypeBuilder>
- StatusTypeBuilder BuildStatus(
- flatbuffers::FlatBufferBuilder *fbb);
+ StatusTypeBuilder BuildStatus(flatbuffers::FlatBufferBuilder *fbb);
// Forces the current goal to the provided goal, bypassing the profiler.
- void ForceGoal(double goal);
+ void ForceGoal(double goal, double goal_velocity = 0.0);
// Sets whether to use the trapezoidal profiler or whether to just bypass it
// and pass the unprofiled goal through directly.
void set_enable_profile(bool enable) { enable_profile_ = enable; }
@@ -183,7 +189,7 @@
// Returns the requested voltage.
double voltage() const { return this->loop_->U(0, 0); }
- // Returns the current position.
+ // Returns the current position or velocity.
double position() const { return this->Y_(0, 0); }
// For testing:
@@ -285,10 +291,10 @@
builder.add_estimator_state(estimator_state);
Eigen::Matrix<double, 3, 1> error = this->controller().error();
- builder.add_position_power(
- this->controller().controller().K(0, 0) * error(0, 0));
- builder.add_velocity_power(
- this->controller().controller().K(0, 1) * error(1, 0));
+ builder.add_position_power(this->controller().controller().K(0, 0) *
+ error(0, 0));
+ builder.add_velocity_power(this->controller().controller().K(0, 1) *
+ error(1, 0));
return builder;
}
@@ -341,8 +347,9 @@
}
template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::ForceGoal(double goal) {
- set_unprofiled_goal(goal, 0.0, false);
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::ForceGoal(
+ double goal, double goal_velocity) {
+ set_unprofiled_goal(goal, goal_velocity, false);
this->loop_->mutable_R() = this->unprofiled_goal_;
this->loop_->mutable_next_R() = this->loop_->R();
@@ -360,7 +367,8 @@
}
template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::Update(bool disable) {
+double SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateController(
+ bool disable) {
// TODO(austin): What do we want to do with the profile on reset? Also, we
// should probably reset R, the offset, the profile, etc.
if (this->should_reset_) {
@@ -389,12 +397,28 @@
CapGoal("next R", &this->loop_->mutable_next_R());
}
- this->loop_->Update(disable);
+ this->loop_->UpdateController(disable);
if (!disable && this->loop_->U(0, 0) != this->loop_->U_uncapped(0, 0)) {
const ::Eigen::Matrix<double, 3, 1> &R = this->loop_->R();
profile_.MoveCurrentState(R.block<2, 1>(0, 0));
}
+
+ return this->loop_->U(0, 0);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateObserver(
+ double voltage) {
+ this->loop_->mutable_U(0, 0) = voltage;
+ this->loop_->UpdateObserver(this->loop_->U(), this->loop_->plant().dt());
+}
+
+template <class ZeroingEstimator>
+double SingleDOFProfiledSubsystem<ZeroingEstimator>::Update(bool disable) {
+ const double voltage = UpdateController(disable);
+ UpdateObserver(voltage);
+ return voltage;
}
template <class ZeroingEstimator>
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 72d3297..f431983 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -266,6 +266,7 @@
name: string, The name of the loop to use when writing the C++ files.
"""
self._name = name
+ self.delayed_u = False
@property
def name(self):
@@ -290,6 +291,7 @@
self.X = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
self.Y = self.C * self.X
self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
+ self.last_U = numpy.matrix(numpy.zeros((self.B.shape[1], 1)))
def PlaceControllerPoles(self, poles):
"""Places the controller poles.
@@ -312,12 +314,21 @@
def Update(self, U):
"""Simulates one time step with the provided U."""
#U = numpy.clip(U, self.U_min, self.U_max)
- self.X = self.A * self.X + self.B * U
- self.Y = self.C * self.X + self.D * U
+ if self.delayed_u:
+ self.X = self.A * self.X + self.B * self.last_U
+ self.Y = self.C * self.X + self.D * self.last_U
+ self.last_U = U.copy()
+ else:
+ self.X = self.A * self.X + self.B * U
+ self.Y = self.C * self.X + self.D * U
def PredictObserver(self, U):
"""Runs the predict step of the observer update."""
- self.X_hat = (self.A * self.X_hat + self.B * U)
+ if self.delayed_u:
+ self.X_hat = (self.A * self.X_hat + self.B * self.last_U)
+ self.last_U = U.copy()
+ else:
+ self.X_hat = (self.A * self.X_hat + self.B * U)
def CorrectObserver(self, U):
"""Runs the correct step of the observer update."""
@@ -325,16 +336,12 @@
KalmanGain = self.KalmanGain
else:
KalmanGain = numpy.linalg.inv(self.A) * self.L
- self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat - self.D * U)
-
- def UpdateObserver(self, U):
- """Updates the observer given the provided U."""
- if hasattr(self, 'KalmanGain'):
- KalmanGain = self.KalmanGain
+ if self.delayed_u:
+ self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat -
+ self.D * self.last_U)
else:
- KalmanGain = numpy.linalg.inv(self.A) * self.L
- self.X_hat = (self.A * self.X_hat + self.B * U + self.A * KalmanGain *
- (self.Y - self.C * self.X_hat - self.D * U))
+ self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat -
+ self.D * U)
def _DumpMatrix(self, matrix_name, matrix, scalar_type):
"""Dumps the provided matrix into a variable called matrix_name.
@@ -389,6 +396,7 @@
ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
+ delayed_u_string = "true" if self.delayed_u else "false"
if plant_coefficient_type.startswith('StateFeedbackPlant'):
ans.append(self._DumpMatrix('A', self.A, scalar_type))
ans.append(self._DumpMatrix('B', self.B, scalar_type))
@@ -396,7 +404,7 @@
' const std::chrono::nanoseconds dt(%d);\n' % (self.dt * 1e9))
ans.append(
' return %s'
- '(A, B, C, D, U_max, U_min, dt);\n' % (plant_coefficient_type))
+ '(A, B, C, D, U_max, U_min, dt, %s);\n' % (plant_coefficient_type, delayed_u_string))
elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
ans.append(
self._DumpMatrix('A_continuous', self.A_continuous,
@@ -405,8 +413,8 @@
self._DumpMatrix('B_continuous', self.B_continuous,
scalar_type))
ans.append(' return %s'
- '(A_continuous, B_continuous, C, D, U_max, U_min);\n' %
- (plant_coefficient_type))
+ '(A_continuous, B_continuous, C, D, U_max, U_min, %s);\n' %
+ (plant_coefficient_type, delayed_u_string))
else:
glog.fatal('Unsupported plant type %s', plant_coefficient_type)
@@ -484,6 +492,7 @@
'%s %s {\n' % (observer_coefficient_type, self.ObserverFunction())
]
+ delayed_u_string = "true" if self.delayed_u else "false"
if observer_coefficient_type.startswith('StateFeedbackObserver'):
if hasattr(self, 'KalmanGain'):
KalmanGain = self.KalmanGain
@@ -496,8 +505,8 @@
ans.append(self._DumpMatrix('KalmanGain', KalmanGain, scalar_type))
ans.append(self._DumpMatrix('Q', Q, scalar_type))
ans.append(self._DumpMatrix('R', R, scalar_type))
- ans.append(' return %s(KalmanGain, Q, R);\n' %
- (observer_coefficient_type, ))
+ ans.append(' return %s(KalmanGain, Q, R, %s);\n' %
+ (observer_coefficient_type, delayed_u_string))
elif observer_coefficient_type.startswith('HybridKalman'):
ans.append(
@@ -510,8 +519,8 @@
self._DumpMatrix('P_steady_state', self.P_steady_state,
scalar_type))
ans.append(
- ' return %s(Q_continuous, R_continuous, P_steady_state);\n' %
- (observer_coefficient_type, ))
+ ' return %s(Q_continuous, R_continuous, P_steady_state, %s);\n' %
+ (observer_coefficient_type, delayed_u_string))
else:
glog.fatal('Unsupported observer type %s',
observer_coefficient_type)
@@ -531,7 +540,12 @@
def PredictHybridObserver(self, U, dt):
self.Discretize(dt)
- self.X_hat = self.A * self.X_hat + self.B * U
+ if self.delayed_u:
+ self.X_hat = self.A * self.X_hat + self.B * self.last_U
+ self.last_U = U.copy()
+ else:
+ self.X_hat = self.A * self.X_hat + self.B * U
+
self.P = (self.A * self.P * self.A.T + self.Q)
def CorrectHybridObserver(self, U):
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 4069036..264b4a6 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -544,7 +544,8 @@
for _ in range(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
- drivetrain.UpdateObserver(U)
+ drivetrain.CorrectObserver(U)
+ drivetrain.PredictObserver(U)
drivetrain.Update(U + numpy.matrix([[1.0], [1.0]]))
close_loop_left.append(drivetrain.X[0, 0])
close_loop_right.append(drivetrain.X[2, 0])
@@ -588,7 +589,8 @@
for _ in range(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
- drivetrain.UpdateObserver(U)
+ drivetrain.CorrectObserver(U)
+ drivetrain.PredictObserver(U)
drivetrain.Update(U)
close_loop_left.append(drivetrain.X[0, 0])
close_loop_right.append(drivetrain.X[2, 0])
@@ -612,7 +614,8 @@
for _ in range(200):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
- drivetrain.UpdateObserver(U)
+ drivetrain.CorrectObserver(U)
+ drivetrain.PredictObserver(U)
drivetrain.Update(U)
close_loop_left.append(drivetrain.X[0, 0])
close_loop_right.append(drivetrain.X[2, 0])
@@ -633,7 +636,8 @@
for _ in range(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
- drivetrain.UpdateObserver(U)
+ drivetrain.CorrectObserver(U)
+ drivetrain.PredictObserver(U)
drivetrain.Update(U)
close_loop_left.append(drivetrain.X[0, 0])
close_loop_right.append(drivetrain.X[2, 0])
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index f591847..53cd6a2 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -39,7 +39,8 @@
D(other.D),
U_min(other.U_min),
U_max(other.U_max),
- dt(other.dt) {}
+ dt(other.dt),
+ delayed_u(other.delayed_u) {}
StateFeedbackPlantCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A,
@@ -48,8 +49,15 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min,
- const std::chrono::nanoseconds dt)
- : A(A), B(B), C(C), D(D), U_min(U_min), U_max(U_max), dt(dt) {}
+ const std::chrono::nanoseconds dt, bool delayed_u)
+ : A(A),
+ B(B),
+ C(C),
+ D(D),
+ U_min(U_min),
+ U_max(U_max),
+ dt(dt),
+ delayed_u(delayed_u) {}
const Eigen::Matrix<Scalar, number_of_states, number_of_states> A;
const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B;
@@ -58,6 +66,12 @@
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
const std::chrono::nanoseconds dt;
+
+ // If true, this adds a single cycle output delay model to the plant. This is
+ // useful for modeling a control loop cycle where you sample, compute, and
+ // then queue the outputs to be ready to be executed when the next cycle
+ // happens.
+ const bool delayed_u;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -78,6 +92,7 @@
::std::swap(coefficients_, other.coefficients_);
X_.swap(other.X_);
Y_.swap(other.Y_);
+ last_U_.swap(other.last_U_);
}
virtual ~StateFeedbackPlant() {}
@@ -143,6 +158,7 @@
void Reset() {
X_.setZero();
Y_.setZero();
+ last_U_.setZero();
}
// Assert that U is within the hardware range.
@@ -164,8 +180,14 @@
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
- X_ = Update(X(), U);
- UpdateY(U);
+ if (coefficients().delayed_u) {
+ X_ = Update(X(), last_U_);
+ UpdateY(last_U_);
+ last_U_ = U;
+ } else {
+ X_ = Update(X(), U);
+ UpdateY(U);
+ }
}
// Computes the new Y given the control input.
@@ -188,6 +210,7 @@
private:
Eigen::Matrix<Scalar, number_of_states, 1> X_;
Eigen::Matrix<Scalar, number_of_outputs, 1> Y_;
+ Eigen::Matrix<Scalar, number_of_inputs, 1> last_U_;
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
@@ -283,12 +306,19 @@
const Eigen::Matrix<Scalar, number_of_states, number_of_states> Q;
const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R;
+ // If true, this adds a single cycle output delay model to the plant. This is
+ // useful for modeling a control loop cycle where you sample, compute, and
+ // then queue the outputs to be ready to be executed when the next cycle
+ // happens.
+ const bool delayed_u;
+
StateFeedbackObserverCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_outputs>
&KalmanGain,
const Eigen::Matrix<Scalar, number_of_states, number_of_states> &Q,
- const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R)
- : KalmanGain(KalmanGain), Q(Q), R(R) {}
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R,
+ bool delayed_u)
+ : KalmanGain(KalmanGain), Q(Q), R(R), delayed_u(delayed_u) {}
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -304,7 +334,7 @@
: coefficients_(::std::move(observers)) {}
StateFeedbackObserver(StateFeedbackObserver &&other)
- : X_hat_(other.X_hat_), index_(other.index_) {
+ : X_hat_(other.X_hat_), last_U_(other.last_U_), index_(other.index_) {
::std::swap(coefficients_, other.coefficients_);
}
@@ -319,16 +349,26 @@
}
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &last_U() const {
+ return last_U_;
+ }
+
void Reset(StateFeedbackPlant<number_of_states, number_of_inputs,
number_of_outputs, Scalar> * /*loop*/) {
X_hat_.setZero();
+ last_U_.setZero();
}
void Predict(StateFeedbackPlant<number_of_states, number_of_inputs,
number_of_outputs, Scalar> *plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds /*dt*/) {
- mutable_X_hat() = plant->Update(X_hat(), new_u);
+ if (plant->coefficients().delayed_u) {
+ mutable_X_hat() = plant->Update(X_hat(), last_U_);
+ last_U_ = new_u;
+ } else {
+ mutable_X_hat() = plant->Update(X_hat(), new_u);
+ }
}
void Correct(const StateFeedbackPlant<number_of_states, number_of_inputs,
@@ -366,6 +406,7 @@
private:
// Internal state estimate.
Eigen::Matrix<Scalar, number_of_states, 1> X_hat_;
+ Eigen::Matrix<Scalar, number_of_inputs, 1> last_U_;
int index_ = 0;
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index ea47580..04d93c4 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -76,17 +76,51 @@
// Resets constrained min/max position
void clear_min_position() { min_position_ = params_.range.lower_hard; }
-
void clear_max_position() { max_position_ = params_.range.upper_hard; }
+ // Sets the unprofiled goal which UpdateController will go to.
+ void set_unprofiled_goal(double position, double velocity);
+ // Changes the profile parameters for UpdateController to track.
+ void AdjustProfile(double velocity, double acceleration);
+
// Returns the current position
double position() const { return profiled_subsystem_.position(); }
+ Eigen::Vector3d estimated_state() const {
+ return profiled_subsystem_.X_hat();
+ }
+ double estimated_position() const { return profiled_subsystem_.X_hat(0, 0); }
+ double estimated_velocity() const { return profiled_subsystem_.X_hat(1, 0); }
+
+ // Corrects the internal state, adjusts limits, and sets nominal goals.
+ // Returns true if the controller should run.
+ bool Correct(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position,
+ bool disabled);
+
+ // Computes the feedback and feed forward steps for the current iteration.
+ // disabled should be true if the controller is disabled from Correct or
+ // another source.
+ double UpdateController(bool disabled);
+
+ // Predicts the observer state with the applied voltage.
+ void UpdateObserver(double voltage);
+
+ // Returns the current status.
+ flatbuffers::Offset<ProfiledJointStatus> MakeStatus(
+ flatbuffers::FlatBufferBuilder *status_fbb);
+
+ // Iterates the controller with the provided goal.
flatbuffers::Offset<ProfiledJointStatus> Iterate(
const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position, double *output,
flatbuffers::FlatBufferBuilder *status_fbb);
+ // Sets the current profile state to solve from. Useful for when an external
+ // controller gives back control and we want the trajectory generator to
+ // take over control again.
+ void ForceGoal(double goal, double goal_velocity);
+
// Resets the profiled subsystem and returns to uninitialized
void Reset();
@@ -109,6 +143,13 @@
State state() const { return state_; }
+ bool running() const { return state_ == State::RUNNING; }
+
+ // Returns the controller.
+ const StateFeedbackLoop<3, 1, 1> &controller() const {
+ return profiled_subsystem_.controller();
+ }
+
private:
State state_ = State::UNINITIALIZED;
double min_position_, max_position_;
@@ -151,15 +192,12 @@
template <typename ZeroingEstimator, typename ProfiledJointStatus,
typename SubsystemParams>
-flatbuffers::Offset<ProfiledJointStatus>
-StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::
- Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
- const typename ZeroingEstimator::Position *position, double *output,
- flatbuffers::FlatBufferBuilder *status_fbb) {
+bool StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams>::
+ Correct(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position,
+ bool disabled) {
CHECK_NOTNULL(position);
- CHECK_NOTNULL(status_fbb);
- bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
if (profiled_subsystem_.error()) {
@@ -213,31 +251,17 @@
if (goal) {
if (goal->profile_params()) {
- profiled_subsystem_.AdjustProfile(
- goal->profile_params()->max_velocity(),
- std::min(goal->profile_params()->max_acceleration(),
- max_acceleration_));
+ AdjustProfile(goal->profile_params()->max_velocity(),
+ goal->profile_params()->max_acceleration());
} else {
- profiled_subsystem_.AdjustProfile(
- profiled_subsystem_.default_velocity(),
- std::min(profiled_subsystem_.default_acceleration(),
- static_cast<double>(max_acceleration_)));
+ AdjustProfile(profiled_subsystem_.default_velocity(),
+ profiled_subsystem_.default_acceleration());
}
- double safe_goal = goal->unsafe_goal();
- if (safe_goal < min_position_) {
- VLOG(1) << "Limiting to " << min_position_ << " from " << safe_goal;
- safe_goal = min_position_;
- }
- if (safe_goal > max_position_) {
- VLOG(1) << "Limiting to " << max_position_ << " from " << safe_goal;
- safe_goal = max_position_;
- }
if (goal->has_ignore_profile()) {
profiled_subsystem_.set_enable_profile(!goal->ignore_profile());
}
- profiled_subsystem_.set_unprofiled_goal(safe_goal,
- goal->goal_velocity());
+ set_unprofiled_goal(goal->unsafe_goal(), goal->goal_velocity());
}
} break;
@@ -254,14 +278,89 @@
profiled_subsystem_.set_max_voltage({{max_voltage}});
+ return disabled;
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+void StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::set_unprofiled_goal(double goal, double goal_velocity) {
+ if (goal < min_position_) {
+ VLOG(1) << "Limiting to " << min_position_ << " from " << goal;
+ goal = min_position_;
+ }
+ if (goal > max_position_) {
+ VLOG(1) << "Limiting to " << max_position_ << " from " << goal;
+ goal = max_position_;
+ }
+ profiled_subsystem_.set_unprofiled_goal(goal, goal_velocity);
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+void StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::AdjustProfile(double velocity, double acceleration) {
+ profiled_subsystem_.AdjustProfile(
+ velocity, std::min(acceleration, static_cast<double>(max_acceleration_)));
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+flatbuffers::Offset<ProfiledJointStatus>
+StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::
+ Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position, double *output,
+ flatbuffers::FlatBufferBuilder *status_fbb) {
+ const bool disabled = Correct(goal, position, output == nullptr);
+
// Calculate the loops for a cycle.
- profiled_subsystem_.Update(disabled);
+ const double voltage = UpdateController(disabled);
+
+ UpdateObserver(voltage);
// Write out all the voltages.
if (output) {
- *output = profiled_subsystem_.voltage();
+ *output = voltage;
}
+ return MakeStatus(status_fbb);
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+double StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::UpdateController(bool disabled) {
+ return profiled_subsystem_.UpdateController(disabled);
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+void StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::UpdateObserver(double voltage) {
+ profiled_subsystem_.UpdateObserver(voltage);
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+void StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::ForceGoal(double goal, double goal_velocity) {
+ profiled_subsystem_.ForceGoal(goal, goal_velocity);
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+flatbuffers::Offset<ProfiledJointStatus>
+StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::MakeStatus(flatbuffers::FlatBufferBuilder *status_fbb) {
+ CHECK_NOTNULL(status_fbb);
+
typename ProfiledJointStatus::Builder status_builder =
profiled_subsystem_
.template BuildStatus<typename ProfiledJointStatus::Builder>(
diff --git a/frc971/raspi/rootfs/make_sd.sh b/frc971/raspi/rootfs/make_sd.sh
index 3bafacc..4c50a6b 100755
--- a/frc971/raspi/rootfs/make_sd.sh
+++ b/frc971/raspi/rootfs/make_sd.sh
@@ -34,9 +34,10 @@
# Put a timestamp on when this card got created and by whom
TIMESTAMP_FILE="${PARTITION}/home/pi/.DiskFlashedDate.txt"
-date > "${TIMESTAMP_FILE}"
-git rev-parse HEAD >> "${TIMESTAMP_FILE}"
-whoami >> "${TIMESTAMP_FILE}"
+echo "Date Imaged: "`date` > "${TIMESTAMP_FILE}"
+echo "Image file: ${IMAGE}" >> "${TIMESTAMP_FILE}"
+echo "Git tag: "`git rev-parse HEAD` >> "${TIMESTAMP_FILE}"
+echo "User: "`whoami` >> "${TIMESTAMP_FILE}"
echo "Starting a shell for any manual configuration"
target /bin/bash --rcfile /root/.bashrc
diff --git a/frc971/raspi/rootfs/modify_rootfs.sh b/frc971/raspi/rootfs/modify_rootfs.sh
index 0020f35..fd9bec9 100755
--- a/frc971/raspi/rootfs/modify_rootfs.sh
+++ b/frc971/raspi/rootfs/modify_rootfs.sh
@@ -102,9 +102,9 @@
# Add a file to show when this image was last modified and by whom
TIMESTAMP_FILE="${PARTITION}/home/pi/.ImageModifiedDate.txt"
-date > "${TIMESTAMP_FILE}"
-git rev-parse HEAD >> "${TIMESTAMP_FILE}"
-whoami >> "${TIMESTAMP_FILE}"
+echo "Date modified:"`date` > "${TIMESTAMP_FILE}"
+echo "Git tag: "`git rev-parse HEAD` >> "${TIMESTAMP_FILE}"
+echo "User: "`whoami` >> "${TIMESTAMP_FILE}"
# Run a prompt as root inside the target to poke around and check things.
target /bin/bash --rcfile /root/.bashrc
diff --git a/frc971/raspi/rootfs/target_configure.sh b/frc971/raspi/rootfs/target_configure.sh
index db3fb36..6d9171e 100755
--- a/frc971/raspi/rootfs/target_configure.sh
+++ b/frc971/raspi/rootfs/target_configure.sh
@@ -41,6 +41,14 @@
libnice-dev \
feh
+# Install WiringPi gpio for PWM control
+if [[ ! -e "/usr/bin/gpio" ]]; then
+ cd /tmp
+ git clone https://github.com/WiringPi/WiringPi.git
+ cd WiringPi
+ ./build
+fi
+
echo 'GOVERNOR="performance"' > /etc/default/cpufrequtils
# Add a .bashrc and friends for root.
diff --git a/scouting/BUILD b/scouting/BUILD
index 836e5c3..8a3b3f0 100644
--- a/scouting/BUILD
+++ b/scouting/BUILD
@@ -1,4 +1,4 @@
-load("@io_bazel_rules_go//go:def.bzl", "go_binary", "go_library")
+load("@io_bazel_rules_go//go:def.bzl", "go_binary", "go_library", "go_test")
go_binary(
name = "sql_demo",
@@ -15,3 +15,19 @@
visibility = ["//visibility:private"],
deps = ["@com_github_mattn_go_sqlite3//:go-sqlite3"],
)
+
+go_library(
+ name = "database",
+ srcs = ["db.go"],
+ importpath = "github.com/frc971/971-Robot-Code/scouting",
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ visibility = ["//visibility:private"],
+ deps = ["@com_github_mattn_go_sqlite3//:go_default_library"],
+)
+
+go_test(
+ name = "db_test",
+ srcs = ["db_test.go"],
+ embed = [":database"],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+)
diff --git a/scouting/db.go b/scouting/db.go
new file mode 100644
index 0000000..86d332e
--- /dev/null
+++ b/scouting/db.go
@@ -0,0 +1,183 @@
+package db
+
+import (
+ "database/sql"
+ "fmt"
+
+ _ "github.com/mattn/go-sqlite3"
+)
+
+type Database struct {
+ *sql.DB
+}
+
+type Match struct {
+ matchNumber, round int
+ compLevel string
+ r1, r2, r3, b1, b2, b3 int
+ // Each of these variables holds the matchID of the corresponding Stats row
+ r1ID, r2ID, r3ID, b1ID, b2ID, b3ID int
+}
+
+type Stats struct {
+ teamNumber, matchNumber int
+ shotsMissed, upperGoalShots, lowerGoalShots int
+ shotsMissedAuto, upperGoalAuto, lowerGoalAuto, playedDefense int
+ climbing int
+}
+
+func NewDatabase() (*Database, error) {
+ database := new(Database)
+ database.DB, _ = sql.Open("sqlite3", "./scouting.db")
+ statement, error_ := database.Prepare("CREATE TABLE IF NOT EXISTS matches " +
+ "(id INTEGER PRIMARY KEY, matchNumber INTEGER, round INTEGER, compLevel INTEGER, r1 INTEGER, r2 INTEGER, r3 INTEGER, b1 INTEGER, b2 INTEGER, b3 INTEGER, r1ID INTEGER, r2ID INTEGER, r3ID INTEGER, b1ID INTEGER, b2ID INTEGER, b3ID INTEGER)")
+ defer statement.Close()
+ if error_ != nil {
+ fmt.Println(error_)
+ return nil, error_
+ }
+ _, error_ = statement.Exec()
+ statement, error_ = database.Prepare("CREATE TABLE IF NOT EXISTS team_match_stats (id INTEGER PRIMARY KEY, teamNumber INTEGER, matchNumber DOUBLE, shotsMissed INTEGER, upperGoalShots INTEGER, lowerGoalShots INTEGER, shotsMissedAuto INTEGER, upperGoalAuto INTEGER, lowerGoalAuto INTEGER, playedDefense INTEGER, climbing INTEGER)")
+ defer statement.Close()
+ if error_ != nil {
+ fmt.Println(error_)
+ return nil, error_
+ }
+ _, error_ = statement.Exec()
+ return database, nil
+}
+
+func (database *Database) Delete() error {
+ statement, error_ := database.Prepare("DROP TABLE IF EXISTS matches")
+ if error_ != nil {
+ fmt.Println(error_)
+ return (error_)
+ }
+ _, error_ = statement.Exec()
+ statement, error_ = database.Prepare("DROP TABLE IF EXISTS team_match_stats")
+ if error_ != nil {
+ fmt.Println(error_)
+ return (error_)
+ }
+ _, error_ = statement.Exec()
+ return nil
+}
+
+// This function will also populate the Stats table with six empty rows every time a match is added
+func (database *Database) AddToMatch(m Match) error {
+ statement, error_ := database.Prepare("INSERT INTO team_match_stats(teamNumber, matchNumber, shotsMissed, upperGoalShots, lowerGoalShots, shotsMissedAuto, upperGoalAuto, lowerGoalAuto, playedDefense, climbing) VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?)")
+ defer statement.Close()
+ if error_ != nil {
+ fmt.Println("failed to prepare stats database:", error_)
+ return (error_)
+ }
+ var rowIds [6]int64
+ for i, teamNumber := range []int{m.r1, m.r2, m.r3, m.b1, m.b2, m.b3} {
+ result, error_ := statement.Exec(teamNumber, m.matchNumber, 0, 0, 0, 0, 0, 0, 0, 0)
+ if error_ != nil {
+ fmt.Println("failed to execute statement 2:", error_)
+ return (error_)
+ }
+ rowIds[i], error_ = result.LastInsertId()
+ }
+ statement, error_ = database.Prepare("INSERT INTO matches(matchNumber, round, compLevel, r1, r2, r3, b1, b2, b3, r1ID, r2ID, r3ID, b1ID, b2ID, b3ID) VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)")
+ defer statement.Close()
+ if error_ != nil {
+ fmt.Println("failed to prepare match database:", error_)
+ return (error_)
+ }
+ _, error_ = statement.Exec(m.matchNumber, m.round, m.compLevel, m.r1, m.r2, m.r3, m.b1, m.b2, m.b3, rowIds[0], rowIds[1], rowIds[2], rowIds[3], rowIds[4], rowIds[5])
+ if error_ != nil {
+ fmt.Println(error_)
+ return (error_)
+ }
+ return nil
+}
+
+func (database *Database) AddToStats(s Stats) error {
+ statement, error_ := database.Prepare("UPDATE team_match_stats SET teamNumber = ?, matchNumber = ?, shotsMissed = ?, upperGoalShots = ?, lowerGoalShots = ?, shotsMissedAuto = ?, upperGoalAuto = ?, lowerGoalAuto = ?, playedDefense = ?, climbing = ? WHERE matchNumber = ? AND teamNumber = ?")
+ if error_ != nil {
+ fmt.Println(error_)
+ return (error_)
+ }
+ _, error_ = statement.Exec(s.teamNumber, s.matchNumber, s.shotsMissed, s.upperGoalShots, s.lowerGoalShots, s.shotsMissedAuto, s.upperGoalAuto, s.lowerGoalAuto, s.playedDefense, s.climbing, s.matchNumber, s.teamNumber)
+ if error_ != nil {
+ fmt.Println(error_)
+ return (error_)
+ }
+ return nil
+}
+
+func (database *Database) ReturnMatches() ([]Match, error) {
+ matches := make([]Match, 0)
+ rows, _ := database.Query("SELECT * FROM matches")
+ defer rows.Close()
+ for rows.Next() {
+ var match Match
+ var id int
+ error_ := rows.Scan(&id, &match.matchNumber, &match.round, &match.compLevel, &match.r1, &match.r2, &match.r3, &match.b1, &match.b2, &match.b3, &match.r1ID, &match.r2ID, &match.r3ID, &match.b1ID, &match.b2ID, &match.b3ID)
+ if error_ != nil {
+ fmt.Println(nil, error_)
+ return nil, error_
+ }
+ matches = append(matches, match)
+ }
+ return matches, nil
+}
+
+func (database *Database) ReturnStats() ([]Stats, error) {
+ rows, _ := database.Query("SELECT * FROM team_match_stats")
+ defer rows.Close()
+ teams := make([]Stats, 0)
+ var id int
+ for rows.Next() {
+ var team Stats
+ error_ := rows.Scan(&id, &team.teamNumber, &team.matchNumber, &team.shotsMissed, &team.upperGoalShots, &team.lowerGoalShots, &team.shotsMissedAuto, &team.upperGoalAuto, &team.lowerGoalAuto, &team.playedDefense, &team.climbing)
+ if error_ != nil {
+ fmt.Println(error_)
+ return nil, error_
+ }
+ teams = append(teams, team)
+ }
+ return teams, nil
+}
+
+func (database *Database) QueryMatches(teamNumber_ int) ([]Match, error) {
+ rows, error_ := database.Query("SELECT * FROM matches WHERE r1 = ? OR r2 = ? OR r3 = ? OR b1 = ? OR b2 = ? OR b3 = ?", teamNumber_, teamNumber_, teamNumber_, teamNumber_, teamNumber_, teamNumber_)
+ if error_ != nil {
+ fmt.Println("failed to execute statement 1:", error_)
+ return nil, error_
+ }
+ defer rows.Close()
+ var matches []Match
+ var id int
+ for rows.Next() {
+ var match Match
+ rows.Scan(&id, &match.matchNumber, &match.round, &match.compLevel, &match.r1, &match.r2, &match.r3, &match.b1, &match.b2, &match.b3, &match.r1ID, &match.r2ID, &match.r3ID, &match.b1ID, &match.b2ID, &match.b3ID)
+ matches = append(matches, match)
+ }
+ return matches, nil
+}
+
+func (database *Database) QueryStats(teamNumber_ int) ([]Stats, error) {
+ rows, error_ := database.Query("SELECT * FROM team_match_stats WHERE teamNumber = ?", teamNumber_)
+ if error_ != nil {
+ fmt.Println("failed to execute statement 3:", error_)
+ return nil, error_
+ }
+ defer rows.Close()
+ var teams []Stats
+ for rows.Next() {
+ var team Stats
+ var id int
+ error_ = rows.Scan(&id, &team.teamNumber, &team.matchNumber, &team.shotsMissed,
+ &team.upperGoalShots, &team.lowerGoalShots, &team.shotsMissedAuto, &team.upperGoalAuto,
+ &team.lowerGoalAuto, &team.playedDefense, &team.climbing)
+ teams = append(teams, team)
+ }
+ if error_ != nil {
+ fmt.Println("failed to execute statement 3:", error_)
+ return nil, error_
+ }
+ return teams, nil
+}
diff --git a/scouting/db_test.go b/scouting/db_test.go
new file mode 100644
index 0000000..3c6d9c5
--- /dev/null
+++ b/scouting/db_test.go
@@ -0,0 +1,163 @@
+package db
+
+import (
+ "fmt"
+ "reflect"
+ "testing"
+)
+
+func TestAddToMatchDB(t *testing.T) {
+ db, error_ := NewDatabase()
+ if error_ != nil {
+ t.Fatalf(error_.Error())
+ }
+ correct := []Match{Match{matchNumber: 7, round: 1, compLevel: "quals", r1: 9999, r2: 1000, r3: 777, b1: 0000, b2: 4321, b3: 1234, r1ID: 1, r2ID: 2, r3ID: 3, b1ID: 4, b2ID: 5, b3ID: 6}}
+ db.AddToMatch(correct[0])
+ got, error_ := db.ReturnMatches()
+ if error_ != nil {
+ t.Fatalf(error_.Error())
+ }
+ if !reflect.DeepEqual(correct, got) {
+ t.Fatalf("Got %#v,\nbut expected %#v.", got, correct)
+ }
+ db.Delete()
+}
+
+func TestAddToStatsDB(t *testing.T) {
+ db, error_ := NewDatabase()
+ if error_ != nil {
+ t.Fatalf(error_.Error())
+ }
+ correct := []Stats{
+ Stats{teamNumber: 1236, matchNumber: 7, shotsMissed: 9, upperGoalShots: 5, lowerGoalShots: 4, shotsMissedAuto: 3, upperGoalAuto: 2, lowerGoalAuto: 1, playedDefense: 2, climbing: 3},
+ Stats{teamNumber: 1001, matchNumber: 7, shotsMissed: 6, upperGoalShots: 9, lowerGoalShots: 9, shotsMissedAuto: 0, upperGoalAuto: 0, lowerGoalAuto: 0, playedDefense: 0, climbing: 0},
+ Stats{teamNumber: 777, matchNumber: 7, shotsMissed: 5, upperGoalShots: 7, lowerGoalShots: 12, shotsMissedAuto: 0, upperGoalAuto: 4, lowerGoalAuto: 0, playedDefense: 0, climbing: 0},
+ Stats{teamNumber: 1000, matchNumber: 7, shotsMissed: 12, upperGoalShots: 6, lowerGoalShots: 10, shotsMissedAuto: 0, upperGoalAuto: 7, lowerGoalAuto: 0, playedDefense: 0, climbing: 0},
+ Stats{teamNumber: 4321, matchNumber: 7, shotsMissed: 14, upperGoalShots: 12, lowerGoalShots: 3, shotsMissedAuto: 0, upperGoalAuto: 7, lowerGoalAuto: 0, playedDefense: 0, climbing: 0},
+ Stats{teamNumber: 1234, matchNumber: 7, shotsMissed: 3, upperGoalShots: 4, lowerGoalShots: 0, shotsMissedAuto: 0, upperGoalAuto: 9, lowerGoalAuto: 0, playedDefense: 0, climbing: 0},
+ }
+ db.AddToMatch(Match{matchNumber: 7, round: 1, compLevel: "quals", r1: 1236, r2: 1001, r3: 777, b1: 1000, b2: 4321, b3: 1234, r1ID: 1, r2ID: 2, r3ID: 3, b1ID: 4, b2ID: 5, b3ID: 6})
+ for i := 0; i < len(correct); i++ {
+ db.AddToStats(correct[i])
+ }
+ got, error_ := db.ReturnStats()
+ if error_ != nil {
+ t.Fatalf(error_.Error())
+ }
+ if !reflect.DeepEqual(correct, got) {
+ t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
+ }
+ db.Delete()
+}
+
+func TestQueryMatchDB(t *testing.T) {
+ db, error_ := NewDatabase()
+ if error_ != nil {
+ t.Fatalf(error_.Error())
+ fmt.Println("Error creating new database")
+ }
+
+ testDatabase := []Match{
+ Match{matchNumber: 2, round: 1, compLevel: "quals", r1: 251, r2: 169, r3: 286, b1: 253, b2: 538, b3: 149},
+ Match{matchNumber: 4, round: 1, compLevel: "quals", r1: 198, r2: 135, r3: 777, b1: 999, b2: 434, b3: 698},
+ Match{matchNumber: 3, round: 1, compLevel: "quals", r1: 147, r2: 421, r3: 538, b1: 126, b2: 448, b3: 262},
+ Match{matchNumber: 6, round: 1, compLevel: "quals", r1: 191, r2: 132, r3: 773, b1: 994, b2: 435, b3: 696},
+ }
+
+ for i := 0; i < len(testDatabase); i++ {
+ db.AddToMatch(testDatabase[i])
+ }
+
+ correct := []Match{
+ Match{matchNumber: 2, round: 1, compLevel: "quals", r1: 251, r2: 169, r3: 286, b1: 253, b2: 538, b3: 149, r1ID: 1, r2ID: 2, r3ID: 3, b1ID: 4, b2ID: 5, b3ID: 6},
+ Match{matchNumber: 3, round: 1, compLevel: "quals", r1: 147, r2: 421, r3: 538, b1: 126, b2: 448, b3: 262, r1ID: 13, r2ID: 14, r3ID: 15, b1ID: 16, b2ID: 17, b3ID: 18},
+ }
+
+ got, error_ := db.QueryMatches(538)
+ if !reflect.DeepEqual(correct, got) {
+ t.Fatalf("Got %#v,\nbut expected %#v.", got, correct)
+ }
+ db.Delete()
+}
+
+func TestQueryStatsDB(t *testing.T) {
+ db, error_ := NewDatabase()
+ if error_ != nil {
+ t.Fatalf(error_.Error())
+ }
+ testDatabase := []Stats{
+ Stats{teamNumber: 1235, matchNumber: 94, shotsMissed: 2, upperGoalShots: 2, lowerGoalShots: 2, shotsMissedAuto: 2, upperGoalAuto: 2, lowerGoalAuto: 2, playedDefense: 2, climbing: 2},
+ Stats{teamNumber: 1234, matchNumber: 94, shotsMissed: 4, upperGoalShots: 4, lowerGoalShots: 4, shotsMissedAuto: 4, upperGoalAuto: 4, lowerGoalAuto: 4, playedDefense: 7, climbing: 2},
+ Stats{teamNumber: 1233, matchNumber: 94, shotsMissed: 3, upperGoalShots: 3, lowerGoalShots: 3, shotsMissedAuto: 3, upperGoalAuto: 3, lowerGoalAuto: 3, playedDefense: 3, climbing: 3},
+ Stats{teamNumber: 1232, matchNumber: 94, shotsMissed: 5, upperGoalShots: 5, lowerGoalShots: 5, shotsMissedAuto: 5, upperGoalAuto: 5, lowerGoalAuto: 5, playedDefense: 7, climbing: 1},
+ Stats{teamNumber: 1231, matchNumber: 94, shotsMissed: 6, upperGoalShots: 6, lowerGoalShots: 6, shotsMissedAuto: 6, upperGoalAuto: 6, lowerGoalAuto: 6, playedDefense: 7, climbing: 1},
+ Stats{teamNumber: 1239, matchNumber: 94, shotsMissed: 7, upperGoalShots: 7, lowerGoalShots: 7, shotsMissedAuto: 7, upperGoalAuto: 7, lowerGoalAuto: 3, playedDefense: 7, climbing: 1},
+ }
+ db.AddToMatch(Match{matchNumber: 94, round: 1, compLevel: "quals", r1: 1235, r2: 1234, r3: 1233, b1: 1232, b2: 1231, b3: 1239})
+ for i := 0; i < len(testDatabase); i++ {
+ db.AddToStats(testDatabase[i])
+ }
+ correct := []Stats{
+ Stats{teamNumber: 1235, matchNumber: 94, shotsMissed: 2, upperGoalShots: 2, lowerGoalShots: 2, shotsMissedAuto: 2, upperGoalAuto: 2, lowerGoalAuto: 2, playedDefense: 2, climbing: 2},
+ }
+ got, error_ := db.QueryStats(1235)
+ if error_ != nil {
+ t.Fatalf(error_.Error())
+ }
+ if !reflect.DeepEqual(correct, got) {
+ t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
+ }
+ db.Delete()
+}
+
+func TestReturnMatchDB(t *testing.T) {
+ db, error_ := NewDatabase()
+ if error_ != nil {
+ t.Fatalf(error_.Error())
+ }
+ correct := []Match{
+ Match{matchNumber: 2, round: 1, compLevel: "quals", r1: 251, r2: 169, r3: 286, b1: 253, b2: 538, b3: 149, r1ID: 1, r2ID: 2, r3ID: 3, b1ID: 4, b2ID: 5, b3ID: 6},
+ Match{matchNumber: 3, round: 1, compLevel: "quals", r1: 147, r2: 421, r3: 538, b1: 126, b2: 448, b3: 262, r1ID: 7, r2ID: 8, r3ID: 9, b1ID: 10, b2ID: 11, b3ID: 12},
+ Match{matchNumber: 4, round: 1, compLevel: "quals", r1: 251, r2: 169, r3: 286, b1: 653, b2: 538, b3: 149, r1ID: 13, r2ID: 14, r3ID: 15, b1ID: 16, b2ID: 17, b3ID: 18},
+ Match{matchNumber: 5, round: 1, compLevel: "quals", r1: 198, r2: 1421, r3: 538, b1: 26, b2: 448, b3: 262, r1ID: 19, r2ID: 20, r3ID: 21, b1ID: 22, b2ID: 23, b3ID: 24},
+ Match{matchNumber: 6, round: 1, compLevel: "quals", r1: 251, r2: 188, r3: 286, b1: 555, b2: 538, b3: 149, r1ID: 25, r2ID: 26, r3ID: 27, b1ID: 28, b2ID: 29, b3ID: 30},
+ }
+ for i := 0; i < len(correct); i++ {
+ db.AddToMatch(correct[i])
+ }
+ got, error_ := db.ReturnMatches()
+ if error_ != nil {
+ t.Fatalf(error_.Error())
+ }
+ if !reflect.DeepEqual(correct, got) {
+ t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
+ }
+ db.Delete()
+}
+
+func TestReturnStatsDB(t *testing.T) {
+ db, error_ := NewDatabase()
+ if error_ != nil {
+ t.Fatalf(error_.Error())
+ }
+ correct := []Stats{
+ Stats{teamNumber: 1235, matchNumber: 94, shotsMissed: 2, upperGoalShots: 2, lowerGoalShots: 2, shotsMissedAuto: 2, upperGoalAuto: 2, lowerGoalAuto: 2, playedDefense: 2, climbing: 2},
+ Stats{teamNumber: 1236, matchNumber: 94, shotsMissed: 4, upperGoalShots: 4, lowerGoalShots: 4, shotsMissedAuto: 4, upperGoalAuto: 4, lowerGoalAuto: 4, playedDefense: 7, climbing: 2},
+ Stats{teamNumber: 1237, matchNumber: 94, shotsMissed: 3, upperGoalShots: 3, lowerGoalShots: 3, shotsMissedAuto: 3, upperGoalAuto: 3, lowerGoalAuto: 3, playedDefense: 3, climbing: 3},
+ Stats{teamNumber: 1238, matchNumber: 94, shotsMissed: 5, upperGoalShots: 5, lowerGoalShots: 5, shotsMissedAuto: 5, upperGoalAuto: 5, lowerGoalAuto: 5, playedDefense: 7, climbing: 1},
+ Stats{teamNumber: 1239, matchNumber: 94, shotsMissed: 6, upperGoalShots: 6, lowerGoalShots: 6, shotsMissedAuto: 6, upperGoalAuto: 6, lowerGoalAuto: 6, playedDefense: 7, climbing: 1},
+ Stats{teamNumber: 1233, matchNumber: 94, shotsMissed: 7, upperGoalShots: 7, lowerGoalShots: 7, shotsMissedAuto: 7, upperGoalAuto: 7, lowerGoalAuto: 3, playedDefense: 7, climbing: 1},
+ }
+ db.AddToMatch(Match{matchNumber: 94, round: 1, compLevel: "quals", r1: 1235, r2: 1236, r3: 1237, b1: 1238, b2: 1239, b3: 1233})
+ for i := 0; i < len(correct); i++ {
+ db.AddToStats(correct[i])
+ }
+ got, error_ := db.ReturnStats()
+ if error_ != nil {
+ t.Fatalf(error_.Error())
+ }
+ if !reflect.DeepEqual(correct, got) {
+ t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
+ }
+ db.Delete()
+}
diff --git a/tools/dependency_rewrite b/tools/dependency_rewrite
index f2172cc..d5190dd 100644
--- a/tools/dependency_rewrite
+++ b/tools/dependency_rewrite
@@ -7,6 +7,7 @@
rewrite nodejs.org/(.*) software.frc971.org/Build-Dependencies/nodejs.org/$1
rewrite static.rust-lang.org/(.*) software.frc971.org/Build-Dependencies/static.rust-lang.org/$1
rewrite storage.googleapis.com/(.*) software.frc971.org/Build-Dependencies/storage.googleapis.com/$1
+rewrite files.pythonhosted.org/(.*) software.frc971.org/Build-Dependencies/files.pythonhosted.org/$1
allow golang.org
allow software.frc971.org
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index 710233b..c3e8d86 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -217,7 +217,8 @@
U = sprung_shooter.K * (R - sprung_shooter.X_hat)
U = ClipDeltaU(sprung_shooter, voltage, U)
sprung_shooter.Y = raw_sprung_shooter.Y + 0.01
- sprung_shooter.UpdateObserver(U)
+ sprung_shooter.CorrectObserver(U)
+ sprung_shooter.PredictObserver(U)
voltage += U
raw_sprung_shooter.Update(voltage)
close_loop_x.append(raw_sprung_shooter.X[0, 0] * 10)
@@ -240,7 +241,8 @@
U = shooter.K * (R - shooter.X_hat)
U = ClipDeltaU(shooter, voltage, U)
shooter.Y = raw_shooter.Y + 0.01
- shooter.UpdateObserver(U)
+ shooter.CorrectObserver(U)
+ shooter.PredictObserver(U)
voltage += U
raw_shooter.Update(voltage)
close_loop_x.append(raw_shooter.X[0, 0] * 10)
diff --git a/y2022/control_loops/python/BUILD b/y2022/control_loops/python/BUILD
index d3a5a62..17f7b2a 100644
--- a/y2022/control_loops/python/BUILD
+++ b/y2022/control_loops/python/BUILD
@@ -55,6 +55,8 @@
],
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
+ "//aos/util:py_trapezoid_profile",
+ "//frc971/control_loops/python:angular_system",
"//frc971/control_loops/python:controls",
"@matplotlib_repo//:matplotlib3",
],
@@ -72,6 +74,7 @@
":python_init",
"//external:python-gflags",
"//external:python-glog",
+ "@osqp_amd64//:python_osqp",
],
)
diff --git a/y2022/control_loops/python/catapult.py b/y2022/control_loops/python/catapult.py
index ae6079a..ce7496a 100755
--- a/y2022/control_loops/python/catapult.py
+++ b/y2022/control_loops/python/catapult.py
@@ -3,6 +3,7 @@
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
import numpy
+import osqp
import math
import scipy.optimize
import sys
@@ -39,29 +40,35 @@
M_cup = (1750 * 0.0254 * 0.04 * 2 * math.pi * (ball_diameter / 2.)**2.0)
J_cup = M_cup * lever**2.0 + M_cup * (ball_diameter / 2.)**2.0
-print("J ball", ball_mass * lever * lever)
-print("J bar", J_bar)
-print("bar mass", M_bar)
-print("J cup", J_cup)
-print("cup mass", M_cup)
J = (J_ball + J_bar + J_cup * 1.5)
-print("J", J)
+JEmpty = (J_bar + J_cup * 1.5)
-kCatapult = catapult_lib.CatapultParams(
- name='Finisher',
+kCatapultWithBall = catapult_lib.CatapultParams(
+ name='Catapult',
motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.03),
G=G,
J=J,
- lever=lever,
- q_pos=0.01,
- q_vel=10.0,
- q_voltage=4.0,
- r_pos=0.01,
- controller_poles=[.93],
- dt=0.00505)
+ radius=lever,
+ q_pos=2.8,
+ q_vel=20.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=1.0,
+ kalman_q_voltage=1.5,
+ kalman_r_position=0.05)
-catapult = catapult_lib.Catapult(kCatapult)
+kCatapultEmpty = catapult_lib.CatapultParams(
+ name='Catapult',
+ motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.03),
+ G=G,
+ J=JEmpty,
+ radius=lever,
+ q_pos=2.8,
+ q_vel=20.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=1.0,
+ kalman_q_voltage=1.5,
+ kalman_r_position=0.05)
# Ideas for adjusting the cost function:
#
@@ -76,8 +83,86 @@
# We really want our cost function to be robust so that we can tolerate the
# battery not delivering like we want at the end.
+def quadratic_cost(catapult, X_initial, X_final, horizon):
+ Q_final = numpy.matrix([[10000.0, 0.0], [0.0, 10000.0]])
-def mpc_cost(X_initial, X_final, u_matrix):
+ As = numpy.vstack([catapult.A**(n + 1) for n in range(0, horizon)])
+ Af = catapult.A**horizon
+
+ Bs = numpy.matrix(numpy.zeros((2 * horizon, horizon)))
+ for n in range(0, horizon):
+ for m in range(0, n + 1):
+ Bs[n * 2:(n * 2) + 2, m] = catapult.A**(n - m) * catapult.B
+
+ Bf = Bs[horizon * 2 - 2:, :]
+
+ P_final = 2.0 * Bf.transpose() * Q_final * Bf
+ q_final = (2.0 * (Af * X_initial - X_final).transpose() * Q_final *
+ Bf).transpose()
+
+ constant_final = (Af * X_initial - X_final).transpose() * Q_final * (
+ Af * X_initial - X_final)
+
+ m = numpy.matrix([[catapult.A[1, 1] ** (n + 1)] for n in range(horizon)])
+ M = Bs[1:horizon * 2:2, :]
+
+ W = numpy.matrix(numpy.identity(horizon) - numpy.eye(horizon, horizon, -1)) / catapult.dt
+ w = -numpy.matrix(numpy.eye(horizon, 1, 0)) / catapult.dt
+
+
+ Pi = numpy.diag([
+ (0.01 ** 2.0) + (0.02 * max(0.0, 20 - (horizon - n)) / 20.0) ** 2.0 for n in range(horizon)
+ ])
+
+ P_accel = 2.0 * M.transpose() * W.transpose() * Pi * W * M
+ q_accel = 2.0 * (((W * m + w) * X_initial[1, 0]).transpose() * Pi * W * M).transpose()
+ constant_accel = ((W * m + w) * X_initial[1, 0]).transpose() * Pi * (
+ (W * m + w) * X_initial[1, 0])
+
+ return ((P_accel + P_final), (q_accel + q_final), (constant_accel + constant_final))
+
+
+def new_cost(catapult, X_initial, X_final, u):
+ u_matrix = numpy.matrix(u).transpose()
+ Q_final = numpy.matrix([[10000.0, 0.0], [0.0, 10000.0]])
+
+ As = numpy.vstack([catapult.A**(n + 1) for n in range(0, len(u))])
+ Af = catapult.A**len(u)
+
+ Bs = numpy.matrix(numpy.zeros((2 * len(u), len(u))))
+ for n in range(0, len(u)):
+ for m in range(0, n + 1):
+ Bs[n * 2:(n * 2) + 2, m] = catapult.A**(n - m) * catapult.B
+
+ Bf = Bs[len(u) * 2 - 2:, :]
+
+ P_final = 2.0 * Bf.transpose() * Q_final * Bf
+ q_final = (2.0 * (Af * X_initial - X_final).transpose() * Q_final *
+ Bf).transpose()
+
+ constant_final = (Af * X_initial - X_final).transpose() * Q_final * (
+ Af * X_initial - X_final)
+
+ m = numpy.matrix([[catapult.A[1, 1] ** (n + 1)] for n in range(len(u))])
+ M = Bs[1:len(u) * 2:2, :]
+
+ W = numpy.matrix(numpy.identity(len(u)) - numpy.eye(len(u), len(u), -1)) / catapult.dt
+ w = -numpy.matrix(numpy.eye(len(u), 1, 0)) * X_initial[1, 0] / catapult.dt
+
+ accel = W * (M * u_matrix + m * X_initial[1, 0]) + w
+
+ Pi = numpy.diag([
+ (0.01 ** 2.0) + (0.02 * max(0.0, 20 - (len(u) - n)) / 20.0) ** 2.0 for n in range(len(u))
+ ])
+
+ P_accel = 2.0 * M.transpose() * W.transpose() * Pi * W * M
+ q_accel = 2.0 * ((W * m * X_initial[1, 0] + w).transpose() * Pi * W * M).transpose()
+ constant_accel = (W * m * X_initial[1, 0] +
+ w).transpose() * Pi * (W * m * X_initial[1, 0] + w)
+
+
+def mpc_cost(catapult, X_initial, X_final, u_matrix):
+
X = X_initial.copy()
cost = 0.0
last_u = u_matrix[0]
@@ -118,7 +203,7 @@
return cost
-def SolveCatapult(X_initial, X_final, u):
+def SolveCatapult(catapult, X_initial, X_final, u):
""" Solves for the optimal action given a seed, state, and target """
def vbat_constraint(z, i):
return 12.0 - z[i]
@@ -126,10 +211,31 @@
def forward(z, i):
return z[i]
- def mpc_cost2(u_matrix):
- return mpc_cost(X_initial, X_final, u_matrix)
+ P, q, c = quadratic_cost(catapult, X_initial, X_final, len(u))
- constraints = [{
+
+ def mpc_cost2(u_solver):
+ u_matrix = numpy.matrix(u_solver).transpose()
+ cost = mpc_cost(catapult, X_initial, X_final, u_solver)
+ return cost
+
+
+ def mpc_cost3(u_solver):
+ u_matrix = numpy.matrix(u_solver).transpose()
+ return (0.5 * u_matrix.transpose() * P * u_matrix +
+ q.transpose() * u_matrix + c)[0, 0]
+
+ # If we provide scipy with the analytical jacobian and hessian, it solves
+ # more accurately and a *lot* faster.
+ def jacobian(u):
+ u_matrix = numpy.matrix(u).transpose()
+ return numpy.array(P * u_matrix + q)
+
+ def hessian(u):
+ return numpy.array(P)
+
+ constraints = []
+ constraints += [{
'type': 'ineq',
'fun': vbat_constraint,
'args': (i, )
@@ -141,9 +247,12 @@
'args': (i, )
} for i in numpy.arange(len(u))]
- result = scipy.optimize.minimize(mpc_cost2,
+ result = scipy.optimize.minimize(mpc_cost3,
u,
+ jac=jacobian,
+ hess=hessian,
method='SLSQP',
+ tol=1e-12,
constraints=constraints)
print(result)
@@ -151,7 +260,7 @@
def CatapultProblem():
- c = catapult_lib.Catapult(kCatapult)
+ c = catapult_lib.Catapult(kCatapultWithBall)
kHorizon = 40
@@ -171,7 +280,8 @@
# Iteratively solve our MPC and simulate it.
u_samples = []
for i in range(kHorizon):
- u_horizon = SolveCatapult(X, X_final, u)
+ u_horizon = SolveCatapult(c, X, X_final, u)
+
u_samples.append(u_horizon[0])
v_prior = X[1, 0]
X = c.A * X + c.B * numpy.matrix([[u_horizon[0]]])
@@ -198,33 +308,85 @@
def main(argv):
- # Do all our math with a lower voltage so we have headroom.
- U = numpy.matrix([[9.0]])
- print(
- "For G:", G, " max speed ",
- catapult_lib.MaxSpeed(params=kCatapult,
- U=U,
- final_position=math.pi / 2.0))
-
- CatapultProblem()
-
if FLAGS.plot:
- catapult_lib.PlotShot(kCatapult, U, final_position=math.pi / 4.0)
+ # Do all our math with a lower voltage so we have headroom.
+ U = numpy.matrix([[9.0]])
+
+ prob = osqp.OSQP()
+
+ kHorizon = 40
+ catapult = catapult_lib.Catapult(kCatapultWithBall)
+ X_initial = numpy.matrix([[0.0], [0.0]])
+ X_final = numpy.matrix([[2.0], [25.0]])
+ P, q, c = quadratic_cost(catapult, X_initial, X_final, kHorizon)
+ A = numpy.identity(kHorizon)
+ l = numpy.zeros((kHorizon, 1))
+ u = numpy.ones((kHorizon, 1)) * 12.0
+
+ prob.setup(scipy.sparse.csr_matrix(P),
+ q,
+ scipy.sparse.csr_matrix(A),
+ l,
+ u,
+ warm_start=True)
+
+ result = prob.solve()
+ # Check solver status
+ if result.info.status != 'solved':
+ raise ValueError('OSQP did not solve the problem!')
+
+ # Apply first control input to the plant
+ print(result.x)
+
+ glog.debug("J ball", ball_mass * lever * lever)
+ glog.debug("J bar", J_bar)
+ glog.debug("bar mass", M_bar)
+ glog.debug("J cup", J_cup)
+ glog.debug("cup mass", M_cup)
+ glog.debug("J", J)
+ glog.debug("J Empty", JEmpty)
+
+ glog.debug(
+ "For G:", G, " max speed ",
+ catapult_lib.MaxSpeed(params=kCatapultWithBall,
+ U=U,
+ final_position=math.pi / 2.0))
+
+ CatapultProblem()
+
+ catapult_lib.PlotStep(params=kCatapultWithBall,
+ R=numpy.matrix([[1.0], [0.0]]))
+ catapult_lib.PlotKick(params=kCatapultWithBall,
+ R=numpy.matrix([[1.0], [0.0]]))
+ return 0
+
+ catapult_lib.PlotShot(kCatapultWithBall,
+ U,
+ final_position=math.pi / 4.0)
gs = []
speed = []
for i in numpy.linspace(0.01, 0.15, 150):
- kCatapult.G = i
- gs.append(kCatapult.G)
+ kCatapultWithBall.G = i
+ gs.append(kCatapultWithBall.G)
speed.append(
- catapult_lib.MaxSpeed(params=kCatapult,
+ catapult_lib.MaxSpeed(params=kCatapultWithBall,
U=U,
final_position=math.pi / 2.0))
pylab.plot(gs, speed, label="max_speed")
pylab.show()
- return 0
+
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the catapult and integral catapult.'
+ )
+ else:
+ namespaces = ['y2022', 'control_loops', 'superstructure', 'catapult']
+ catapult_lib.WriteCatapult([kCatapultWithBall, kCatapultEmpty], argv[1:3], argv[3:5], namespaces)
+ return 0
if __name__ == '__main__':
argv = FLAGS(sys.argv)
+ glog.init()
sys.exit(main(argv))
diff --git a/y2022/control_loops/python/catapult_lib.py b/y2022/control_loops/python/catapult_lib.py
index 256793a..d6040d1 100644
--- a/y2022/control_loops/python/catapult_lib.py
+++ b/y2022/control_loops/python/catapult_lib.py
@@ -1,180 +1,33 @@
+from frc971.control_loops.python import angular_system
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
+from aos.util.trapezoid_profile import TrapezoidProfile
import numpy
from matplotlib import pylab
import gflags
import glog
-
-class CatapultParams(object):
- def __init__(self,
- name,
- motor,
- G,
- J,
- lever,
- q_pos,
- q_vel,
- q_voltage,
- r_pos,
- controller_poles,
- dt=0.00505):
- self.name = name
- self.motor = motor
- self.G = G
- self.J = J
- self.lever = lever
- self.q_pos = q_pos
- self.q_vel = q_vel
- self.q_voltage = q_voltage
- self.r_pos = r_pos
- self.dt = dt
- self.controller_poles = controller_poles
+CatapultParams = angular_system.AngularSystemParams
-class VelocityCatapult(control_loop.HybridControlLoop):
+# TODO(austin): This is mostly the same as angular_system. Can we either wrap an angular_system or assign it?
+class Catapult(angular_system.AngularSystem):
def __init__(self, params, name="Catapult"):
- super(VelocityCatapult, self).__init__(name=name)
- self.params = params
- # Set Motor
- self.motor = self.params.motor
- # Gear ratio
- self.G = self.params.G
- # Moment of inertia of the catapult wheel in kg m^2
- self.J = self.params.J + self.motor.motor_inertia / (self.G**2.0)
- # Control loop time step
- self.dt = self.params.dt
-
- # State feedback matrices
- # [angular velocity]
- self.A_continuous = numpy.matrix([[
- -self.motor.Kt / self.motor.Kv /
- (self.J * self.G * self.G * self.motor.resistance)
- ]])
- self.B_continuous = numpy.matrix(
- [[self.motor.Kt / (self.J * self.G * self.motor.resistance)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
-
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
-
- self.PlaceControllerPoles(self.params.controller_poles)
-
- # Generated controller not used.
- self.PlaceObserverPoles([0.3])
-
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
-
- qff_vel = 8.0
- self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])
-
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
-
- glog.debug('K: %s', str(self.K))
- glog.debug('Poles: %s',
- str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
-
-
-class Catapult(VelocityCatapult):
- def __init__(self, params, name="Catapult"):
- super(Catapult, self).__init__(params, name=name)
-
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
-
- self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
- self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
- self.A_continuous[0, 1] = 1
-
- self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
- self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
-
- # State feedback matrices
- # [position, angular velocity]
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
-
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
-
- rpl = 0.45
- ipl = 0.07
- self.PlaceObserverPoles([rpl + 1j * ipl, rpl - 1j * ipl])
-
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 2)))
- self.K[0, 1:2] = self.K_unaugmented
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 2)))
- self.Kff[0, 1:2] = self.Kff_unaugmented
+ super(Catapult, self).__init__(params, name)
+ # Signal that we have a single cycle output delay to compensate for in
+ # our observer.
+ self.delayed_u = True
self.InitializeState()
-class IntegralCatapult(Catapult):
+class IntegralCatapult(angular_system.IntegralAngularSystem):
def __init__(self, params, name="IntegralCatapult"):
super(IntegralCatapult, self).__init__(params, name=name)
-
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
-
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
-
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
-
- # states
- # [position, velocity, voltage_error]
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
-
- glog.debug('A_continuous %s' % str(self.A_continuous))
- glog.debug('B_continuous %s' % str(self.B_continuous))
- glog.debug('C %s' % str(self.C))
-
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
-
- glog.debug('A %s' % str(self.A))
- glog.debug('B %s' % str(self.B))
-
- q_pos = self.params.q_pos
- q_vel = self.params.q_vel
- q_voltage = self.params.q_voltage
- self.Q_continuous = numpy.matrix([[(q_pos**2.0), 0.0,
- 0.0], [0.0, (q_vel**2.0), 0.0],
- [0.0, 0.0, (q_voltage**2.0)]])
-
- r_pos = self.params.r_pos
- self.R_continuous = numpy.matrix([[(r_pos**2.0)]])
-
- _, _, self.Q, self.R = controls.kalmd(
- A_continuous=self.A_continuous,
- B_continuous=self.B_continuous,
- Q_continuous=self.Q_continuous,
- R_continuous=self.R_continuous,
- dt=self.dt)
-
- glog.debug('Q_discrete %s' % (str(self.Q)))
- glog.debug('R_discrete %s' % (str(self.R)))
-
- self.KalmanGain, self.P_steady_state = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
-
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 3)))
- self.Kff[0, 0:2] = self.Kff_unaugmented
+ # Signal that we have a single cycle output delay to compensate for in
+ # our observer.
+ self.delayed_u = True
self.InitializeState()
@@ -201,7 +54,7 @@
while True:
X_hat = catapult.X
if catapult.X[0, 0] > final_position:
- return catapult.X[1, 0] * params.lever
+ return catapult.X[1, 0] * params.radius
if observer_catapult is not None:
X_hat = observer_catapult.X_hat
@@ -210,13 +63,13 @@
if observer_catapult is not None:
observer_catapult.Y = catapult.Y
- observer_catapult.CorrectHybridObserver(U)
+ observer_catapult.CorrectObserver(U)
applied_U = U.copy()
catapult.Update(applied_U)
if observer_catapult is not None:
- observer_catapult.PredictHybridObserver(U, catapult.dt)
+ observer_catapult.PredictObserver(U)
def PlotShot(params, U, final_position):
@@ -262,7 +115,7 @@
X_hat = observer_catapult.X_hat
x_hat.append(observer_catapult.X_hat[0, 0])
w_hat.append(observer_catapult.X_hat[1, 0])
- v_hat.append(observer_catapult.X_hat[1, 0] * params.lever)
+ v_hat.append(observer_catapult.X_hat[1, 0] * params.radius)
U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
x.append(catapult.X[0, 0])
@@ -277,13 +130,13 @@
if observer_catapult is not None:
observer_catapult.Y = catapult.Y
- observer_catapult.CorrectHybridObserver(U)
+ observer_catapult.CorrectObserver(U)
offset.append(observer_catapult.X_hat[2, 0])
catapult.Update(U)
if observer_catapult is not None:
- observer_catapult.PredictHybridObserver(U, catapult.dt)
+ observer_catapult.PredictObserver(U)
t.append(initial_t + i * catapult.dt)
u.append(U[0, 0])
@@ -306,3 +159,133 @@
pylab.legend()
pylab.show()
+
+
+RunTest = angular_system.RunTest
+
+
+def PlotStep(params, R, plant_params=None):
+ """Plots a step move to the goal.
+
+ Args:
+ params: CatapultParams for the controller and observer
+ plant_params: CatapultParams for the plant. Defaults to params if
+ plant_params is None.
+ R: numpy.matrix(2, 1), the goal"""
+ plant = Catapult(plant_params or params, params.name)
+ controller = IntegralCatapult(params, params.name)
+ observer = IntegralCatapult(params, params.name)
+
+ # Test moving the system.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ augmented_R = numpy.matrix(numpy.zeros((3, 1)))
+ augmented_R[0:2, :] = R
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=False,
+ kick_time=1.0,
+ kick_magnitude=0.0)
+
+
+def PlotKick(params, R, plant_params=None):
+ """Plots a step motion with a kick at 1.0 seconds.
+
+ Args:
+ params: CatapultParams for the controller and observer
+ plant_params: CatapultParams for the plant. Defaults to params if
+ plant_params is None.
+ R: numpy.matrix(2, 1), the goal"""
+ plant = Catapult(plant_params or params, params.name)
+ controller = IntegralCatapult(params, params.name)
+ observer = IntegralCatapult(params, params.name)
+
+ # Test moving the system.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ augmented_R = numpy.matrix(numpy.zeros((3, 1)))
+ augmented_R[0:2, :] = R
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=False,
+ kick_time=1.0,
+ kick_magnitude=2.0)
+
+
+def PlotMotion(params,
+ R,
+ max_velocity=10.0,
+ max_acceleration=70.0,
+ plant_params=None):
+ """Plots a trapezoidal motion.
+
+ Args:
+ params: CatapultParams for the controller and observer
+ plant_params: CatapultParams for the plant. Defaults to params if
+ plant_params is None.
+ R: numpy.matrix(2, 1), the goal,
+ max_velocity: float, The max velocity of the profile.
+ max_acceleration: float, The max acceleration of the profile.
+ """
+ plant = Catapult(plant_params or params, params.name)
+ controller = IntegralCatapult(params, params.name)
+ observer = IntegralCatapult(params, params.name)
+
+ # Test moving the system.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ augmented_R = numpy.matrix(numpy.zeros((3, 1)))
+ augmented_R[0:2, :] = R
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=True,
+ max_velocity=max_velocity,
+ max_acceleration=max_acceleration)
+
+
+def WriteCatapult(params, plant_files, controller_files, year_namespaces):
+ """Writes out the constants for a catapult to a file.
+
+ Args:
+ params: list of CatapultParams or CatapultParams, the
+ parameters defining the system.
+ plant_files: list of strings, the cc and h files for the plant.
+ controller_files: list of strings, the cc and h files for the integral
+ controller.
+ year_namespaces: list of strings, the namespace list to use.
+ """
+ # Write the generated constants out to a file.
+ catapults = []
+ integral_catapults = []
+
+ if type(params) is list:
+ name = params[0].name
+ for index, param in enumerate(params):
+ catapults.append(Catapult(param, param.name + str(index)))
+ integral_catapults.append(
+ IntegralCatapult(param, 'Integral' + param.name + str(index)))
+ else:
+ name = params.name
+ catapults.append(Catapult(params, params.name))
+ integral_catapults.append(
+ IntegralCatapult(params, 'Integral' + params.name))
+
+ loop_writer = control_loop.ControlLoopWriter(name,
+ catapults,
+ namespaces=year_namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', catapults[0].G))
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f',
+ catapults[0].motor.free_speed))
+ loop_writer.Write(plant_files[0], plant_files[1])
+
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'Integral' + name, integral_catapults, namespaces=year_namespaces)
+ integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/y2022/control_loops/superstructure/catapult/BUILD b/y2022/control_loops/superstructure/catapult/BUILD
new file mode 100644
index 0000000..289dfde
--- /dev/null
+++ b/y2022/control_loops/superstructure/catapult/BUILD
@@ -0,0 +1,68 @@
+genrule(
+ name = "genrule_catapult",
+ outs = [
+ "catapult_plant.h",
+ "catapult_plant.cc",
+ "integral_catapult_plant.h",
+ "integral_catapult_plant.cc",
+ ],
+ cmd = "$(location //y2022/control_loops/python:catapult) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2022/control_loops/python:catapult",
+ ],
+)
+
+cc_library(
+ name = "catapult_plants",
+ srcs = [
+ "catapult_plant.cc",
+ "integral_catapult_plant.cc",
+ ],
+ hdrs = [
+ "catapult_plant.h",
+ "integral_catapult_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
+
+cc_library(
+ name = "catapult",
+ srcs = [
+ "catapult.cc",
+ ],
+ hdrs = [
+ "catapult.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":catapult_plants",
+ "//aos:realtime",
+ "//third_party/osqp-cpp",
+ ],
+)
+
+cc_test(
+ name = "catapult_test",
+ srcs = [
+ "catapult_test.cc",
+ ],
+ deps = [
+ ":catapult",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_binary(
+ name = "catapult_main",
+ srcs = [
+ "catapult_main.cc",
+ ],
+ deps = [
+ ":catapult",
+ "//aos:init",
+ ],
+)
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
new file mode 100644
index 0000000..243cb2d
--- /dev/null
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -0,0 +1,260 @@
+#include "y2022/control_loops/superstructure/catapult/catapult.h"
+
+#include "Eigen/Dense"
+#include "Eigen/Sparse"
+#include "aos/realtime.h"
+#include "aos/time/time.h"
+#include "glog/logging.h"
+#include "osqp++.h"
+#include "osqp.h"
+#include "y2022/control_loops/superstructure/catapult/catapult_plant.h"
+
+namespace y2022 {
+namespace control_loops {
+namespace superstructure {
+namespace catapult {
+namespace chrono = std::chrono;
+
+namespace {
+osqp::OsqpInstance MakeInstance(
+ size_t horizon, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P) {
+ osqp::OsqpInstance instance;
+ instance.objective_matrix = P.cast<c_float>().sparseView();
+
+ instance.constraint_matrix =
+ Eigen::SparseMatrix<c_float, Eigen::ColMajor, osqp::c_int>(horizon,
+ horizon);
+ instance.constraint_matrix.setIdentity();
+
+ instance.lower_bounds =
+ Eigen::Matrix<c_float, Eigen::Dynamic, 1>::Zero(horizon, 1);
+ instance.upper_bounds =
+ Eigen::Matrix<c_float, Eigen::Dynamic, 1>::Ones(horizon, 1) * 12.0;
+ return instance;
+}
+} // namespace
+
+MPCProblem::MPCProblem(size_t horizon,
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P,
+ Eigen::Matrix<double, Eigen::Dynamic, 1> accel_q,
+ Eigen::Matrix<double, 2, 2> Af,
+ Eigen::Matrix<double, Eigen::Dynamic, 2> final_q)
+ : horizon_(horizon),
+ accel_q_(std::move(accel_q.cast<c_float>())),
+ Af_(std::move(Af.cast<c_float>())),
+ final_q_(std::move(final_q.cast<c_float>())),
+ instance_(MakeInstance(horizon, std::move(P))) {
+ instance_.objective_vector =
+ Eigen::Matrix<c_float, Eigen::Dynamic, 1>(horizon, 1);
+ settings_.max_iter = 25;
+ settings_.check_termination = 25;
+ settings_.warm_start = 0;
+ auto status = solver_.Init(instance_, settings_);
+ CHECK(status.ok()) << status;
+}
+
+void MPCProblem::SetState(Eigen::Matrix<c_float, 2, 1> X_initial,
+ Eigen::Matrix<c_float, 2, 1> X_final) {
+ X_initial_ = X_initial;
+ X_final_ = X_final;
+ instance_.objective_vector =
+ X_initial(1, 0) * accel_q_ + final_q_ * (Af_ * X_initial - X_final);
+
+ auto status = solver_.SetObjectiveVector(instance_.objective_vector);
+ CHECK(status.ok()) << status;
+}
+
+bool MPCProblem::Solve() {
+ const aos::monotonic_clock::time_point start_time =
+ aos::monotonic_clock::now();
+ osqp::OsqpExitCode exit_code = solver_.Solve();
+ const aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();
+ VLOG(1) << "OSQP solved in "
+ << std::chrono::duration<double>(end_time - start_time).count();
+ solve_time_ = std::chrono::duration<double>(end_time - start_time).count();
+ // TODO(austin): Dump the exit codes out as an enum for logging.
+ //
+ // TODO(austin): The dual problem doesn't appear to be converging on all
+ // problems. Are we phrasing something wrong?
+
+ // TODO(austin): Set a time limit so we can't run forever, and signal back
+ // when we hit our limit.
+ return exit_code == osqp::OsqpExitCode::kOptimal;
+}
+
+void MPCProblem::WarmStart(const MPCProblem &p) {
+ CHECK_GE(p.horizon(), horizon())
+ << ": Can only copy a bigger problem's solution into a smaller problem.";
+ auto status = solver_.SetPrimalWarmStart(p.solver_.primal_solution().block(
+ p.horizon() - horizon(), 0, horizon(), 1));
+ CHECK(status.ok()) << status;
+ status = solver_.SetDualWarmStart(p.solver_.dual_solution().block(
+ p.horizon() - horizon(), 0, horizon(), 1));
+ CHECK(status.ok()) << status;
+}
+
+CatapultProblemGenerator::CatapultProblemGenerator(size_t horizon)
+ : plant_(MakeCatapultPlant()),
+ horizon_(horizon),
+ Q_final_(
+ (Eigen::DiagonalMatrix<double, 2>().diagonal() << 10000.0, 10000.0)
+ .finished()),
+ As_(MakeAs()),
+ Bs_(MakeBs()),
+ m_(Makem()),
+ M_(MakeM()),
+ W_(MakeW()),
+ w_(Makew()),
+ Pi_(MakePi()),
+ WM_(W_ * M_),
+ Wmpw_(W_ * m_ + w_) {}
+
+std::unique_ptr<MPCProblem> CatapultProblemGenerator::MakeProblem(
+ size_t horizon) {
+ return std::make_unique<MPCProblem>(
+ horizon, P(horizon), accel_q(horizon), Af(horizon),
+ (2.0 * Q_final_ * Bf(horizon)).transpose());
+}
+
+const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
+CatapultProblemGenerator::P(size_t horizon) {
+ CHECK_GT(horizon, 0u);
+ CHECK_LE(horizon, horizon_);
+ return 2.0 * (WM_.block(0, 0, horizon, horizon).transpose() * Pi(horizon) *
+ WM_.block(0, 0, horizon, horizon) +
+ Bf(horizon).transpose() * Q_final_ * Bf(horizon));
+}
+
+const Eigen::Matrix<double, Eigen::Dynamic, 1> CatapultProblemGenerator::q(
+ size_t horizon, Eigen::Matrix<c_float, 2, 1> X_initial,
+ Eigen::Matrix<c_float, 2, 1> X_final) {
+ CHECK_GT(horizon, 0u);
+ CHECK_LE(horizon, horizon_);
+ return 2.0 * X_initial(1, 0) * accel_q(horizon) +
+ 2.0 * ((Af(horizon) * X_initial - X_final).transpose() * Q_final_ *
+ Bf(horizon))
+ .transpose();
+}
+
+const Eigen::Matrix<double, Eigen::Dynamic, 1>
+CatapultProblemGenerator::accel_q(size_t horizon) {
+ return 2.0 * ((Wmpw_.block(0, 0, horizon, 1)).transpose() * Pi(horizon) *
+ WM_.block(0, 0, horizon, horizon))
+ .transpose();
+}
+
+const Eigen::Matrix<double, 2, 2> CatapultProblemGenerator::Af(size_t horizon) {
+ CHECK_GT(horizon, 0u);
+ CHECK_LE(horizon, horizon_);
+ return As_.block<2, 2>(2 * (horizon - 1), 0);
+}
+
+const Eigen::Matrix<double, 2, Eigen::Dynamic> CatapultProblemGenerator::Bf(
+ size_t horizon) {
+ CHECK_GT(horizon, 0u);
+ CHECK_LE(horizon, horizon_);
+ return Bs_.block(2 * (horizon - 1), 0, 2, horizon);
+}
+
+const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
+CatapultProblemGenerator::Pi(size_t horizon) {
+ CHECK_GT(horizon, 0u);
+ CHECK_LE(horizon, horizon_);
+ return Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>(Pi_).block(
+ horizon_ - horizon, horizon_ - horizon, horizon, horizon);
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, 2> CatapultProblemGenerator::MakeAs() {
+ Eigen::Matrix<double, Eigen::Dynamic, 2> As =
+ Eigen::Matrix<double, Eigen::Dynamic, 2>::Zero(horizon_ * 2, 2);
+ for (size_t i = 0; i < horizon_; ++i) {
+ if (i == 0) {
+ As.block<2, 2>(0, 0) = plant_.A();
+ } else {
+ As.block<2, 2>(i * 2, 0) = plant_.A() * As.block<2, 2>((i - 1) * 2, 0);
+ }
+ }
+ return As;
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
+CatapultProblemGenerator::MakeBs() {
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Bs =
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(horizon_ * 2,
+ horizon_);
+ for (size_t i = 0; i < horizon_; ++i) {
+ for (size_t j = 0; j < i + 1; ++j) {
+ if (i == j) {
+ Bs.block<2, 1>(i * 2, j) = plant_.B();
+ } else {
+ Bs.block<2, 1>(i * 2, j) =
+ As_.block<2, 2>((i - j - 1) * 2, 0) * plant_.B();
+ }
+ }
+ }
+ return Bs;
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, 1> CatapultProblemGenerator::Makem() {
+ Eigen::Matrix<double, Eigen::Dynamic, 1> m =
+ Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(horizon_, 1);
+ for (size_t i = 0; i < horizon_; ++i) {
+ m(i, 0) = As_(1 + 2 * i, 1);
+ }
+ return m;
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
+CatapultProblemGenerator::MakeM() {
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> M =
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(horizon_,
+ horizon_);
+ for (size_t i = 0; i < horizon_; ++i) {
+ for (size_t j = 0; j < horizon_; ++j) {
+ M(i, j) = Bs_(2 * i + 1, j);
+ }
+ }
+ return M;
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
+CatapultProblemGenerator::MakeW() {
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> W =
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Identity(horizon_,
+ horizon_);
+ for (size_t i = 0; i < horizon_ - 1; ++i) {
+ W(i + 1, i) = -1.0;
+ }
+ W /= std::chrono::duration<double>(plant_.dt()).count();
+ return W;
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, 1> CatapultProblemGenerator::Makew() {
+ Eigen::Matrix<double, Eigen::Dynamic, 1> w =
+ Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(horizon_, 1);
+ w(0, 0) = -1.0 / std::chrono::duration<double>(plant_.dt()).count();
+ return w;
+}
+
+Eigen::DiagonalMatrix<double, Eigen::Dynamic>
+CatapultProblemGenerator::MakePi() {
+ Eigen::DiagonalMatrix<double, Eigen::Dynamic> Pi(horizon_);
+ for (size_t i = 0; i < horizon_; ++i) {
+ Pi.diagonal()(i) =
+ std::pow(0.01, 2.0) +
+ std::pow(0.02 * std::max(0.0, (20 - ((int)horizon_ - (int)i)) / 20.),
+ 2.0);
+ }
+ return Pi;
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
+CatapultProblemGenerator::MakeP() {
+ return 2.0 * (M_.transpose() * W_.transpose() * Pi_ * W_ * M_ +
+ Bf(horizon_).transpose() * Q_final_ * Bf(horizon_));
+}
+
+} // namespace catapult
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2022
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
new file mode 100644
index 0000000..85e9242
--- /dev/null
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -0,0 +1,132 @@
+#ifndef Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_CATAPULT_CATAPULT_H_
+#define Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_CATAPULT_CATAPULT_H_
+
+#include "Eigen/Dense"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "glog/logging.h"
+#include "osqp++.h"
+
+namespace y2022 {
+namespace control_loops {
+namespace superstructure {
+namespace catapult {
+
+// MPC problem for a specified horizon. This contains all the state for the
+// solver, setters to modify the current and target state, and a way to fetch
+// the solution.
+class MPCProblem {
+ public:
+ MPCProblem(size_t horizon,
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P,
+ Eigen::Matrix<double, Eigen::Dynamic, 1> accel_q,
+ Eigen::Matrix<double, 2, 2> Af,
+ Eigen::Matrix<double, Eigen::Dynamic, 2> final_q);
+
+ MPCProblem(MPCProblem const &) = delete;
+ void operator=(MPCProblem const &x) = delete;
+
+ // Sets the current and final state. This keeps the problem in tact and
+ // doesn't recreate it, so it will be fast.
+ void SetState(Eigen::Matrix<double, 2, 1> X_initial,
+ Eigen::Matrix<double, 2, 1> X_final);
+
+ // Solves our problem.
+ bool Solve();
+
+ double solve_time() const { return solve_time_; }
+
+ // Returns the solution that the solver found when Solve was last called.
+ double U(size_t i) const { return solver_.primal_solution()(i); }
+
+ // Returns the number of U's to be solved.
+ size_t horizon() const { return horizon_; }
+
+ // Warm starts the optimizer with the provided solution to make it solve
+ // faster.
+ void WarmStart(const MPCProblem &p);
+
+ private:
+ // The number of u's to solve for.
+ const size_t horizon_;
+
+ // The problem statement variables needed by SetState to update q.
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> accel_q_;
+ const Eigen::Matrix<double, 2, 2> Af_;
+ const Eigen::Matrix<double, Eigen::Dynamic, 2> final_q_;
+
+ Eigen::Matrix<double, 2, 1> X_initial_;
+ Eigen::Matrix<double, 2, 1> X_final_;
+
+ // Solver state.
+ osqp::OsqpInstance instance_;
+ osqp::OsqpSolver solver_;
+ osqp::OsqpSettings settings_;
+
+ double solve_time_ = 0;
+};
+
+// Decently efficient problem generator for multiple horizons given a max
+// horizon to solve for.
+//
+// The math is documented in mpc.tex
+class CatapultProblemGenerator {
+ public:
+ // Builds a problem generator for the specified max horizon and caches a lot
+ // of the state.
+ CatapultProblemGenerator(size_t horizon);
+
+ // Returns the maximum horizon.
+ size_t horizon() const { return horizon_; }
+
+ // Makes a problem for the specificed horizon.
+ std::unique_ptr<MPCProblem> MakeProblem(size_t horizon);
+
+ // Returns the P and Q matrices for the problem statement.
+ // cost = 0.5 X.T P X + q.T X
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P(size_t horizon);
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> q(
+ size_t horizon, Eigen::Matrix<double, 2, 1> X_initial,
+ Eigen::Matrix<double, 2, 1> X_final);
+
+ private:
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> accel_q(size_t horizon);
+
+ const Eigen::Matrix<double, 2, 2> Af(size_t horizon);
+ const Eigen::Matrix<double, 2, Eigen::Dynamic> Bf(size_t horizon);
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Pi(
+ size_t horizon);
+
+ // These functions are used in the constructor to build up the matrices below.
+ Eigen::Matrix<double, Eigen::Dynamic, 2> MakeAs();
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MakeBs();
+ Eigen::Matrix<double, Eigen::Dynamic, 1> Makem();
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MakeM();
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MakeW();
+ Eigen::Matrix<double, Eigen::Dynamic, 1> Makew();
+ Eigen::DiagonalMatrix<double, Eigen::Dynamic> MakePi();
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MakeP();
+
+ const StateFeedbackPlant<2, 1, 1> plant_;
+ const size_t horizon_;
+
+ const Eigen::DiagonalMatrix<double, 2> Q_final_;
+
+ const Eigen::Matrix<double, Eigen::Dynamic, 2> As_;
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Bs_;
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> m_;
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> M_;
+
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> W_;
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> w_;
+ const Eigen::DiagonalMatrix<double, Eigen::Dynamic> Pi_;
+
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> WM_;
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Wmpw_;
+};
+
+} // namespace catapult
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2022
+
+#endif // Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_CATAPULT_CATAPULT_H_
diff --git a/y2022/control_loops/superstructure/catapult/catapult_main.cc b/y2022/control_loops/superstructure/catapult/catapult_main.cc
new file mode 100644
index 0000000..53d357a
--- /dev/null
+++ b/y2022/control_loops/superstructure/catapult/catapult_main.cc
@@ -0,0 +1,126 @@
+#include "aos/init.h"
+#include "aos/realtime.h"
+#include "aos/time/time.h"
+#include "y2022/control_loops/superstructure/catapult/catapult.h"
+#include "y2022/control_loops/superstructure/catapult/catapult_plant.h"
+
+namespace y2022 {
+namespace control_loops {
+namespace superstructure {
+namespace catapult {
+namespace chrono = std::chrono;
+
+void OSQPSolve() {
+ Eigen::Matrix<double, 2, 1> X_initial(0.0, 0.0);
+ Eigen::Matrix<double, 2, 1> X_final(2.0, 25.0);
+
+ LOG(INFO) << "Starting a dynamic OSQP solve";
+ CatapultProblemGenerator g(35);
+ const StateFeedbackPlant<2, 1, 1> plant = MakeCatapultPlant();
+
+ constexpr int kHorizon = 35;
+
+ // TODO(austin): This is a good unit test! Make sure computing the problem
+ // different ways comes out the same.
+ {
+ CatapultProblemGenerator g2(10);
+ constexpr int kTestHorizon = 10;
+ CHECK(g2.P(kTestHorizon) == g.P(kTestHorizon))
+ << g2.P(kTestHorizon) - g.P(kTestHorizon);
+ CHECK(g2.q(kTestHorizon, X_initial, X_final) ==
+ g.q(kTestHorizon, X_initial, X_final))
+ << g2.q(kTestHorizon, X_initial, X_final) -
+ g.q(kTestHorizon, X_initial, X_final);
+ }
+
+ osqp::OsqpInstance instance;
+ instance.objective_matrix = g.P(kHorizon).sparseView();
+
+ instance.objective_vector = g.q(kHorizon, X_initial, X_final);
+
+ instance.constraint_matrix =
+ Eigen::SparseMatrix<double, Eigen::ColMajor, osqp::c_int>(kHorizon,
+ kHorizon);
+ instance.constraint_matrix.setIdentity();
+
+ instance.lower_bounds =
+ Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(kHorizon, 1);
+ instance.upper_bounds =
+ Eigen::Matrix<double, Eigen::Dynamic, 1>::Ones(kHorizon, 1) * 12.0;
+
+ osqp::OsqpSolver solver;
+ osqp::OsqpSettings settings;
+ // Edit settings if appropriate.
+ auto status = solver.Init(instance, settings);
+ CHECK(status.ok()) << status;
+
+ aos::LockAllMemory();
+ aos::ExpandStackSize();
+ aos::SetCurrentThreadRealtimePriority(60);
+
+ for (int i = 0; i < 10; ++i) {
+ const aos::monotonic_clock::time_point start_time =
+ aos::monotonic_clock::now();
+ osqp::OsqpExitCode exit_code = solver.Solve();
+ const aos::monotonic_clock::time_point end_time =
+ aos::monotonic_clock::now();
+ LOG(INFO) << "OSQP solved in "
+ << chrono::duration<double>(end_time - start_time).count();
+ CHECK(exit_code == osqp::OsqpExitCode::kOptimal);
+ }
+
+ double optimal_objective = solver.objective_value();
+ Eigen::Matrix<double, Eigen::Dynamic, 1> optimal_solution =
+ solver.primal_solution();
+
+ LOG(INFO) << "Cost: " << optimal_objective;
+ LOG(INFO) << "U: " << optimal_solution;
+
+ std::vector<std::unique_ptr<MPCProblem>> problems;
+ for (size_t i = g.horizon(); i > 0; --i) {
+ LOG(INFO) << "Made problem " << i;
+ problems.emplace_back(g.MakeProblem(i));
+ }
+
+ std::unique_ptr<MPCProblem> p = g.MakeProblem(kHorizon);
+
+ p->SetState(X_initial, X_final);
+ p->Solve();
+ p->Solve();
+ p->Solve();
+ p->Solve();
+
+ Eigen::Vector2d X = X_initial;
+ for (size_t i = 0; i < g.horizon(); ++i) {
+ problems[i]->SetState(X, X_final);
+ if (i != 0) {
+ problems[i]->WarmStart(*problems[i - 1]);
+ }
+
+ problems[i]->Solve();
+ X = plant.A() * X + plant.B() * problems[i]->U(0);
+
+ LOG(INFO) << "Dynamic u(" << i << "): " << problems[i]->U(0) << " -> "
+ << X.transpose();
+ }
+
+ aos::UnsetCurrentThreadRealtimePriority();
+}
+
+int Main(int /*argc*/, char ** /*argv*/) {
+ OSQPSolve();
+
+ gflags::ShutDownCommandLineFlags();
+ return 0;
+}
+
+} // namespace catapult
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2022
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ return y2022::control_loops::superstructure::catapult::Main(argc, argv);
+}
diff --git a/y2022/control_loops/superstructure/catapult/catapult_test.cc b/y2022/control_loops/superstructure/catapult/catapult_test.cc
new file mode 100644
index 0000000..b37c3e0
--- /dev/null
+++ b/y2022/control_loops/superstructure/catapult/catapult_test.cc
@@ -0,0 +1,62 @@
+#include "y2022/control_loops/superstructure/catapult/catapult.h"
+
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+#include "y2022/control_loops/superstructure/catapult/catapult_plant.h"
+
+namespace y2022 {
+namespace control_loops {
+namespace superstructure {
+namespace catapult {
+namespace testing {
+
+// Tests that computing P and q with 2 different horizons comes out the same.
+TEST(MPCTest, HorizonTest) {
+ Eigen::Matrix<double, 2, 1> X_initial(0.0, 0.0);
+ Eigen::Matrix<double, 2, 1> X_final(2.0, 25.0);
+
+ CatapultProblemGenerator g(35);
+
+ CatapultProblemGenerator g2(10);
+ constexpr int kTestHorizon = 10;
+ EXPECT_TRUE(g2.P(kTestHorizon) == g.P(kTestHorizon))
+ << g2.P(kTestHorizon) - g.P(kTestHorizon);
+ EXPECT_TRUE(g2.q(kTestHorizon, X_initial, X_final) ==
+ g.q(kTestHorizon, X_initial, X_final))
+ << g2.q(kTestHorizon, X_initial, X_final) -
+ g.q(kTestHorizon, X_initial, X_final);
+}
+
+// Tests that we can get to the destination.
+TEST(MPCTest, NearGoal) {
+ Eigen::Matrix<double, 2, 1> X_initial(0.0, 0.0);
+ Eigen::Matrix<double, 2, 1> X_final(2.0, 25.0);
+
+ CatapultProblemGenerator g(35);
+ const StateFeedbackPlant<2, 1, 1> plant = MakeCatapultPlant();
+
+ std::vector<std::unique_ptr<MPCProblem>> problems;
+ for (size_t i = g.horizon(); i > 0; --i) {
+ problems.emplace_back(g.MakeProblem(i));
+ }
+
+ Eigen::Vector2d X = X_initial;
+ for (size_t i = 0; i < g.horizon(); ++i) {
+ problems[i]->SetState(X, X_final);
+ if (i != 0) {
+ problems[i]->WarmStart(*problems[i - 1]);
+ }
+
+ problems[i]->Solve();
+ X = plant.A() * X + plant.B() * problems[i]->U(0);
+ }
+
+ EXPECT_NEAR(X_final.x(), X.x(), 1e-2);
+ EXPECT_NEAR(X_final.y(), X.y(), 1e-2);
+}
+
+} // namespace testing
+} // namespace catapult
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2022
diff --git a/y2022/control_loops/superstructure/catapult/mpc.tex b/y2022/control_loops/superstructure/catapult/mpc.tex
new file mode 100644
index 0000000..b7110fe
--- /dev/null
+++ b/y2022/control_loops/superstructure/catapult/mpc.tex
@@ -0,0 +1,202 @@
+\documentclass[a4paper,12pt]{article}
+\usepackage{amsmath}
+\usepackage{graphicx}
+\begin{document}
+
+TODO(austin): Now that the python matches the original problem and solves, confirm the paper matches what got implemented.
+
+osqp!
+
+\section{Catapult MPC}
+We want to phrase our problem as trying to solve for the set of control inputs
+which get us closest to the destination, but minimizes acceleration.
+Specifically, we want to minimize acceleration close to the end.
+We also have a voltage limit.
+
+Our model is
+
+\begin{equation}
+\label{cost}
+ \begin{bmatrix} x_1 \\ v_1 \end{bmatrix} =
+ \begin{bmatrix} a_{00} & a_{01} \\ 0 & a_{11} \end{bmatrix}
+ \begin{bmatrix} x_0 \\ v_0 \end{bmatrix} +
+ \begin{bmatrix} b_{0} \\ b_{1} \end{bmatrix} \begin{bmatrix} u_0 \end{bmatrix}
+\end{equation}
+
+Our acceleration can be measured as:
+
+\begin{equation}
+\label{accel}
+ \frac{ \left( \boldsymbol{X_1(1)} - \boldsymbol{X_1(0)} \right)}{\Delta t}
+\end{equation}
+
+This simplifies to:
+
+\begin{equation}
+ \frac{a_{11} v_0 + b_1 u_0 - v_0}{\Delta t}
+\end{equation}
+
+and finally
+
+\begin{equation}
+ \frac{(a_{11} - 1) v_0 + b_1 u_0}{\Delta t}
+\end{equation}
+
+
+We can also compute our state matrix as a function of inital state and the control inputs.
+
+\begin{equation}
+\label{all_x}
+ \begin{bmatrix} X_1 \\ X_2 \\ X_3 \\ \vdots \end{bmatrix} =
+ \begin{bmatrix} A \\
+ A^2 \\
+ A^3 \\
+ \vdots \end{bmatrix}
+ X_0 +
+ \begin{bmatrix} B & 0 & 0 & 0 \\
+ A B & B & 0 & 0 \\
+ A^2 B & A B & B & 0 \\
+ \vdots & \ddots & & \hdots \end{bmatrix}
+ \begin{bmatrix} U_0 \\ U_1 \\ U_2 \\ \vdots \end{bmatrix}
+\end{equation}
+
+\section{MPC problem formulation}
+
+We want to penalize both final state and intermediate acceleration.
+
+\begin{equation}
+C = \sum_{n=0}^{39} \frac{\left(v(n + 1) - v(n)\right)^2}{\Delta t} \pi_n + (X_{40} - X_{final})^T Q_{final} (X_{40} - X_{final})
+\end{equation}
+
+where $\pi_n$ is a constant only dependent on $n$, and designed such that it depends on the distance to the end of the sequence, not the distance from the start.
+
+In order to use OSQP, which has a code generator, we need to get this into the form of
+
+\begin{tabular}{ l l }
+minimize & $ \frac{1}{2} X^T P X + q^T X $ \\
+subject to & $ l <= A X <= u $ \\
+\end{tabular}
+
+This is the simplest form of a constrained quadratic program that we can solve efficiently.
+Luckily for us, the problem statement above fits that definition.
+
+\section{Manipulating the formulation}
+
+We need to separate constant factors from things dependent on U (or X in OSQP parlance) so we can create these matrices easier.
+
+\subsection{Terminal cost}
+
+Next step is to compute $X_{40}$ using equation \ref{all_x}.
+We can do this by only computing the final row of the matrix.
+
+\begin{equation}
+\label{x40}
+ X_{40} = \begin{bmatrix} A^{39} B & A^{38} B & \hdots & B \end{bmatrix}
+ \begin{bmatrix} U_0 \\
+ \vdots \\
+ U_{39}
+ \end{bmatrix} + A^{40} X_0 = B_f \boldsymbol{U} + A^{40} X_0
+\end{equation}
+
+We can substitute equation \ref{x40} into equation \ref{cost}.
+
+\begin{equation}
+\label{final_cost}
+\begin{aligned}[t]
+ C_f = & \boldsymbol{U}^T B_f^T Q_{final} B_f \boldsymbol{U} \\
+ & + 2 (A^{40} X_0 - X_{final})^T Q_{final} B_f \boldsymbol{U} \\
+ & + (A^{40} X_0 - X_{final})^T Q_{final} (A^{40} X_0 - X_{final})
+\end{aligned}
+\end{equation}
+
+\subsection{Acceleration costs}
+
+We can compute a velocity matrix for all the times by stripping out the positions from equation \ref{all_x} by using every other row.
+We can use this to then compute the accelerations for each time slice and then penalize them.
+
+\begin{equation}
+ \begin{bmatrix} v_1 \\ v_2 \\ \vdots \\ v_{40} \end{bmatrix} =
+ M \boldsymbol{U} + \begin{bmatrix} a_{11} \\ a^2_{11} \\ \vdots \\ a^{40}_{11} \end{bmatrix} v_0 =
+ M \boldsymbol{U} + m v_0
+\end{equation}
+
+We can then use equation \ref{accel} in matrix form to convert a velocity matrix to an acceleration matrix.
+
+\begin{equation}
+ \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \alpha_3 \\ \vdots \\ \alpha_{40} \end{bmatrix} =
+ \frac{1}{\Delta t} \left(
+ \begin{bmatrix} 1 & 0 & 0 & \hdots & 0 \\
+ -1 & 1 & 0 & \hdots & 0 \\
+ 0 & -1 & 1 & \hdots & 0 \\
+ \vdots & & & \ddots & \vdots \\
+ 0 & 0 & \hdots & -1 & 1 \\
+ \end{bmatrix}
+ \begin{bmatrix} v_1 \\ v_2 \\ \vdots \\ v_{40} \end{bmatrix} - \begin{bmatrix} v_0 \\ 0 \\ \vdots \\ 0 \end{bmatrix}
+\right)
+\end{equation}
+
+We can pull some of these terms out to make it easier to work with.
+
+\begin{equation}
+\boldsymbol{\alpha} = W V + w v_0
+\end{equation}
+
+Our acceleration cost function is then:
+
+\begin{equation}
+C_a = \boldsymbol{\alpha}^T
+ \begin{bmatrix} \pi_1 & & 0 \\
+ & \pi_2 & \\
+ 0 & & \ddots \end{bmatrix} \boldsymbol{\alpha} =
+ \boldsymbol{\alpha}^T \Pi \boldsymbol{\alpha}
+\end{equation}
+
+We can substitute everything in to get something as a function of $U$.
+
+\begin{equation}
+C_a = \left(W \left(M \boldsymbol{U} + m v_0 \right) + w v_0 \right)^T \Pi \left(W \left(M \boldsymbol{U} + m v_0 \right) + w v_0 \right)
+\end{equation}
+
+And then simplify this down into the expected form.
+
+\begin{equation}
+ C_a = \left(W M \boldsymbol{U} + (W m + w) v_0 \right)^T \Pi \left(W M \boldsymbol{U} + (W m + w) v_0 \right)
+\end{equation}
+
+\begin{equation}
+\label{accel_cost}
+\begin{aligned}[t]
+C_a = & \boldsymbol{U}^T M^T W^T \Pi W M \boldsymbol{U} \\
+ & + 2 v_0 (W m + w)^T \Pi W M \boldsymbol{U} \\
+& + v_0 (W m + w)^T \Pi \left(W m + w \right) v_0
+\end{aligned}
+\end{equation}
+
+\subsection{Overall cost}
+
+We can combine equations \ref{final_cost} and \ref{accel_cost} to get our overall cost in the correct form.
+
+\begin{equation}
+\begin{aligned}[t]
+C &= \boldsymbol{U}^T \left( M^T W^T \Pi W M + B_f^T Q_{final} B_f \right) \boldsymbol{U} \\
+ &+ \left(2 v_0 (W m + w)^T \Pi W M
+ - 2 X_{final}^T Q_{final} B_f
+\right) \boldsymbol{U} \\
+& + X_{final}^T Q_{final} X_{final}
+ + v_0 (W m + w)^T \Pi \left(W m + w \right) v_0
+\end{aligned}
+\end{equation}
+
+
+\section{Response}
+
+For a reasonable request (11 m/s after 90 degrees), we get the following response
+
+\begin{figure}
+ \centering
+ \includegraphics[width=\linewidth]{response.png}
+\end{figure}
+
+This is well within 1\% error, and avoid saturation and keeps the acceleration down at the end.
+
+\end{document}
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 676a4b9..965cc67 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -86,6 +86,7 @@
],
hdrs = [
"camera_reader.h",
+ "gpio.h",
],
data = [
"//y2022:config",
diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index 330dcbc..2a0bfbc 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -15,6 +15,7 @@
#include "frc971/vision/vision_generated.h"
#include "y2022/vision/calibration_data.h"
#include "y2022/vision/calibration_generated.h"
+#include "y2022/vision/gpio.h"
#include "y2022/vision/target_estimate_generated.h"
namespace y2022 {
@@ -22,7 +23,8 @@
using namespace frc971::vision;
-// TODO<Jim/Milind>: Need to add in senders to send out the blob data/stats
+// TODO<jim>: Probably need to break out LED control to separate process
+// TODO<jim>: Need to add sync with camera to strobe lights
class CameraReader {
public:
@@ -36,11 +38,21 @@
image_sender_(event_loop->MakeSender<CameraImage>("/camera")),
target_estimate_sender_(
event_loop->MakeSender<TargetEstimate>("/camera")),
- read_image_timer_(event_loop->AddTimer([this]() { ReadImage(); })) {
+ read_image_timer_(event_loop->AddTimer([this]() { ReadImage(); })),
+ gpio_pwm_control_(GPIOPWMControl(GPIO_PIN_SCK_PWM, duty_cycle_)),
+ gpio_disable_control_(
+ GPIOControl(GPIO_PIN_MOSI_DISABLE, kGPIOOut, kGPIOLow)) {
event_loop->OnRun(
[this]() { read_image_timer_->Setup(event_loop_->monotonic_now()); });
}
+ void SetDutyCycle(double duty_cycle) {
+ duty_cycle_ = duty_cycle;
+ gpio_pwm_control_.setPWMDutyCycle(duty_cycle_);
+ }
+
+ double GetDutyCycle() { return duty_cycle_; }
+
private:
const calibration::CameraCalibration *FindCameraCalibration() const;
@@ -86,6 +98,10 @@
// We schedule this immediately to read an image. Having it on a timer
// means other things can run on the event loop in between.
aos::TimerHandler *const read_image_timer_;
+
+ double duty_cycle_ = 0.0;
+ GPIOPWMControl gpio_pwm_control_;
+ GPIOControl gpio_disable_control_;
};
} // namespace vision
diff --git a/y2022/vision/camera_reader_main.cc b/y2022/vision/camera_reader_main.cc
index 52ee51a..a82d2ac 100644
--- a/y2022/vision/camera_reader_main.cc
+++ b/y2022/vision/camera_reader_main.cc
@@ -6,6 +6,7 @@
// bazel run //y2022/vision:camera_reader -- --config y2022/config.json
// --override_hostname pi-7971-1 --ignore_timestamps true
DEFINE_string(config, "config.json", "Path to the config file to use.");
+DEFINE_double(duty_cycle, 0.5, "Duty cycle of the LEDs");
DEFINE_uint32(exposure, 5,
"Exposure time, in 100us increments; 0 implies auto exposure");
@@ -40,6 +41,7 @@
CameraReader camera_reader(&event_loop, &calibration_data.message(),
&v4l2_reader);
+ camera_reader.SetDutyCycle(FLAGS_duty_cycle);
event_loop.Run();
}
diff --git a/y2022/vision/gpio.h b/y2022/vision/gpio.h
new file mode 100644
index 0000000..90ad812
--- /dev/null
+++ b/y2022/vision/gpio.h
@@ -0,0 +1,353 @@
+#ifndef Y2022_VISION_GPIO_H_
+#define Y2022_VISION_GPIO_H_
+
+/* Modified from: https://elinux.org/RPi_GPIO_Code_Samples
+ * Raspberry Pi GPIO example using sysfs interface.
+ */
+
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include "aos/init.h"
+
+namespace y2022 {
+namespace vision {
+
+using namespace frc971::vision;
+
+// Physical pin 19 maps to sysfs pin 10
+// This pin is MOSI, being used to control the LED Disable
+static constexpr int GPIO_PIN_MOSI_DISABLE = 10;
+
+// Physical pin 33 maps to sysfs pin 13
+// This pin is SCK, being used to control the LED PWM
+static constexpr int GPIO_PIN_SCK_PWM = 13;
+
+static constexpr int BUFFER_MAX = 3;
+static constexpr int VALUE_MAX = 30;
+
+// Is GPIO an input (read) or output (write)
+static constexpr int kGPIOIn = 0;
+static constexpr int kGPIOOut = 1;
+
+// Pin voltage level low or high
+static constexpr int kGPIOLow = 0;
+static constexpr int kGPIOHigh = 1;
+
+class GPIOControl {
+ public:
+ GPIOControl(int pin, int direction, int default_value = kGPIOLow)
+ : pin_(pin), direction_(direction), default_value_(default_value) {
+ // Set up the pin
+ GPIOExport();
+ GPIODirection();
+
+ // Get the file descriptor used to read / write
+ gpio_rw_fd_ = GPIOGetFileDesc();
+
+ // If it's an output, set it in the appropriate default state
+ if (direction_ == kGPIOOut) {
+ GPIOWrite(default_value_);
+ }
+ }
+
+ ~GPIOControl() {
+ if (direction_ == kGPIOOut) {
+ // Set pin to "default" value (e.g., turn off before closing)
+ GPIOWrite(default_value_);
+ }
+
+ close(gpio_rw_fd_);
+ GPIOUnexport();
+ }
+
+ //
+ // Set up pin for active use ("export" it)
+ // Keeping this static method that allows exporting a specific pin #
+ //
+ static int GPIOExport(int pin) {
+ VLOG(2) << "GPIOExport of pin " << pin;
+
+ char buffer[BUFFER_MAX];
+ size_t bytes_to_write, bytes_written;
+
+ int fd = open("/sys/class/gpio/export", O_WRONLY);
+ if (-1 == fd) {
+ LOG(INFO) << "Failed to open export for writing!";
+ return (-1);
+ }
+
+ bytes_to_write = snprintf(buffer, BUFFER_MAX, "%d", pin);
+ bytes_written = write(fd, buffer, bytes_to_write);
+ if (bytes_to_write != bytes_written) {
+ LOG(INFO) << "Issue exporting GPIO pin " << pin
+ << ". May be OK if pin already exported";
+ }
+
+ close(fd);
+
+ // Adding this sleep, since we get some sort of race condition
+ // if we try to act on this too soon after export
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ return 0;
+ }
+
+ // For calling pin defined by this class
+ int GPIOExport() { return GPIOExport(pin_); }
+
+ //
+ // Remove pin from active use ("unexport" it)
+ // Keeping this static method that allows unexporting of a specific pin #
+ //
+ static int GPIOUnexport(int pin) {
+ VLOG(2) << "GPIOUnexport of pin " << pin;
+
+ int fd = open("/sys/class/gpio/unexport", O_WRONLY);
+ if (-1 == fd) {
+ LOG(INFO) << "Failed to open unexport for writing!";
+ return (-1);
+ }
+
+ char buffer[BUFFER_MAX];
+ ssize_t bytes_to_write, bytes_written;
+ bytes_to_write = snprintf(buffer, BUFFER_MAX, "%d", pin);
+ bytes_written = write(fd, buffer, bytes_to_write);
+ if (bytes_to_write != bytes_written) {
+ LOG(INFO) << "Issue unexporting GPIO pin " << pin
+ << ". May be OK if pin already unexported";
+ }
+
+ close(fd);
+ return 0;
+ }
+
+ int GPIOUnexport() { return GPIOUnexport(pin_); }
+
+ //
+ // Set input / output direction of a pin
+ // Keeping this static method that allows setting direction of a specific pin
+ //
+ static int GPIODirection(int pin, int dir) {
+ VLOG(2) << "Setting GPIODirection for pin " << pin << " to " << dir;
+ constexpr int DIRECTION_MAX = 50;
+ char path[DIRECTION_MAX];
+
+ snprintf(path, DIRECTION_MAX, "/sys/class/gpio/gpio%d/direction", pin);
+ int fd = open(path, O_WRONLY);
+ if (-1 == fd) {
+ LOG(ERROR) << "Failed to open gpio direction for writing!\nPath = "
+ << path;
+ return (-1);
+ }
+
+ if (-1 == write(fd, ((kGPIOIn == dir) ? "in" : "out"),
+ ((kGPIOIn == dir) ? 2 : 3))) {
+ LOG(ERROR) << "Failed to set direction!";
+ return (-1);
+ }
+
+ close(fd);
+ // TODO: See if we can remove this sleep
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ return 0;
+ }
+
+ // Call for internal use of pin_ and direction_
+ int GPIODirection() { return GPIODirection(pin_, direction_); }
+
+ //
+ // Read pin
+ // Keeping this static method that allows reading of a specific pin with no
+ // file descriptor open
+ //
+ static int GPIORead(int pin, int fd = -1) {
+ char value_str[3];
+ bool need_to_close_fd = false;
+
+ if (fd == -1) {
+ char path[VALUE_MAX];
+
+ snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
+ fd = open(path, O_RDONLY);
+ if (-1 == fd) {
+ LOG(ERROR) << "Failed to open gpio value for reading on pin " << pin;
+ return (-1);
+ }
+ need_to_close_fd = true;
+ }
+
+ if (-1 == read(fd, value_str, 3)) {
+ LOG(ERROR) << "Failed to read value on pin " << pin;
+ return (-1);
+ }
+
+ if (need_to_close_fd) {
+ close(fd);
+ }
+
+ return (atoi(value_str));
+ }
+
+ int GPIORead() { return GPIORead(pin_, gpio_rw_fd_); }
+
+ //
+ // Write to pin
+ // Keeping this static method that allows writing to a specific pin with no
+ // file descriptor open
+ //
+ static int GPIOWrite(int pin, int value, int fd = -1) {
+ static const char s_values_str[] = "01";
+ bool need_to_close_fd = false;
+
+ if (fd == -1) {
+ char path[VALUE_MAX];
+ snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
+ fd = open(path, O_WRONLY);
+ if (-1 == fd) {
+ LOG(ERROR) << "Failed to open gpio pin " << pin
+ << " for reading / writing";
+ return (-1);
+ }
+ LOG(INFO) << "Opened fd as " << fd;
+ need_to_close_fd = true;
+ }
+
+ if (1 != write(fd, &s_values_str[(kGPIOLow == value) ? 0 : 1], 1)) {
+ LOG(ERROR) << "Failed to write value " << value << " to pin " << pin;
+ return (-1);
+ }
+
+ if (need_to_close_fd) {
+ close(fd);
+ }
+ return 0;
+ }
+
+ int GPIOWrite(int value) { return GPIOWrite(pin_, value, gpio_rw_fd_); }
+
+ int GPIOGetFileDesc() {
+ char path[VALUE_MAX];
+ snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin_);
+ int fd = open(path, O_WRONLY);
+ if (-1 == fd) {
+ LOG(ERROR) << "Failed to open gpio pin " << pin_
+ << " for reading / writing";
+ return (-1);
+ }
+ return fd;
+ }
+
+ // pin # to read / write to
+ int pin_;
+ // direction for reading / writing
+ int direction_;
+ // A default ("safe") value to set the pin to on start / finish
+ int default_value_;
+ // file descriptor for reading / writing
+ int gpio_rw_fd_;
+};
+
+// Control GPIO pin using HW Control
+// A bit hacky-- uses system call to gpio library
+
+class GPIOPWMControl {
+ public:
+ GPIOPWMControl(int pin, double duty_cycle)
+ : pin_(pin), gpio_control_(GPIOControl(pin_, kGPIOOut, kGPIOLow)) {
+ VLOG(2) << "Using gpio command-based PWM control";
+ // Set pin to PWM mode
+ std::string cmd = "gpio -g mode " + std::to_string(pin_) + " pwm";
+ int ret = std::system(cmd.c_str());
+ CHECK_EQ(ret, 0) << "cmd: " + cmd + " failed with return value " << ret;
+
+ // Set PWM mode in Mark-Space mode (duty cycle is on, then off)
+ cmd = "gpio pwm-ms";
+ ret = std::system(cmd.c_str());
+ CHECK_EQ(ret, 0) << "cmd: " + cmd + " failed with return value " << ret;
+
+ // Our PWM clock is going at 19.2 MHz
+ // With a clock divisor of 192, we get a clock tick of 100kHz
+ // or 10 us / tick
+ cmd = "gpio pwmc 192";
+ ret = std::system(cmd.c_str());
+ CHECK_EQ(ret, 0) << "cmd: " + cmd + " failed with return value " << ret;
+
+ // With a range of 100, and 10 us / tick, this gives
+ // a period of 100*10us = 1ms, or 1kHz frequency
+ cmd = "gpio pwmr " + std::to_string(kRange);
+ ret = std::system(cmd.c_str());
+ CHECK_EQ(ret, 0) << "cmd: " + cmd + " failed with return value " << ret;
+
+ setPWMDutyCycle(duty_cycle);
+ };
+
+ ~GPIOPWMControl() { setPWMDutyCycle(0.0); }
+
+ int setPWMDutyCycle(double duty_cycle) {
+ CHECK_GE(duty_cycle, 0.0) << "Duty Cycle must be between 0 and 1";
+ CHECK_LE(duty_cycle, 1.0) << "Duty Cycle must be between 0 and 1";
+ int val = static_cast<int>(duty_cycle * kRange);
+ std::string cmd =
+ "gpio -g pwm " + std::to_string(pin_) + " " + std::to_string(val);
+ int ret = std::system(cmd.c_str());
+ CHECK_EQ(ret, 0) << "cmd: " + cmd + " failed with return value " << ret;
+
+ // TODO: Maybe worth doing error handling / return
+ return ret;
+ }
+
+ static constexpr int kRange = 100;
+ int pin_;
+ GPIOControl gpio_control_;
+};
+
+// Hack up a SW controlled PWM, in case we need it
+
+class GPIOSWPWMControl {
+ public:
+ GPIOSWPWMControl(aos::ShmEventLoop *event_loop, int pin, double frequency,
+ double duty_cycle)
+ : event_loop_(event_loop),
+ pin_(pin),
+ frequency_(frequency),
+ duty_cycle_(duty_cycle),
+ leds_on_(false),
+ gpio_control_(GPIOControl(pin_, kGPIOOut, kGPIOLow)),
+ pwm_timer_(event_loop_->AddTimer([this]() {
+ gpio_control_.GPIOWrite(leds_on_ ? kGPIOHigh : kGPIOLow);
+ int period_us = static_cast<int>(1000000.0 / frequency_);
+ int on_time_us =
+ static_cast<int>(duty_cycle_ * 1000000.0 / frequency_);
+
+ // Trigger the next change
+ // If the leds are currently off, turn them on for duty_cycle % of
+ // period If they are currently on, turn them off for 1 -
+ // duty_cycle % of period
+ pwm_timer_->Setup(
+ event_loop_->monotonic_now() +
+ std::chrono::microseconds(leds_on_ ? (period_us - on_time_us)
+ : on_time_us));
+ leds_on_ = !leds_on_;
+ })) {
+ pwm_timer_->Setup(event_loop_->monotonic_now());
+ };
+
+ void set_duty_cycle(double duty_cycle) { duty_cycle_ = duty_cycle; }
+ void set_frequency(double frequency) { frequency_ = frequency; }
+
+ aos::ShmEventLoop *const event_loop_;
+ int pin_;
+ double frequency_;
+ double duty_cycle_;
+ bool leds_on_;
+ GPIOControl gpio_control_;
+ aos::TimerHandler *const pwm_timer_;
+};
+
+} // namespace vision
+} // namespace y2022
+#endif // Y2022_VISION_GPIO_H_
diff --git a/y2022/vision/leds_ctrl.sh b/y2022/vision/leds_ctrl.sh
new file mode 100755
index 0000000..172a4d3
--- /dev/null
+++ b/y2022/vision/leds_ctrl.sh
@@ -0,0 +1,25 @@
+#!/bin/bash
+
+# Helper script to turn LEDS on or off
+# pass argument "off" to turn off; otherwise, it turns them on
+
+
+if [ "$1" == "off" ]; then
+ echo "Turning LEDS off"
+ if [ -e /sys/class/gpio/gpio13/value ]; then
+ echo 0 > /sys/class/gpio/gpio13/value
+ echo 13 > /sys/class/gpio/unexport
+ fi
+ if [ -e /sys/class/gpio/gpio10/value ]; then
+ echo 1 > /sys/class/gpio/gpio10/value
+ echo 10 > /sys/class/gpio/unexport
+ fi
+else
+ echo "Turning LEDS on full"
+ echo 13 > /sys/class/gpio/export
+ echo 10 > /sys/class/gpio/export
+ echo "out" > /sys/class/gpio/gpio10/direction
+ echo "out" > /sys/class/gpio/gpio13/direction
+ echo 1 > /sys/class/gpio/gpio13/value
+ echo 0 > /sys/class/gpio/gpio10/value
+fi