Fix signs on wheels pointed backwards
When we go backwards, we want to flip the sign of the lateral force, and
want to also flip the direction of the contact patch piece of the self
aligning moment.
The easiest way to do this is to take the sin of the angle, rather than
doing a piecewise linear function of the angle to flip the sign.
Change-Id: I7b348a7c0b9449c28c01957aea7d4a16a52b863b
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
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..bb5a51a 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(
@@ -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 = sin_atan2");
result_py->emplace_back(" fmax = casadi.fmax");
result_py->emplace_back(" fabs = casadi.fabs");
@@ -525,10 +536,8 @@
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("def sin_atan2(y, x):");
+ result_py.emplace_back(" return casadi.sin(casadi.atan2(y, x))");
result_py.emplace_back("");
result_py.emplace_back("# Returns the derivative of our state vector");
@@ -787,8 +796,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..0a40b22 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."""
@@ -180,27 +188,39 @@
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)])
+ sin_atan2 = casadi.Function('sin_atan2', [y, x],
+ [dynamics.sin_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)))
+ numpy.sin(module_angle),
+ sin_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 +242,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)