Add unit tests for physics for swerve
We can put the robot in a bunch of well defined situtations and confirm
the physics behaves reasonably.
Change-Id: I085986b6a5abc2053698a661d9f79f6841bb1ed7
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
index 09b363a..ed2c604 100644
--- a/frc971/control_loops/swerve/BUILD
+++ b/frc971/control_loops/swerve/BUILD
@@ -165,3 +165,16 @@
"@pip//scipy",
],
)
+
+py_test(
+ name = "physics_test",
+ srcs = [
+ "dynamics.py",
+ "physics_test.py",
+ ],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ "@pip//casadi",
+ "@pip//numpy",
+ ],
+)
diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index 1761308..9f0f5b7 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -25,8 +25,8 @@
DEFINE_string(output_base, "",
"Path to strip off the front of the output paths.");
-DEFINE_string(cc_output_path, "", "Path to write generated header code to");
-DEFINE_string(h_output_path, "", "Path to write generated cc code to");
+DEFINE_string(cc_output_path, "", "Path to write generated cc code to");
+DEFINE_string(h_output_path, "", "Path to write generated header code to");
DEFINE_string(py_output_path, "", "Path to write generated py code to");
DEFINE_string(casadi_py_output_path, "",
"Path to write casadi generated py code to");
@@ -76,6 +76,7 @@
RCP<const Symbol> alphad;
RCP<const Basic> alphad_eqn;
+ DenseMatrix contact_patch_velocity;
DenseMatrix wheel_ground_velocity;
RCP<const Basic> slip_angle;
RCP<const Basic> slip_ratio;
@@ -99,7 +100,7 @@
Cx_ = symbol("Cx");
Cy_ = symbol("Cy");
- r_w_ = symbol("r_w_");
+ rw_ = symbol("rw");
m_ = symbol("m");
J_ = symbol("J");
@@ -133,7 +134,7 @@
Cx_ = real_double(5 * 9.8 / 0.05 / 4.0);
Cy_ = real_double(5 * 9.8 / 0.05 / 4.0);
- r_w_ = real_double(2 * 0.0254);
+ rw_ = real_double(2 * 0.0254);
m_ = real_double(25.0); // base is 20 kg without battery
J_ = real_double(6.0);
@@ -462,7 +463,8 @@
void WriteCasadiVariables(std::vector<std::string> *result_py) {
result_py->emplace_back(" sin = casadi.sin");
result_py->emplace_back(" cos = casadi.cos");
- result_py->emplace_back(" atan2 = casadi.atan2");
+ result_py->emplace_back(" atan2 = half_atan2");
+ result_py->emplace_back(" fmax = casadi.fmax");
result_py->emplace_back(" fabs = casadi.fabs");
// Start by writing out variables matching each of the symbol names we use
@@ -510,7 +512,17 @@
result_py.emplace_back("");
result_py.emplace_back("import casadi");
result_py.emplace_back("");
- result_py.emplace_back("# Returns the derivative of our state vector");
+ result_py.emplace_back(absl::Substitute("WHEEL_RADIUS = $0", ccode(*rw_)));
+ result_py.emplace_back(
+ 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,");
result_py.emplace_back("# thetas1, thetad1, omegas1, omegad1,");
@@ -567,6 +579,32 @@
result_py.emplace_back("");
result_py.emplace_back(
"# Returns the velocity of the wheel in steer module coordinates.");
+ result_py.emplace_back("def contact_patch_velocity(i, X, U):");
+ WriteCasadiVariables(&result_py);
+ result_py.emplace_back(
+ " result = casadi.SX.sym('contact_patch_velocity', 2, 1)");
+
+ for (size_t m = 0; m < kNumModules; ++m) {
+ if (m == 0) {
+ result_py.emplace_back(" if i == 0:");
+ } else {
+ result_py.emplace_back(absl::Substitute(" elif i == $0:", m));
+ }
+ for (int j = 0; j < 2; ++j) {
+ result_py.emplace_back(absl::Substitute(
+ " result[$0, 0] = $1", j,
+ ccode(*modules_[m].contact_patch_velocity.get(j, 0))));
+ }
+ }
+ result_py.emplace_back(" else:");
+ result_py.emplace_back(
+ " raise ValueError(\"Invalid module number\")");
+ result_py.emplace_back(
+ " return casadi.Function('contact_patch_velocity', [X, U], "
+ "[result])");
+ result_py.emplace_back("");
+ result_py.emplace_back(
+ "# Returns the velocity of the wheel in steer module coordinates.");
result_py.emplace_back("def wheel_ground_velocity(i, X, U):");
WriteCasadiVariables(&result_py);
result_py.emplace_back(
@@ -581,13 +619,15 @@
for (int j = 0; j < 2; ++j) {
result_py.emplace_back(absl::Substitute(
" result[$0, 0] = $1", j,
- ccode(*modules_[m].wheel_ground_velocity.get(0, 0))));
+ ccode(*modules_[m].wheel_ground_velocity.get(j, 0))));
}
}
result_py.emplace_back(" else:");
result_py.emplace_back(
" raise ValueError(\"Invalid module number\")");
- result_py.emplace_back(" return result");
+ result_py.emplace_back(
+ " return casadi.Function('wheel_ground_velocity', [X, U], "
+ "[result])");
result_py.emplace_back("");
result_py.emplace_back("# Returns the slip angle of the ith wheel.");
@@ -599,8 +639,9 @@
} else {
result_py.emplace_back(absl::Substitute(" elif i == $0:", m));
}
- result_py.emplace_back(absl::Substitute(" return $0",
- ccode(*modules_[m].slip_angle)));
+ result_py.emplace_back(absl::Substitute(
+ " return casadi.Function('slip_angle', [X, U], [$0])",
+ ccode(*modules_[m].slip_angle)));
}
result_py.emplace_back(" raise ValueError(\"Invalid module number\")");
result_py.emplace_back("");
@@ -613,8 +654,9 @@
} else {
result_py.emplace_back(absl::Substitute(" elif i == $0:", m));
}
- result_py.emplace_back(absl::Substitute(" return $0",
- ccode(*modules_[m].slip_ratio)));
+ result_py.emplace_back(absl::Substitute(
+ " return casadi.Function('slip_ratio', [X, U], [$0])",
+ ccode(*modules_[m].slip_ratio)));
}
result_py.emplace_back(" raise ValueError(\"Invalid module number\")");
@@ -639,7 +681,8 @@
result_py.emplace_back(" else:");
result_py.emplace_back(
" raise ValueError(\"Invalid module number\")");
- result_py.emplace_back(" return result");
+ result_py.emplace_back(
+ " return casadi.Function('wheel_force', [X, U], [result])");
aos::util::WriteStringToFileOrDie(py_path, absl::StrJoin(result_py, "\n"));
}
@@ -668,14 +711,13 @@
result.alphad = symbol(absl::StrFormat("alphad%u", m));
// Velocity of the module in field coordinates
- DenseMatrix robot_velocity = DenseMatrix(2, 1);
- mul_dense_dense(R(theta_), DenseMatrix(2, 1, {vx_, vy_}), robot_velocity);
+ DenseMatrix robot_velocity = DenseMatrix(2, 1, {vx_, vy_});
VLOG(1) << "robot velocity: " << robot_velocity.__str__();
// Velocity of the contact patch in field coordinates
DenseMatrix temp_matrix = DenseMatrix(2, 1);
DenseMatrix temp_matrix2 = DenseMatrix(2, 1);
- DenseMatrix contact_patch_velocity = DenseMatrix(2, 1);
+ result.contact_patch_velocity = DenseMatrix(2, 1);
mul_dense_dense(R(theta_), mounting_location, temp_matrix);
add_dense_dense(angle_cross(temp_matrix, omega_), robot_velocity,
@@ -684,14 +726,16 @@
DenseMatrix(2, 1, {caster_, integer(0)}), temp_matrix);
add_dense_dense(temp_matrix2,
angle_cross(temp_matrix, add(omega_, result.omegas)),
- contact_patch_velocity);
+ result.contact_patch_velocity);
VLOG(1);
- VLOG(1) << "contact patch velocity: " << contact_patch_velocity.__str__();
+ VLOG(1) << "contact patch velocity: "
+ << result.contact_patch_velocity.__str__();
// Relative velocity of the surface of the wheel to the ground.
result.wheel_ground_velocity = DenseMatrix(2, 1);
- mul_dense_dense(R(neg(add(result.thetas, theta_))), contact_patch_velocity,
+ mul_dense_dense(R(neg(add(result.thetas, theta_))),
+ result.contact_patch_velocity,
result.wheel_ground_velocity);
VLOG(1);
@@ -704,9 +748,11 @@
VLOG(1);
VLOG(1) << "slip angle: " << result.slip_angle->__str__();
+ // TODO(austin): Does this handle decel properly?
result.slip_ratio = div(
- sub(mul(r_w_, result.omegad), result.wheel_ground_velocity.get(0, 0)),
- abs(result.wheel_ground_velocity.get(0, 0)));
+ sub(mul(rw_, result.omegad), result.wheel_ground_velocity.get(0, 0)),
+ SymEngine::max(
+ {real_double(0.02), abs(result.wheel_ground_velocity.get(0, 0))}));
VLOG(1);
VLOG(1) << "Slip ratio " << result.slip_ratio->__str__();
@@ -725,7 +771,7 @@
// alphas = ...
RCP<const Basic> lhms =
mul(add(neg(wb_), mul(add(rs_, rp_), sub(integer(1), div(rb1_, rp_)))),
- mul(div(r_w_, rb2_), neg(result.Fwx)));
+ mul(div(rw_, rb2_), neg(result.Fwx)));
RCP<const Basic> lhs = add(add(Ms, div(mul(Jsm_, result.Is), Gs_)), lhms);
RCP<const Basic> rhs = add(Jsm_, div(div(Js_, Gs_), Gs_));
RCP<const Basic> accel_steer_eqn = simplify(div(lhs, rhs));
@@ -743,7 +789,7 @@
RCP<const Basic> drive_eqn = sub(
add(mul(neg(Jdm_), div(alphamd, Gd_)), mul(Ktd_, div(result.Id, Gd_))),
- mul(neg(result.Fwx), r_w_));
+ mul(neg(result.Fwx), rw_));
VLOG(1) << "drive_eqn: " << drive_eqn->__str__();
@@ -771,7 +817,7 @@
mul_dense_dense(R(add(theta_, result.thetas)),
DenseMatrix(2, 1, {result.Fwx, result.Fwy}), mat_output);
- // Comput the resulting force from the module.
+ // Compute the resulting force from the module.
DenseMatrix F = mat_output;
RCP<const Basic> torque = simplify(force_cross(mounting_location, F));
@@ -797,7 +843,7 @@
}
DenseMatrix angle_cross(DenseMatrix a, RCP<const Basic> b) {
- return DenseMatrix(2, 1, {mul(a.get(1, 0), b), mul(neg(a.get(0, 0)), b)});
+ return DenseMatrix(2, 1, {mul(neg(a.get(1, 0)), b), mul(a.get(0, 0), b)});
}
RCP<const Basic> force_cross(DenseMatrix r, DenseMatrix f) {
@@ -822,7 +868,7 @@
RCP<const Basic> Cx_;
RCP<const Basic> Cy_;
- RCP<const Basic> r_w_;
+ RCP<const Basic> rw_;
RCP<const Basic> m_;
RCP<const Basic> J_;
RCP<const Basic> Gd1_;
diff --git a/frc971/control_loops/swerve/physics_test.py b/frc971/control_loops/swerve/physics_test.py
new file mode 100644
index 0000000..7ea7123
--- /dev/null
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -0,0 +1,194 @@
+#!/usr/bin/python3
+
+import numpy
+import sys, os
+import casadi
+from numpy.testing import assert_array_equal, assert_array_almost_equal
+import unittest
+
+from frc971.control_loops.swerve import dynamics
+
+
+def state_vector(velocity=numpy.array([[1.0], [0.0]]),
+ dx=0.0,
+ dy=0.0,
+ theta=0.0,
+ omega=0.0,
+ module_omega=0.0,
+ module_angle=0.0):
+ """Returns the state vector with the requested state."""
+ X_initial = numpy.zeros((25, 1))
+ # All the wheels are spinning at the speed needed to hit the velocity in m/s
+ X_initial[2, 0] = module_omega
+ X_initial[3, 0] = numpy.linalg.norm(velocity) / (dynamics.WHEEL_RADIUS)
+
+ X_initial[6, 0] = module_omega
+ X_initial[7, 0] = numpy.linalg.norm(velocity) / (dynamics.WHEEL_RADIUS)
+
+ X_initial[10, 0] = module_omega
+ X_initial[11, 0] = numpy.linalg.norm(velocity) / (dynamics.WHEEL_RADIUS)
+
+ X_initial[14, 0] = module_omega
+ X_initial[15, 0] = numpy.linalg.norm(velocity) / (dynamics.WHEEL_RADIUS)
+
+ X_initial[0, 0] = module_angle
+ X_initial[4, 0] = module_angle
+ X_initial[8, 0] = module_angle
+ X_initial[12, 0] = module_angle
+
+ X_initial[18, 0] = theta
+
+ X_initial[19, 0] = velocity[0, 0] + dx
+ X_initial[20, 0] = velocity[1, 0] + dy
+ X_initial[21, 0] = omega
+
+ return X_initial
+
+
+class TestHPolytope(unittest.TestCase):
+ I = numpy.zeros((8, 1))
+
+ def setUp(self):
+ pass
+
+ def test_contact_patch_velocity(self):
+ """Tests that the contact patch velocity makes sense."""
+ for i in range(4):
+ contact_patch_velocity = dynamics.contact_patch_velocity(
+ i, casadi.SX.sym("X", 25, 1), casadi.SX.sym("U", 8, 1))
+ wheel_ground_velocity = dynamics.wheel_ground_velocity(
+ i, casadi.SX.sym("X", 25, 1), casadi.SX.sym("U", 8, 1))
+
+ # No angular velocity should result in just linear motion.
+ for velocity in [
+ numpy.array([[1.5], [0.0]]),
+ numpy.array([[0.0], [1.0]]),
+ numpy.array([[-1.5], [0.0]]),
+ numpy.array([[0.0], [-1.0]]),
+ numpy.array([[2.0], [-1.7]]),
+ ]:
+ for theta in [0.0, 1.0, -1.0, 100.0]:
+ patch_velocity = numpy.array(
+ contact_patch_velocity(
+ state_vector(velocity=velocity, theta=theta),
+ self.I))
+
+ assert_array_equal(patch_velocity, velocity)
+
+ # Now, test that spinning the robot results in module velocities.
+ # We are assuming that each module is on a square robot.
+ module_center_of_mass_angle = i * numpy.pi / 2.0 + numpy.pi / 4.0
+ for theta in [-module_center_of_mass_angle, 0.0, 1.0, -1.0, 100.0]:
+ for omega in [0.65, -0.1]:
+ # Point each module to the center to make the math easier.
+ patch_velocity = numpy.array(
+ contact_patch_velocity(
+ state_vector(
+ velocity=numpy.array([[0.0], [0.0]]),
+ theta=theta,
+ omega=omega,
+ module_angle=module_center_of_mass_angle),
+ self.I))
+
+ assert_array_almost_equal(
+ patch_velocity,
+ (dynamics.ROBOT_WIDTH / numpy.sqrt(2.0) +
+ dynamics.CASTER) * omega * numpy.array([[
+ -numpy.sin(theta + module_center_of_mass_angle)
+ ], [numpy.cos(theta + module_center_of_mass_angle)]]))
+
+ # Point the wheel along +x, rotate it by theta, then spin it.
+ # Confirm the velocities come out right.
+ patch_velocity = numpy.array(
+ contact_patch_velocity(
+ state_vector(
+ velocity=numpy.array([[0.0], [0.0]]),
+ theta=-module_center_of_mass_angle,
+ module_omega=omega,
+ module_angle=(theta +
+ module_center_of_mass_angle)),
+ self.I))
+
+ assert_array_almost_equal(
+ patch_velocity,
+ dynamics.CASTER * omega *
+ numpy.array([[-numpy.sin(theta)], [numpy.cos(theta)]]))
+
+ # Now, test that the rotation back to wheel coordinates works.
+ # The easiest way to do this is to point the wheel in a direction,
+ # move in that direction, and confirm that there is no lateral velocity.
+ for robot_angle in [0.0, 1.0, -5.0]:
+ for module_angle in [0.0, 1.0, -5.0]:
+ wheel_patch_velocity = numpy.array(
+ wheel_ground_velocity(
+ state_vector(velocity=numpy.array(
+ [[numpy.cos(robot_angle + module_angle)],
+ [numpy.sin(robot_angle + module_angle)]]),
+ theta=robot_angle,
+ module_angle=module_angle), self.I))
+
+ assert_array_almost_equal(wheel_patch_velocity,
+ numpy.array([[1], [0]]))
+
+ def test_slip_angle(self):
+ """Tests that the slip_angle calculation works."""
+ 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)])
+
+ slip_angle = dynamics.slip_angle(i, casadi.SX.sym("X", 25, 1),
+ casadi.SX.sym("U", 8, 1))
+
+ for wrap in range(-3, 3):
+ 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)))
+
+ computed_angle = slip_angle(
+ state_vector(velocity=velocity,
+ module_angle=numpy.pi * wrap + theta),
+ self.I)
+
+ self.assertAlmostEqual(theta, computed_angle)
+
+ def test_wheel_forces(self):
+ """Tests that the per module forces have the right signs."""
+ for i in range(4):
+ wheel_force = dynamics.wheel_force(i, casadi.SX.sym("X", 25, 1),
+ casadi.SX.sym("U", 8, 1))
+
+ # Robot is moving faster than the wheels, it should decelerate.
+ robot_faster = numpy.array(
+ wheel_force(state_vector(dx=0.01), self.I))
+ self.assertLess(robot_faster[0, 0], -0.1)
+ self.assertEqual(robot_faster[1, 0], 0.0)
+
+ # Robot is now going slower than the wheels. It should accelerate.
+ robot_slower = numpy.array(
+ wheel_force(state_vector(dx=-0.01), self.I))
+ self.assertGreater(robot_slower[0, 0], 0.1)
+ self.assertEqual(robot_slower[1, 0], 0.0)
+
+ # Positive lateral velocity -> negative force.
+ robot_left = numpy.array(wheel_force(state_vector(dy=0.01),
+ self.I))
+ self.assertEqual(robot_left[0, 0], 0.0)
+ self.assertLess(robot_left[1, 0], -0.1)
+
+ # Negative lateral velocity -> positive force.
+ robot_right = numpy.array(
+ wheel_force(state_vector(dy=-0.01), self.I))
+ self.assertEqual(robot_right[0, 0], 0.0)
+ self.assertGreater(robot_right[1, 0], 0.1)
+
+
+if __name__ == '__main__':
+ unittest.main()