Merge "Add swerve note latex file"
diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
index 0efe040..29dbf76 100644
--- a/frc971/control_loops/swerve/BUILD
+++ b/frc971/control_loops/swerve/BUILD
@@ -126,7 +126,7 @@
)
run_binary(
- name = "dynamics_codegen",
+ name = "dynamics_codegen_cc",
outs = [
"dynamics.cc",
"dynamics.h",
@@ -143,6 +143,32 @@
tool = ":generate_physics",
)
+run_binary(
+ name = "dynamics_codegen_nocaster",
+ outs = [
+ "nocaster_dynamics.py",
+ ],
+ args = [
+ "--caster=0.0",
+ "--output_base=$(BINDIR)/",
+ "--casadi_py_output_path=$(location :nocaster_dynamics.py)",
+ ],
+ tool = ":generate_physics",
+)
+
+run_binary(
+ name = "dynamics_codegen_bigcaster",
+ outs = [
+ "bigcaster_dynamics.py",
+ ],
+ args = [
+ "--caster=0.05",
+ "--output_base=$(BINDIR)/",
+ "--casadi_py_output_path=$(location :bigcaster_dynamics.py)",
+ ],
+ tool = ":generate_physics",
+)
+
cc_library(
name = "dynamics",
srcs = ["dynamics.cc"],
@@ -170,7 +196,9 @@
py_test(
name = "physics_test",
srcs = [
+ "bigcaster_dynamics.py",
"dynamics.py",
+ "nocaster_dynamics.py",
"physics_test.py",
],
target_compatible_with = ["@platforms//cpu:x86_64"],
diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index ee5850c..781943c 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -34,6 +34,7 @@
"Path to write generated py code to");
ABSL_FLAG(std::string, casadi_py_output_path, "",
"Path to write casadi generated py code to");
+ABSL_FLAG(double, caster, 0.01, "Caster in meters for the module.");
ABSL_FLAG(bool, symbolic, false, "If true, write everything out symbolically.");
@@ -56,6 +57,7 @@
using SymEngine::real_double;
using SymEngine::RealDouble;
using SymEngine::Set;
+using SymEngine::sign;
using SymEngine::simplify;
using SymEngine::sin;
using SymEngine::solve;
@@ -173,7 +175,7 @@
robot_width_ = real_double(24.75 * 0.0254);
- caster_ = real_double(0.01);
+ caster_ = real_double(absl::GetFlag(FLAGS_caster));
contact_patch_length_ = real_double(0.02);
}
@@ -377,6 +379,14 @@
result_cc.emplace_back("#include <cmath>");
result_cc.emplace_back("");
result_cc.emplace_back("namespace frc971::control_loops::swerve {");
+ result_cc.emplace_back("namespace {");
+ result_cc.emplace_back("");
+ result_cc.emplace_back("double sign(double x) {");
+ result_cc.emplace_back(" return (x > 0) - (x < 0);");
+ result_cc.emplace_back("}");
+
+ result_cc.emplace_back("");
+ result_cc.emplace_back("} // namespace");
result_cc.emplace_back("");
result_cc.emplace_back("Eigen::Matrix<double, 25, 1> SwervePhysics(");
result_cc.emplace_back(
@@ -436,20 +446,20 @@
}
result_cc.emplace_back(
- absl::Substitute(" result($0, 0) = omega;", kNumModules * 4));
+ absl::Substitute(" result($0, 0) = vx;", kNumModules * 4 + 0));
result_cc.emplace_back(
- absl::Substitute(" result($0, 0) = vx;", kNumModules * 4 + 1));
+ absl::Substitute(" result($0, 0) = vy;", kNumModules * 4 + 1));
result_cc.emplace_back(
- absl::Substitute(" result($0, 0) = vy;", kNumModules * 4 + 2));
+ absl::Substitute(" result($0, 0) = omega;", kNumModules * 4 + 2));
- result_cc.emplace_back(absl::Substitute(
- " result($0, 0) = $1;", kNumModules * 4 + 3, ccode(*angular_accel_)));
result_cc.emplace_back(absl::Substitute(" result($0, 0) = $1;",
- kNumModules * 4 + 4,
+ kNumModules * 4 + 3,
ccode(*accel_.get(0, 0))));
result_cc.emplace_back(absl::Substitute(" result($0, 0) = $1;",
- kNumModules * 4 + 5,
+ kNumModules * 4 + 4,
ccode(*accel_.get(1, 0))));
+ result_cc.emplace_back(absl::Substitute(
+ " result($0, 0) = $1;", kNumModules * 4 + 5, ccode(*angular_accel_)));
result_cc.emplace_back(
absl::Substitute(" result($0, 0) = 0.0;", kNumModules * 4 + 6));
@@ -470,8 +480,9 @@
void WriteCasadiVariables(std::vector<std::string> *result_py) {
result_py->emplace_back(" sin = casadi.sin");
+ result_py->emplace_back(" sign = casadi.sign");
result_py->emplace_back(" cos = casadi.cos");
- result_py->emplace_back(" atan2 = half_atan2");
+ result_py->emplace_back(" atan2 = casadi.atan2");
result_py->emplace_back(" fmax = casadi.fmax");
result_py->emplace_back(" fabs = casadi.fabs");
@@ -525,11 +536,6 @@
absl::Substitute("ROBOT_WIDTH = $0", ccode(*robot_width_)));
result_py.emplace_back(absl::Substitute("CASTER = $0", ccode(*caster_)));
result_py.emplace_back("");
- result_py.emplace_back("def half_atan2(y, x):");
- result_py.emplace_back(
- " return casadi.fmod(casadi.atan2(y, x) + casadi.pi * 3.0 / 2.0, "
- "casadi.pi) - casadi.pi / 2.0");
- result_py.emplace_back("");
result_py.emplace_back("# Returns the derivative of our state vector");
result_py.emplace_back("# [thetas0, thetad0, omegas0, omegad0,");
@@ -770,8 +776,8 @@
VLOG(1) << "wheel ground velocity: "
<< result.wheel_ground_velocity.__str__();
- result.slip_angle = neg(atan2(result.wheel_ground_velocity.get(1, 0),
- result.wheel_ground_velocity.get(0, 0)));
+ result.slip_angle = sin(neg(atan2(result.wheel_ground_velocity.get(1, 0),
+ result.wheel_ground_velocity.get(0, 0))));
VLOG(1);
VLOG(1) << "slip angle: " << result.slip_angle->__str__();
@@ -787,8 +793,12 @@
result.Fwx = simplify(mul(Cx_, result.slip_ratio));
result.Fwy = simplify(mul(Cy_, result.slip_angle));
+ // The self-aligning moment needs to flip when the module flips direction.
result.Ms = mul(neg(result.Fwy),
- add(div(contact_patch_length_, integer(3)), caster_));
+ add(div(mul(sign(result.wheel_ground_velocity.get(0, 0)),
+ contact_patch_length_),
+ integer(3)),
+ caster_));
VLOG(1);
VLOG(1) << "Ms " << result.Ms->__str__();
VLOG(1);
diff --git a/frc971/control_loops/swerve/physics_test.py b/frc971/control_loops/swerve/physics_test.py
index 7f35f56..ecfc1ce 100644
--- a/frc971/control_loops/swerve/physics_test.py
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -6,7 +6,9 @@
from numpy.testing import assert_array_equal, assert_array_almost_equal
import unittest
+from frc971.control_loops.swerve import bigcaster_dynamics
from frc971.control_loops.swerve import dynamics
+from frc971.control_loops.swerve import nocaster_dynamics
def state_vector(velocity=numpy.array([[1.0], [0.0]]),
@@ -70,35 +72,41 @@
class TestSwervePhysics(unittest.TestCase):
I = numpy.zeros((8, 1))
- def setUp(self):
- self.swerve_physics = wrap(dynamics.swerve_physics)
+ def wrap(self, python_module):
+ self.swerve_physics = wrap(python_module.swerve_physics)
self.contact_patch_velocity = [
- wrap_module(dynamics.contact_patch_velocity, i) for i in range(4)
+ wrap_module(python_module.contact_patch_velocity, i)
+ for i in range(4)
]
self.wheel_ground_velocity = [
- wrap_module(dynamics.wheel_ground_velocity, i) for i in range(4)
+ wrap_module(python_module.wheel_ground_velocity, i)
+ for i in range(4)
]
self.wheel_slip_velocity = [
- wrap_module(dynamics.wheel_slip_velocity, i) for i in range(4)
+ wrap_module(python_module.wheel_slip_velocity, i) for i in range(4)
]
self.wheel_force = [
- wrap_module(dynamics.wheel_force, i) for i in range(4)
+ wrap_module(python_module.wheel_force, i) for i in range(4)
]
self.module_angular_accel = [
- wrap_module(dynamics.module_angular_accel, i) for i in range(4)
+ wrap_module(python_module.module_angular_accel, i)
+ for i in range(4)
]
- self.F = [wrap_module(dynamics.F, i) for i in range(4)]
+ self.F = [wrap_module(python_module.F, i) for i in range(4)]
self.mounting_location = [
- wrap_module(dynamics.mounting_location, i) for i in range(4)
+ wrap_module(python_module.mounting_location, i) for i in range(4)
]
self.slip_angle = [
- wrap_module(dynamics.slip_angle, i) for i in range(4)
+ wrap_module(python_module.slip_angle, i) for i in range(4)
]
self.slip_ratio = [
- wrap_module(dynamics.slip_ratio, i) for i in range(4)
+ wrap_module(python_module.slip_ratio, i) for i in range(4)
]
- self.Ms = [wrap_module(dynamics.Ms, i) for i in range(4)]
+ self.Ms = [wrap_module(python_module.Ms, i) for i in range(4)]
+
+ def setUp(self):
+ self.wrap(dynamics)
def test_contact_patch_velocity(self):
"""Tests that the contact patch velocity makes sense."""
@@ -178,29 +186,31 @@
velocity = numpy.array([[1.5], [0.0]])
for i in range(4):
- x = casadi.SX.sym("x")
- y = casadi.SX.sym("y")
- half_atan2 = casadi.Function('half_atan2', [y, x],
- [dynamics.half_atan2(y, x)])
-
- for wrap in range(-3, 3):
+ for wrap in range(-1, 2):
for theta in [0.0, 0.6, -0.4]:
module_angle = numpy.pi * wrap + theta
- self.assertAlmostEqual(
- theta,
- half_atan2(numpy.sin(module_angle),
- numpy.cos(module_angle)))
-
+ # We have redefined the angle to be the sin of the angle.
+ # That way, when the module flips directions, the slip angle also flips
+ # directions to keep it stable.
computed_angle = self.slip_angle[i](state_vector(
velocity=velocity,
module_angle=numpy.pi * wrap + theta), self.I)[0, 0]
- self.assertAlmostEqual(theta, computed_angle)
+ expected = numpy.sin(numpy.pi * wrap + theta)
+
+ self.assertAlmostEqual(
+ expected,
+ computed_angle,
+ msg=f"Trying wrap {wrap} theta {theta}")
def test_wheel_torque(self):
"""Tests that the per module self aligning forces have the right signs."""
- X = state_vector(module_angles=[-0.001, -0.001, 0.001, 0.001])
+ # Point all the modules in a little bit.
+ X = state_vector(
+ velocity=numpy.array([[1.0], [0.0]]),
+ module_angles=[-0.001, -0.001, 0.001, 0.001],
+ )
xdot_equal = self.swerve_physics(X, self.I)
self.assertGreater(xdot_equal[2, 0], 0.0)
@@ -222,54 +232,248 @@
# Shouldn't be spinning.
self.assertAlmostEqual(xdot_equal[21, 0], 0.0, places=2)
- # Now, make the bot want to go left.
+ # Now, make the bot want to go left by going to the other side.
# The wheels will be going too fast based on our calcs, so they should be decelerating.
- X = state_vector(module_angles=[0.01, 0.01, 0.01, 0.01])
+ X = state_vector(
+ velocity=numpy.array([[1.0], [0.0]]),
+ module_angles=[0.01, 0.01, 0.01, 0.01],
+ )
xdot_left = self.swerve_physics(X, self.I)
- self.assertLess(xdot_left[2, 0], -0.1)
+ self.assertLess(xdot_left[2, 0], -0.05)
self.assertLess(xdot_left[3, 0], 0.0)
self.assertLess(self.Ms[0](X, self.I)[0, 0], 0.0)
- self.assertLess(xdot_left[6, 0], -0.1)
+ self.assertLess(xdot_left[6, 0], -0.05)
self.assertLess(xdot_left[7, 0], 0.0)
self.assertLess(self.Ms[1](X, self.I)[0, 0], 0.0)
- self.assertLess(xdot_left[10, 0], -0.1)
+ self.assertLess(xdot_left[10, 0], -0.05)
self.assertLess(xdot_left[11, 0], 0.0)
self.assertLess(self.Ms[2](X, self.I)[0, 0], 0.0)
- self.assertLess(xdot_left[14, 0], -0.1)
+ self.assertLess(xdot_left[14, 0], -0.05)
self.assertLess(xdot_left[15, 0], 0.0)
self.assertLess(self.Ms[3](X, self.I)[0, 0], 0.0)
self.assertGreater(xdot_left[19, 0], 0.0001)
- self.assertGreater(xdot_left[20, 0], 0.1)
+ self.assertGreater(xdot_left[20, 0], 0.05)
# Shouldn't be spinning.
self.assertAlmostEqual(xdot_left[21, 0], 0.0)
# And now do it to the right too.
- X = state_vector(module_angles=[-0.01, -0.01, -0.01, -0.01])
+ X = state_vector(
+ velocity=numpy.array([[1.0], [0.0]]),
+ module_angles=[-0.01, -0.01, -0.01, -0.01],
+ )
xdot_right = self.swerve_physics(X, self.I)
- self.assertGreater(xdot_right[2, 0], 0.1)
+ self.assertGreater(xdot_right[2, 0], 0.05)
self.assertLess(xdot_right[3, 0], 0.0)
self.assertGreater(self.Ms[0](X, self.I)[0, 0], 0.0)
- self.assertGreater(xdot_right[6, 0], 0.1)
+ self.assertGreater(xdot_right[6, 0], 0.05)
self.assertLess(xdot_right[7, 0], 0.0)
self.assertGreater(self.Ms[1](X, self.I)[0, 0], 0.0)
- self.assertGreater(xdot_right[10, 0], 0.1)
+ self.assertGreater(xdot_right[10, 0], 0.05)
self.assertLess(xdot_right[11, 0], 0.0)
self.assertGreater(self.Ms[2](X, self.I)[0, 0], 0.0)
- self.assertGreater(xdot_right[14, 0], 0.1)
+ self.assertGreater(xdot_right[14, 0], 0.05)
self.assertLess(xdot_right[15, 0], 0.0)
self.assertGreater(self.Ms[3](X, self.I)[0, 0], 0.0)
self.assertGreater(xdot_right[19, 0], 0.0001)
- self.assertLess(xdot_right[20, 0], -0.1)
+ self.assertLess(xdot_right[20, 0], -0.05)
+ # Shouldn't be spinning.
+ self.assertAlmostEqual(xdot_right[21, 0], 0.0)
+
+ def test_wheel_torque_backwards_nocaster(self):
+ """Tests that the per module self aligning forces have the right signs when going backwards."""
+ self.wrap(nocaster_dynamics)
+ # Point all the modules in a little bit, going backwards.
+ X = state_vector(
+ velocity=numpy.array([[1.0], [0.0]]),
+ module_angles=[
+ numpy.pi - 0.001,
+ numpy.pi - 0.001,
+ numpy.pi + 0.001,
+ numpy.pi + 0.001,
+ ],
+ drive_wheel_velocity=-1,
+ )
+ xdot_equal = self.swerve_physics(X, self.I)
+
+ self.assertGreater(xdot_equal[2, 0], 0.0, msg="Steering backwards")
+ self.assertAlmostEqual(xdot_equal[3, 0], 0.0, places=1)
+ self.assertGreater(self.Ms[0](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_equal[6, 0], 0.0, msg="Steering backwards")
+ self.assertAlmostEqual(xdot_equal[7, 0], 0.0, places=1)
+ self.assertGreater(self.Ms[1](X, self.I)[0, 0], 0.0)
+
+ self.assertLess(xdot_equal[10, 0], 0.0, msg="Steering backwards")
+ self.assertAlmostEqual(xdot_equal[11, 0], 0.0, places=1)
+ self.assertLess(self.Ms[2](X, self.I)[0, 0], 0.0)
+
+ self.assertLess(xdot_equal[14, 0], 0.0, msg="Steering backwards")
+ self.assertAlmostEqual(xdot_equal[15, 0], 0.0, places=1)
+ self.assertLess(self.Ms[3](X, self.I)[0, 0], 0.0)
+
+ # Shouldn't be spinning.
+ self.assertAlmostEqual(xdot_equal[21, 0], 0.0, places=2)
+
+ # Now, make the bot want to go left by going to the other side.
+ # The wheels will be going too fast based on our calcs, so they should be decelerating.
+ X = state_vector(
+ velocity=numpy.array([[1.0], [0.0]]),
+ module_angles=[numpy.pi + 0.01] * 4,
+ drive_wheel_velocity=-1,
+ )
+ xdot_left = self.swerve_physics(X, self.I)
+
+ self.assertLess(xdot_left[2, 0], -0.05)
+ self.assertGreater(xdot_left[3, 0], 0.0)
+ self.assertLess(self.Ms[0](X, self.I)[0, 0], 0.0)
+
+ self.assertLess(xdot_left[6, 0], -0.05)
+ self.assertGreater(xdot_left[7, 0], 0.0)
+ self.assertLess(self.Ms[1](X, self.I)[0, 0], 0.0)
+
+ self.assertLess(xdot_left[10, 0], -0.05)
+ self.assertGreater(xdot_left[11, 0], 0.0)
+ self.assertLess(self.Ms[2](X, self.I)[0, 0], 0.0)
+
+ self.assertLess(xdot_left[14, 0], -0.05)
+ self.assertGreater(xdot_left[15, 0], 0.0)
+ self.assertLess(self.Ms[3](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_left[19, 0], 0.0001)
+ self.assertGreater(xdot_left[20, 0], 0.05)
+ # Shouldn't be spinning.
+ self.assertAlmostEqual(xdot_left[21, 0], 0.0)
+
+ # And now do it to the right too.
+ X = state_vector(
+ velocity=numpy.array([[1.0], [0.0]]),
+ drive_wheel_velocity=-1,
+ module_angles=[-0.01 + numpy.pi] * 4,
+ )
+ xdot_right = self.swerve_physics(X, self.I)
+
+ self.assertGreater(xdot_right[2, 0], 0.05)
+ self.assertGreater(xdot_right[3, 0], 0.0)
+ self.assertGreater(self.Ms[0](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_right[6, 0], 0.05)
+ self.assertGreater(xdot_right[7, 0], 0.0)
+ self.assertGreater(self.Ms[1](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_right[10, 0], 0.05)
+ self.assertGreater(xdot_right[11, 0], 0.0)
+ self.assertGreater(self.Ms[2](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_right[14, 0], 0.05)
+ self.assertGreater(xdot_right[15, 0], 0.0)
+ self.assertGreater(self.Ms[3](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_right[19, 0], 0.0001)
+ self.assertLess(xdot_right[20, 0], -0.05)
+ # Shouldn't be spinning.
+ self.assertAlmostEqual(xdot_right[21, 0], 0.0)
+
+ def test_wheel_torque_backwards_caster(self):
+ """Tests that the per module self aligning forces have the right signs when going backwards with a lot of caster."""
+ self.wrap(bigcaster_dynamics)
+ # Point all the modules in a little bit, going backwards.
+ X = state_vector(
+ velocity=numpy.array([[1.0], [0.0]]),
+ module_angles=[
+ numpy.pi - 0.001,
+ numpy.pi - 0.001,
+ numpy.pi + 0.001,
+ numpy.pi + 0.001,
+ ],
+ drive_wheel_velocity=-1,
+ )
+ xdot_equal = self.swerve_physics(X, self.I)
+
+ self.assertLess(xdot_equal[2, 0], 0.0, msg="Steering backwards")
+ self.assertAlmostEqual(xdot_equal[3, 0], 0.0, places=1)
+ self.assertLess(self.Ms[0](X, self.I)[0, 0], 0.0)
+
+ self.assertLess(xdot_equal[6, 0], 0.0, msg="Steering backwards")
+ self.assertAlmostEqual(xdot_equal[7, 0], 0.0, places=1)
+ self.assertLess(self.Ms[1](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_equal[10, 0], 0.0, msg="Steering backwards")
+ self.assertAlmostEqual(xdot_equal[11, 0], 0.0, places=1)
+ self.assertGreater(self.Ms[2](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_equal[14, 0], 0.0, msg="Steering backwards")
+ self.assertAlmostEqual(xdot_equal[15, 0], 0.0, places=1)
+ self.assertGreater(self.Ms[3](X, self.I)[0, 0], 0.0)
+
+ # Shouldn't be spinning.
+ self.assertAlmostEqual(xdot_equal[21, 0], 0.0, places=2)
+
+ # Now, make the bot want to go left by going to the other side.
+ # The wheels will be going too fast based on our calcs, so they should be decelerating.
+ X = state_vector(
+ velocity=numpy.array([[1.0], [0.0]]),
+ module_angles=[numpy.pi + 0.01] * 4,
+ drive_wheel_velocity=-1,
+ )
+ xdot_left = self.swerve_physics(X, self.I)
+
+ self.assertGreater(xdot_left[2, 0], -0.05)
+ self.assertGreater(xdot_left[3, 0], 0.0)
+ self.assertGreater(self.Ms[0](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_left[6, 0], -0.05)
+ self.assertGreater(xdot_left[7, 0], 0.0)
+ self.assertGreater(self.Ms[1](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_left[10, 0], -0.05)
+ self.assertGreater(xdot_left[11, 0], 0.0)
+ self.assertGreater(self.Ms[2](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_left[14, 0], -0.05)
+ self.assertGreater(xdot_left[15, 0], 0.0)
+ self.assertGreater(self.Ms[3](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_left[19, 0], 0.0001)
+ self.assertGreater(xdot_left[20, 0], 0.05)
+ # Shouldn't be spinning.
+ self.assertAlmostEqual(xdot_left[21, 0], 0.0)
+
+ # And now do it to the right too.
+ X = state_vector(
+ velocity=numpy.array([[1.0], [0.0]]),
+ drive_wheel_velocity=-1,
+ module_angles=[-0.01 + numpy.pi] * 4,
+ )
+ xdot_right = self.swerve_physics(X, self.I)
+
+ self.assertLess(xdot_right[2, 0], 0.05)
+ self.assertGreater(xdot_right[3, 0], 0.0)
+ self.assertLess(self.Ms[0](X, self.I)[0, 0], 0.0)
+
+ self.assertLess(xdot_right[6, 0], 0.05)
+ self.assertGreater(xdot_right[7, 0], 0.0)
+ self.assertLess(self.Ms[1](X, self.I)[0, 0], 0.0)
+
+ self.assertLess(xdot_right[10, 0], 0.05)
+ self.assertGreater(xdot_right[11, 0], 0.0)
+ self.assertLess(self.Ms[2](X, self.I)[0, 0], 0.0)
+
+ self.assertLess(xdot_right[14, 0], 0.05)
+ self.assertGreater(xdot_right[15, 0], 0.0)
+ self.assertLess(self.Ms[3](X, self.I)[0, 0], 0.0)
+
+ self.assertGreater(xdot_right[19, 0], 0.0001)
+ self.assertLess(xdot_right[20, 0], -0.05)
# Shouldn't be spinning.
self.assertAlmostEqual(xdot_right[21, 0], 0.0)