Add some "simplified" swerve dynamics
This provides some dynamics which ignore a bunch of the dynamics from
the more complete physics, with the intention of trying to provide
something that is cheaper to linearize and work with while being easier
to verify/reason about than the more complex physics.
Change-Id: I654d42d3fbdd2fa9f8f7e666f72ff99d4ce3ada0
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
index f51ba04..e52efcd 100644
--- a/frc971/control_loops/swerve/BUILD
+++ b/frc971/control_loops/swerve/BUILD
@@ -368,3 +368,28 @@
"//aos/testing:googletest",
],
)
+
+cc_library(
+ name = "simplified_dynamics",
+ hdrs = ["simplified_dynamics.h"],
+ deps = [
+ ":auto_diff_jacobian",
+ ":eigen_dynamics",
+ ":motors",
+ "//aos/util:math",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
+ ],
+)
+
+cc_test(
+ name = "simplified_dynamics_test",
+ srcs = ["simplified_dynamics_test.cc"],
+ deps = [
+ ":simplified_dynamics",
+ "//aos/testing:googletest",
+ "//aos/time",
+ "//frc971/control_loops:jacobian",
+ "@com_google_absl//absl/log",
+ ],
+)
diff --git a/frc971/control_loops/swerve/simplified_dynamics.h b/frc971/control_loops/swerve/simplified_dynamics.h
new file mode 100644
index 0000000..9afe636
--- /dev/null
+++ b/frc971/control_loops/swerve/simplified_dynamics.h
@@ -0,0 +1,372 @@
+#ifndef FRC971_CONTROL_LOOPS_SWERVE_SIMPLIFIED_DYNAMICS_H_
+#define FRC971_CONTROL_LOOPS_SWERVE_SIMPLIFIED_DYNAMICS_H_
+#include "absl/log/check.h"
+#include "absl/log/log.h"
+
+#include "aos/util/math.h"
+#include "frc971/control_loops/swerve/auto_diff_jacobian.h"
+#include "frc971/control_loops/swerve/dynamics.h"
+#include "frc971/control_loops/swerve/motors.h"
+
+namespace frc971::control_loops::swerve {
+
+// Provides a simplified set of physics representing a swerve drivetrain.
+// Broadly speaking, these dynamics model:
+// * Standard motors on the drive and steer axes, with no coupling between
+// the motors.
+// * Assume that the steer direction of each module is only influenced by
+// the inertia of the motor rotor plus some small aligning force.
+// * Assume that torque from the drive motor is transferred directly to the
+// carpet.
+// * Assume that a lateral force on the wheel is generated proportional to the
+// slip angle of the wheel.
+//
+// This class is templated on a Scalar that is used to determine whether you
+// want to use double or single-precision floats for the calculations here.
+//
+// Several individual methods on this class are also templated on a LocalScalar
+// type. This is provided to allow those methods to be called with ceres Jets to
+// do autodifferentiation within various solvers/jacobian calculators.
+template <typename Scalar = double>
+class SimplifiedDynamics {
+ public:
+ struct ModuleParams {
+ // Module position relative to the center of mass of the robot.
+ Eigen::Matrix<Scalar, 2, 1> position;
+ // Coefficient dictating how much sideways force is generated at a given
+ // slip angle. Units are effectively Newtons / radian of slip.
+ Scalar slip_angle_coefficient;
+ // Coefficient dicating how much the steer wheel is forced into alignment
+ // by the motion of the wheel over the ground (i.e., we are assuming that
+ // if you push the robot along it will cause the wheels to eventually
+ // align with the direction of motion).
+ // In radians / sec^2 / radians of slip angle.
+ Scalar slip_angle_alignment_coefficient;
+ // Parameters for the steer and drive motors.
+ Motor steer_motor;
+ Motor drive_motor;
+ // radians of module steering = steer_ratio * radians of motor shaft
+ Scalar steer_ratio;
+ // meters of driving = drive_ratio * radians of motor shaft
+ Scalar drive_ratio;
+ };
+ struct Parameters {
+ // Mass of the robot, in kg.
+ Scalar mass;
+ // Moment of inertia of the robot about the yaw axis, in kg * m^2.
+ Scalar moment_of_inertia;
+ // Note: While this technically would support an arbitrary number of
+ // modules, the statically-sized state vectors do limit us to 4 modules
+ // currently, and it should not be counted on that other pieces of code will
+ // be able to support non-4-module swerves.
+ std::vector<ModuleParams> modules;
+ };
+ enum States {
+ // Thetas* and Omegas* are the yaw and yaw rate of the indicated modules.
+ // (note that we do not actually need to track drive speed per module,
+ // as with current control we have the ability to directly command torque
+ // to those motors; however, if we wished to account for the saturation
+ // limits on the motor, then we would need to have access to those states,
+ // although they can be fully derived from the robot vx, vy, theta, and
+ // omega).
+ kThetas0 = 0,
+ kOmegas0 = 1,
+ kThetas1 = 2,
+ kOmegas1 = 3,
+ kThetas2 = 4,
+ kOmegas2 = 5,
+ kThetas3 = 6,
+ kOmegas3 = 7,
+ // Robot yaw, in radians.
+ kTheta = 8,
+ // Robot speed in the global frame, in meters / sec.
+ kVx = 9,
+ kVy = 10,
+ // Robot yaw rate, in radians / sec.
+ kOmega = 11,
+ kNumVelocityStates = 12,
+ // Augmented states for doing position control.
+ // Robot X position in the global frame.
+ kX = 12,
+ // Robot Y position in the global frame.
+ kY = 13,
+ kNumPositionStates = 14,
+ };
+ using Inputs = InputStates::States;
+
+ template <typename ScalarT = Scalar>
+ using VelocityState = Eigen::Matrix<ScalarT, kNumVelocityStates, 1>;
+ template <typename ScalarT = Scalar>
+ using PositionState = Eigen::Matrix<ScalarT, kNumPositionStates, 1>;
+ template <typename ScalarT = Scalar>
+ using VelocityStateSquare =
+ Eigen::Matrix<ScalarT, kNumVelocityStates, kNumVelocityStates>;
+ template <typename ScalarT = Scalar>
+ using PositionStateSquare =
+ Eigen::Matrix<ScalarT, kNumPositionStates, kNumPositionStates>;
+ template <typename ScalarT = Scalar>
+ using PositionBMatrix =
+ Eigen::Matrix<ScalarT, kNumPositionStates, kNumInputs>;
+ template <typename ScalarT = Scalar>
+ using Input = Eigen::Matrix<ScalarT, kNumInputs, 1>;
+
+ SimplifiedDynamics(const Parameters ¶ms) : params_(params) {
+ for (size_t module_index = 0; module_index < params_.modules.size();
+ ++module_index) {
+ module_dynamics_.emplace_back(params_, module_index);
+ }
+ }
+
+ // Returns the derivative of state for the given state and input.
+ template <typename LocalScalar>
+ PositionState<LocalScalar> Dynamics(const PositionState<LocalScalar> &state,
+ const Input<LocalScalar> &input) const {
+ PositionState<LocalScalar> Xdot = PositionState<LocalScalar>::Zero();
+
+ for (const ModuleDynamics &module : module_dynamics_) {
+ Xdot += module.PartialDynamics(state, input);
+ }
+
+ // And finally catch the global states:
+ Xdot(kX) = state(kVx);
+ Xdot(kY) = state(kVy);
+ Xdot(kTheta) = state(kOmega);
+
+ return Xdot;
+ }
+
+ template <typename LocalScalar>
+ VelocityState<LocalScalar> VelocityDynamics(
+ const VelocityState<LocalScalar> &state,
+ const Input<LocalScalar> &input) const {
+ PositionState<LocalScalar> input_state = PositionState<LocalScalar>::Zero();
+ input_state.template topRows<kNumVelocityStates>() = state;
+ return Dynamics(input_state, input).template topRows<kNumVelocityStates>();
+ }
+
+ std::pair<PositionStateSquare<>, PositionBMatrix<>> LinearizedDynamics(
+ const PositionState<> &state, const Input<> &input) {
+ DynamicsFunctor functor(*this);
+ Eigen::Matrix<Scalar, kNumPositionStates + kNumInputs, 1> parameters;
+ parameters.template topRows<kNumPositionStates>() = state;
+ parameters.template bottomRows<kNumInputs>() = input;
+ const Eigen::Matrix<Scalar, kNumPositionStates,
+ kNumPositionStates + kNumInputs>
+ jacobian =
+ AutoDiffJacobian<Scalar, DynamicsFunctor,
+ kNumPositionStates + kNumInputs,
+ kNumPositionStates>::Jacobian(functor, parameters);
+ return {
+ jacobian.template block<kNumPositionStates, kNumPositionStates>(0, 0),
+ jacobian.template block<kNumPositionStates, kNumInputs>(
+ 0, kNumPositionStates)};
+ }
+
+ private:
+ // Wrapper to provide an operator() for the dynamisc class that allows it to
+ // be used by the auto-differentiation code.
+ class DynamicsFunctor {
+ public:
+ DynamicsFunctor(const SimplifiedDynamics &dynamics) : dynamics_(dynamics) {}
+
+ template <typename LocalScalar>
+ Eigen::Matrix<LocalScalar, kNumPositionStates, 1> operator()(
+ const Eigen::Map<const Eigen::Matrix<
+ LocalScalar, kNumPositionStates + kNumInputs, 1>>
+ input) const {
+ return dynamics_.Dynamics(
+ PositionState<LocalScalar>(
+ input.template topRows<kNumPositionStates>()),
+ Input<LocalScalar>(input.template bottomRows<kNumInputs>()));
+ }
+
+ private:
+ const SimplifiedDynamics &dynamics_;
+ };
+
+ // Represents the dynamics of an individual module.
+ class ModuleDynamics {
+ public:
+ ModuleDynamics(const Parameters &robot_params, const size_t module_index)
+ : robot_params_(robot_params), module_index_(module_index) {
+ CHECK_LT(module_index_, robot_params_.modules.size());
+ }
+
+ // This returns the portions of the derivative of state that are due to the
+ // individual module. The result from this function should be able to be
+ // naively summed with the dynamics for each other module plus some global
+ // dynamics (which take care of that e.g. xdot = vx) and give you the
+ // overall dynamics of the system.
+ template <typename LocalScalar>
+ PositionState<LocalScalar> PartialDynamics(
+ const PositionState<LocalScalar> &state,
+ const Input<LocalScalar> &input) const {
+ PositionState<LocalScalar> Xdot = PositionState<LocalScalar>::Zero();
+
+ Xdot(ThetasIdx()) = state(OmegasIdx());
+
+ // Steering dynamics for an individual module assume ~zero friction,
+ // and thus ~the only inertia is from the motor rotor itself.
+ // torque_motor = stall_torque / stall_current * current
+ // accel_motor = torque_motor / motor_inertia
+ // accel_steer = accel_motor * steer_ratio
+ const Motor &steer_motor = module_params().steer_motor;
+ const LocalScalar steer_motor_accel =
+ input(IsIdx()) *
+ static_cast<Scalar>(
+ module_params().steer_ratio * steer_motor.stall_torque /
+ (steer_motor.stall_current * steer_motor.motor_inertia));
+
+ // For the impacts of the modules on the overall robot
+ // dynamics (X, Y, and theta acceleration), we calculate the forces
+ // generated by the module and then apply them. These forces come from
+ // two effects in this model:
+ // 1. Slip angle of the module (dependent on the current robot velocity &
+ // module steer angle).
+ // 2. Drive torque from the module (dependent on the current drive
+ // current and module steer angle).
+ // We assume no torque is generated from e.g. the wheel resisting the
+ // steering motion.
+ //
+ // clang-format off
+ //
+ // For slip angle we have:
+ // wheel_velocity = R(-theta - theta_steer) * (
+ // robot_vel + omega.cross(R(theta) * module_position))
+ // slip_angle = -atan2(wheel_velocity)
+ // slip_force = slip_angle_coefficient * slip_angle
+ // slip_force_direction = theta + theta_steer + pi / 2
+ // force_x = slip_force * cos(slip_force_direction)
+ // force_y = slip_force * sin(slip_force_direction)
+ // accel_* = force_* / mass
+ // # And now calculate torque from slip angle.
+ // torque_vec = module_position.cross([slip_force * cos(theta_steer + pi / 2),
+ // slip_force * sin(theta_steer + pi / 2),
+ // 0.0])
+ // torque_vec = module_position.cross([slip_force * -sin(theta_steer),
+ // slip_force * cos(theta_steer),
+ // 0.0])
+ // robot_torque = torque_vec.z()
+ //
+ // For drive torque we have:
+ // drive_force = (drive_current * stall_torque / stall_current) / drive_ratio
+ // drive_force_direction = theta + theta_steer
+ // force_x = drive_force * cos(drive_force_direction)
+ // force_y = drive_force * sin(drive_force_direction)
+ // torque_vec = drive_force * module_position.cross([cos(theta_steer),
+ // sin(theta_steer),
+ // 0.0])
+ // torque = torque_vec.z()
+ //
+ // clang-format on
+
+ const Eigen::Matrix<Scalar, 3, 1> module_position{
+ {module_params().position.x()},
+ {module_params().position.y()},
+ {0.0}};
+ const LocalScalar theta = state(kTheta);
+ const LocalScalar theta_steer = state(ThetasIdx());
+ const Eigen::Matrix<LocalScalar, 3, 1> wheel_velocity_in_global_frame =
+ Eigen::Matrix<LocalScalar, 3, 1>(state(kVx), state(kVy),
+ static_cast<LocalScalar>(0.0)) +
+ (Eigen::Matrix<LocalScalar, 3, 1>(static_cast<LocalScalar>(0.0),
+ static_cast<LocalScalar>(0.0),
+ state(kOmega))
+ .cross(Eigen::AngleAxis<LocalScalar>(
+ theta, Eigen::Matrix<LocalScalar, 3, 1>::UnitZ()) *
+ module_position));
+ const Eigen::Matrix<LocalScalar, 3, 1> wheel_velocity_in_wheel_frame =
+ Eigen::AngleAxis<LocalScalar>(
+ -theta - theta_steer, Eigen::Matrix<LocalScalar, 3, 1>::UnitZ()) *
+ wheel_velocity_in_global_frame;
+ // The complicated dynamics use some obnoxious-looking functions to
+ // try to approximate how the slip angle behaves a low speeds to better
+ // condition the dynamics. Because I couldn't be bothered to copy those
+ // dynamics, instead just bias the slip angle to zero at low speeds.
+ const LocalScalar wheel_speed = wheel_velocity_in_wheel_frame.norm();
+ const Scalar start_speed = 0.1;
+ const LocalScalar heading_truth_proportion = -expm1(
+ /*arbitrary large number=*/static_cast<Scalar>(-100.0) *
+ (wheel_speed - start_speed));
+ const LocalScalar wheel_heading =
+ (wheel_speed < start_speed)
+ ? static_cast<LocalScalar>(0.0)
+ : heading_truth_proportion *
+ atan2(wheel_velocity_in_wheel_frame.y(),
+ wheel_velocity_in_wheel_frame.x());
+
+ // We wrap slip_angle with a sin() not because there is actually a sin()
+ // in the real math but rather because we need to smoothly and correctly
+ // handle slip angles between pi / 2 and 3 * pi / 2.
+ const LocalScalar slip_angle = sin(-wheel_heading);
+ const LocalScalar slip_force =
+ module_params().slip_angle_coefficient * slip_angle;
+ const LocalScalar slip_force_direction =
+ theta + theta_steer + static_cast<Scalar>(M_PI_2);
+ const Eigen::Matrix<LocalScalar, 3, 1> slip_force_vec =
+ slip_force * UnitYawVector<LocalScalar>(slip_force_direction);
+ const LocalScalar slip_torque =
+ module_position
+ .cross(slip_force *
+ UnitYawVector<LocalScalar>(theta_steer +
+ static_cast<Scalar>(M_PI_2)))
+ .z();
+
+ // drive torque calculations
+ const Motor &drive_motor = module_params().drive_motor;
+ const LocalScalar drive_force =
+ input(IdIdx()) * static_cast<Scalar>(drive_motor.stall_torque /
+ drive_motor.stall_current /
+ module_params().drive_ratio);
+ const Eigen::Matrix<LocalScalar, 3, 1> drive_force_vec =
+ drive_force * UnitYawVector<LocalScalar>(theta + theta_steer);
+ const LocalScalar drive_torque =
+ drive_force *
+ module_position.cross(UnitYawVector<LocalScalar>(theta_steer)).z();
+ // We add in an aligning force on the wheels primarily to help provide a
+ // bit of impetus to the controllers/solvers to discourage aggressive
+ // slip angles. If we do not include this, then the dynamics make it look
+ // like there are no losses to using extremely aggressive slip angles.
+ const LocalScalar wheel_alignment_accel =
+ -module_params().slip_angle_alignment_coefficient * slip_angle;
+
+ Xdot(OmegasIdx()) = steer_motor_accel + wheel_alignment_accel;
+ // Sum up all the forces.
+ Xdot(kVx) =
+ (slip_force_vec.x() + drive_force_vec.x()) / robot_params_.mass;
+ Xdot(kVy) =
+ (slip_force_vec.y() + drive_force_vec.y()) / robot_params_.mass;
+ Xdot(kOmega) =
+ (slip_torque + drive_torque) / robot_params_.moment_of_inertia;
+
+ return Xdot;
+ }
+
+ private:
+ template <typename LocalScalar>
+ Eigen::Matrix<LocalScalar, 3, 1> UnitYawVector(LocalScalar yaw) const {
+ return Eigen::Matrix<LocalScalar, 3, 1>{
+ {static_cast<LocalScalar>(cos(yaw))},
+ {static_cast<LocalScalar>(sin(yaw))},
+ {static_cast<LocalScalar>(0.0)}};
+ }
+ size_t ThetasIdx() const { return kThetas0 + 2 * module_index_; }
+ size_t OmegasIdx() const { return kOmegas0 + 2 * module_index_; }
+ size_t IsIdx() const { return Inputs::kIs0 + 2 * module_index_; }
+ size_t IdIdx() const { return Inputs::kId0 + 2 * module_index_; }
+
+ const ModuleParams &module_params() const {
+ return robot_params_.modules[module_index_];
+ }
+
+ const Parameters robot_params_;
+
+ const size_t module_index_;
+ };
+
+ Parameters params_;
+ std::vector<ModuleDynamics> module_dynamics_;
+};
+
+} // namespace frc971::control_loops::swerve
+#endif // FRC971_CONTROL_LOOPS_SWERVE_SIMPLIFIED_DYNAMICS_H_
diff --git a/frc971/control_loops/swerve/simplified_dynamics_test.cc b/frc971/control_loops/swerve/simplified_dynamics_test.cc
new file mode 100644
index 0000000..894a85b
--- /dev/null
+++ b/frc971/control_loops/swerve/simplified_dynamics_test.cc
@@ -0,0 +1,244 @@
+#include "frc971/control_loops/swerve/simplified_dynamics.h"
+
+#include <functional>
+
+#include "absl/log/log.h"
+#include "gtest/gtest.h"
+
+#include "aos/time/time.h"
+#include "frc971/control_loops/jacobian.h"
+
+namespace frc971::control_loops::swerve::testing {
+class SimplifiedDynamicsTest : public ::testing::Test {
+ protected:
+ using Dynamics = SimplifiedDynamics<double>;
+ using PositionState = Dynamics::PositionState<double>;
+ using States = Dynamics::States;
+ using Inputs = Dynamics::Inputs;
+ using Input = Dynamics::Input<double>;
+ using ModuleParams = Dynamics::ModuleParams;
+ using Parameters = Dynamics::Parameters;
+ static ModuleParams MakeModule(const Eigen::Vector2d &position,
+ bool wheel_alignment) {
+ return ModuleParams{
+ .position = position,
+ .slip_angle_coefficient = 200.0,
+ .slip_angle_alignment_coefficient = wheel_alignment ? 1.0 : 0.0,
+ .steer_motor = KrakenFOC(),
+ .drive_motor = KrakenFOC(),
+ .steer_ratio = 0.1,
+ .drive_ratio = 0.01};
+ }
+ static Parameters MakeParams(bool wheel_alignment) {
+ return {.mass = 60,
+ .moment_of_inertia = 2,
+ .modules = {
+ MakeModule({1.0, 1.0}, wheel_alignment),
+ MakeModule({-1.0, 1.0}, wheel_alignment),
+ MakeModule({-1.0, -1.0}, wheel_alignment),
+ MakeModule({1.0, -1.0}, wheel_alignment),
+ }};
+ }
+ SimplifiedDynamicsTest() : dynamics_(MakeParams(false)) {}
+
+ PositionState ValidateDynamics(const PositionState &state,
+ const Input &input) {
+ const PositionState Xdot = dynamics_.Dynamics(state, input);
+ // Sanity check simple invariants:
+ EXPECT_EQ(Xdot(Dynamics::kX), state(Dynamics::kVx));
+ EXPECT_EQ(Xdot(Dynamics::kY), state(Dynamics::kVy));
+ EXPECT_EQ(Xdot(Dynamics::kTheta), state(Dynamics::kOmega));
+
+ // Check that the dynamics linearization produces numbers that match numeric
+ // differentiation of the dynamics.
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
+ const auto linearized_dynamics = dynamics_.LinearizedDynamics(state, input);
+ const aos::monotonic_clock::duration auto_diff_time =
+ aos::monotonic_clock::now() - start_time;
+
+ start_time = aos::monotonic_clock::now();
+ auto numerical_A = NumericalJacobianX(
+ std::bind(&Dynamics::Dynamics<double>, &dynamics_,
+ std::placeholders::_1, std::placeholders::_2),
+ state, input);
+ auto numerical_B = NumericalJacobianU(
+ std::bind(&Dynamics::Dynamics<double>, &dynamics_,
+ std::placeholders::_1, std::placeholders::_2),
+ state, input);
+ const aos::monotonic_clock::duration numerical_time =
+ aos::monotonic_clock::now() - start_time;
+ VLOG(1) << "Autodifferentiation took " << auto_diff_time
+ << " while numerical differentiation took " << numerical_time;
+ EXPECT_LT((numerical_A - linearized_dynamics.first).norm(), 1e-6)
+ << "Numerical result:\n"
+ << numerical_A << "\nAuto-diff result:\n"
+ << linearized_dynamics.first;
+ EXPECT_LT((numerical_B - linearized_dynamics.second).norm(), 1e-6)
+ << "Numerical result:\n"
+ << numerical_B << "\nAuto-diff result:\n"
+ << linearized_dynamics.second;
+ return Xdot;
+ }
+
+ Dynamics dynamics_;
+};
+
+// Test that if all states and inputs are at zero that the robot won't move.
+TEST_F(SimplifiedDynamicsTest, ZeroIsZero) {
+ EXPECT_EQ(PositionState::Zero(),
+ ValidateDynamics(PositionState::Zero(), Input::Zero()));
+}
+
+// Test that if we are travelling straight forwards with zero inputs that we
+// just coast.
+TEST_F(SimplifiedDynamicsTest, CoastForwards) {
+ PositionState state = PositionState::Zero();
+ state(States::kVx) = 1.0;
+ PositionState expected = PositionState::Zero();
+ expected(States::kX) = 1.0;
+ EXPECT_EQ(expected, ValidateDynamics(state, Input::Zero()));
+}
+
+// Tests that we can accelerate the robot.
+TEST_F(SimplifiedDynamicsTest, AccelerateStraight) {
+ // Check that the drive currents behave as anticipated and accelerate the
+ // robot.
+ Input input{{0.0}, {1.0}, {0.0}, {1.0}, {0.0}, {1.0}, {0.0}, {1.0}};
+ PositionState state = PositionState::Zero();
+ state(States::kVx) = 0.0;
+ PositionState result = ValidateDynamics(state, input);
+ EXPECT_EQ(result.norm(), result(States::kVx));
+ EXPECT_LT(0.1, result(States::kVx));
+}
+
+// Test that if we are driving straight sideways (so our wheel are at 90
+// degrees) that we experience a force slowing us down.
+TEST_F(SimplifiedDynamicsTest, ForceWheelsSideways) {
+ PositionState state = PositionState::Zero();
+ state(States::kVy) = 1.0;
+ PositionState result = ValidateDynamics(state, Input::Zero());
+ EXPECT_LT(result.topRows<States::kVy>().norm(), 1e-10)
+ << ": All derivatives prior to the vy state should be ~zero.\n"
+ << result;
+ EXPECT_LT(result(States::kVy), -1.0)
+ << ": expected non-trivial deceleration.";
+ EXPECT_EQ(result(States::kOmega), 0.0);
+}
+
+// Tests that we can make the robot spin in place by orienting all the wheels
+TEST_F(SimplifiedDynamicsTest, SpinInPlaceNoSlip) {
+ PositionState state = PositionState::Zero();
+ state(States::kThetas0) = 3.0 * M_PI / 4.0;
+ state(States::kThetas1) = 5.0 * M_PI / 4.0;
+ state(States::kThetas2) = 7.0 * M_PI / 4.0;
+ state(States::kThetas3) = 1.0 * M_PI / 4.0;
+ state(States::kOmega) = 1.0;
+ PositionState result = ValidateDynamics(state, Input::Zero());
+ EXPECT_NEAR(result.norm(), 1.0, 1e-10)
+ << ": Only non-zero state should be kTheta, which should be exactly 1.0.";
+ EXPECT_EQ(result(States::kTheta), 1.0);
+
+ // Sanity check that when we then apply drive torque to the wheels that that
+ // apins the robot more.
+ Input input{{0.0}, {1.0}, {0.0}, {1.0}, {0.0}, {1.0}, {0.0}, {1.0}};
+ result = ValidateDynamics(state, input);
+ EXPECT_EQ(result(States::kTheta), 1.0);
+ EXPECT_LT(1.0, result(States::kOmega));
+ // Everything else should be ~zero.
+ result(States::kTheta) = 0.0;
+ result(States::kOmega) = 0.0;
+ EXPECT_LT(result.norm(), 1e-10);
+}
+
+// Tests that we can spin in place when skid-steering (i.e., all wheels stay
+// pointed straight, but we still attempt to spint he robot).
+TEST_F(SimplifiedDynamicsTest, SpinInPlaceSkidSteer) {
+ PositionState state = PositionState::Zero();
+ state(States::kThetas0) = -M_PI;
+ state(States::kThetas1) = -M_PI;
+ state(States::kThetas2) = 0.0;
+ state(States::kThetas3) = 0.0;
+ state(States::kOmega) = 1.0;
+ PositionState result = ValidateDynamics(state, Input::Zero());
+ EXPECT_EQ(result(States::kTheta), 1.0);
+ EXPECT_LT(result(States::kOmega), -1.0)
+ << "We should be aggressively decelerrating when slipping wheels.";
+ // Everything else should be ~zero.
+ result(States::kTheta) = 0.0;
+ result(States::kOmega) = 0.0;
+ EXPECT_LT(result.norm(), 1e-10);
+
+ // Sanity check that when we then apply drive torque to the wheels that that
+ // we can counteract the spin.
+ Input input{{0.0}, {100.0}, {0.0}, {100.0}, {0.0}, {100.0}, {0.0}, {100.0}};
+ result = ValidateDynamics(state, input);
+ EXPECT_EQ(result(States::kTheta), 1.0);
+ EXPECT_LT(1.0, result(States::kOmega));
+ // Everything else should be ~zero.
+ result(States::kTheta) = 0.0;
+ result(States::kOmega) = 0.0;
+ EXPECT_LT(result.norm(), 1e-10);
+}
+
+// Tests that we can spin in place when skid-steering backwards (ensures that
+// slip angle calculations and the such handle the sign changes correctly).
+TEST_F(SimplifiedDynamicsTest, SpinInPlaceSkidSteerBackwards) {
+ PositionState state = PositionState::Zero();
+ state(States::kThetas0) = 0.0;
+ state(States::kThetas1) = 0.0;
+ state(States::kThetas2) = M_PI;
+ state(States::kThetas3) = M_PI;
+ state(States::kOmega) = 1.0;
+ PositionState result = ValidateDynamics(state, Input::Zero());
+ EXPECT_EQ(result(States::kTheta), 1.0);
+ EXPECT_LT(result(States::kOmega), -1.0)
+ << "We should be aggressively decelerrating when slipping wheels.";
+ // Everything else should be ~zero.
+ result(States::kTheta) = 0.0;
+ result(States::kOmega) = 0.0;
+ EXPECT_LT(result.norm(), 1e-10);
+
+ // Sanity check that when we then apply drive torque to the wheels that that
+ // we can counteract the spin.
+ Input input{{0.0}, {-100.0}, {0.0}, {-100.0},
+ {0.0}, {-100.0}, {0.0}, {-100.0}};
+ result = ValidateDynamics(state, input);
+ EXPECT_EQ(result(States::kTheta), 1.0);
+ EXPECT_LT(1.0, result(States::kOmega));
+ // Everything else should be ~zero.
+ result(States::kTheta) = 0.0;
+ result(States::kOmega) = 0.0;
+ EXPECT_LT(result.norm(), 1e-10);
+}
+
+// Test that if we turn on the wheel alignment forces that it results in forces
+// that cause the wheels to align to straight over time.
+TEST_F(SimplifiedDynamicsTest, WheelAlignmentForces) {
+ dynamics_ = Dynamics(MakeParams(true));
+ PositionState state = PositionState::Zero();
+ state(States::kThetas0) = -0.1;
+ state(States::kThetas1) = -0.1;
+ state(States::kThetas2) = -0.1;
+ state(States::kThetas3) = -0.1;
+ state(States::kVx) = 1.0;
+ PositionState result = ValidateDynamics(state, Input::Zero());
+ EXPECT_LT(1e-2, result(States::kOmegas0));
+ EXPECT_LT(1e-2, result(States::kOmegas1));
+ EXPECT_LT(1e-2, result(States::kOmegas2));
+ EXPECT_LT(1e-2, result(States::kOmegas3));
+}
+
+// Do some fuzz testing of the jacobian calculations.
+TEST_F(SimplifiedDynamicsTest, Fuzz) {
+ for (size_t state_index = 0; state_index < States::kNumPositionStates;
+ ++state_index) {
+ SCOPED_TRACE(state_index);
+ PositionState state = PositionState::Zero();
+ for (const double value : {-1.0, 0.0, 1.0}) {
+ SCOPED_TRACE(value);
+ state(state_index) = value;
+ ValidateDynamics(state, Input::Zero());
+ }
+ }
+}
+} // namespace frc971::control_loops::swerve::testing