Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / b65288d645c4516c2d2edda0818140a90e1c910b / . / frc971 / control_loops / swerve / physics_test.py
blob: ecfc1ce288d2777b9805ff0837af87f3b892d623 [file] [log] [blame]
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]1#!/usr/bin/python3
2
3import numpy
4import sys, os
5import casadi
6from numpy.testing import assert_array_equal, assert_array_almost_equal
7import unittest
8
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]9from frc971.control_loops.swerve import bigcaster_dynamics
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]10from frc971.control_loops.swerve import dynamics
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]11from frc971.control_loops.swerve import nocaster_dynamics
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]12
13
14def state_vector(velocity=numpy.array([[1.0], [0.0]]),
15 dx=0.0,
16 dy=0.0,
17 theta=0.0,
18 omega=0.0,
19 module_omega=0.0,
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]20 module_angle=0.0,
21 drive_wheel_velocity=None,
22 module_angles=None):
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]23 """Returns the state vector with the requested state."""
24 X_initial = numpy.zeros((25, 1))
25 # All the wheels are spinning at the speed needed to hit the velocity in m/s
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]26 drive_wheel_velocity = (drive_wheel_velocity
27 or numpy.linalg.norm(velocity))
28
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]29 X_initial[2, 0] = module_omega
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]30 X_initial[3, 0] = drive_wheel_velocity / (dynamics.WHEEL_RADIUS)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]31
32 X_initial[6, 0] = module_omega
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]33 X_initial[7, 0] = drive_wheel_velocity / (dynamics.WHEEL_RADIUS)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]34
35 X_initial[10, 0] = module_omega
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]36 X_initial[11, 0] = drive_wheel_velocity / (dynamics.WHEEL_RADIUS)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]37
38 X_initial[14, 0] = module_omega
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]39 X_initial[15, 0] = drive_wheel_velocity / (dynamics.WHEEL_RADIUS)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]40
41 X_initial[0, 0] = module_angle
42 X_initial[4, 0] = module_angle
43 X_initial[8, 0] = module_angle
44 X_initial[12, 0] = module_angle
45
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]46 if module_angles is not None:
47 assert (len(module_angles) == 4)
48 X_initial[0, 0] = module_angles[0]
49 X_initial[4, 0] = module_angles[1]
50 X_initial[8, 0] = module_angles[2]
51 X_initial[12, 0] = module_angles[3]
52
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]53 X_initial[18, 0] = theta
54
55 X_initial[19, 0] = velocity[0, 0] + dx
56 X_initial[20, 0] = velocity[1, 0] + dy
57 X_initial[21, 0] = omega
58
59 return X_initial
60
61
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]62def wrap(fn):
63 evaluated_fn = fn(casadi.SX.sym("X", 25, 1), casadi.SX.sym("U", 8, 1))
64 return lambda X, U: numpy.array(evaluated_fn(X, U))
65
66
67def wrap_module(fn, i):
68 evaluated_fn = fn(i, casadi.SX.sym("X", 25, 1), casadi.SX.sym("U", 8, 1))
69 return lambda X, U: numpy.array(evaluated_fn(X, U))
70
71
Austin Schuh6927bc32024-07-14 17:24:56 -0700[diff] [blame]72class TestSwervePhysics(unittest.TestCase):
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]73 I = numpy.zeros((8, 1))
74
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]75 def wrap(self, python_module):
76 self.swerve_physics = wrap(python_module.swerve_physics)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]77 self.contact_patch_velocity = [
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]78 wrap_module(python_module.contact_patch_velocity, i)
79 for i in range(4)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]80 ]
81 self.wheel_ground_velocity = [
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]82 wrap_module(python_module.wheel_ground_velocity, i)
83 for i in range(4)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]84 ]
85 self.wheel_slip_velocity = [
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]86 wrap_module(python_module.wheel_slip_velocity, i) for i in range(4)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]87 ]
88 self.wheel_force = [
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]89 wrap_module(python_module.wheel_force, i) for i in range(4)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]90 ]
91 self.module_angular_accel = [
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]92 wrap_module(python_module.module_angular_accel, i)
93 for i in range(4)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]94 ]
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]95 self.F = [wrap_module(python_module.F, i) for i in range(4)]
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]96 self.mounting_location = [
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]97 wrap_module(python_module.mounting_location, i) for i in range(4)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]98 ]
99
100 self.slip_angle = [
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]101 wrap_module(python_module.slip_angle, i) for i in range(4)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]102 ]
103 self.slip_ratio = [
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]104 wrap_module(python_module.slip_ratio, i) for i in range(4)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]105 ]
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]106 self.Ms = [wrap_module(python_module.Ms, i) for i in range(4)]
107
108 def setUp(self):
109 self.wrap(dynamics)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]110
111 def test_contact_patch_velocity(self):
112 """Tests that the contact patch velocity makes sense."""
113 for i in range(4):
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]114 contact_patch_velocity = wrap_module(
115 dynamics.contact_patch_velocity, i)
116 wheel_ground_velocity = wrap_module(dynamics.wheel_ground_velocity,
117 i)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]118
119 # No angular velocity should result in just linear motion.
120 for velocity in [
121 numpy.array([[1.5], [0.0]]),
122 numpy.array([[0.0], [1.0]]),
123 numpy.array([[-1.5], [0.0]]),
124 numpy.array([[0.0], [-1.0]]),
125 numpy.array([[2.0], [-1.7]]),
126 ]:
127 for theta in [0.0, 1.0, -1.0, 100.0]:
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]128 patch_velocity = contact_patch_velocity(
129 state_vector(velocity=velocity, theta=theta), self.I)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]130
131 assert_array_equal(patch_velocity, velocity)
132
133 # Now, test that spinning the robot results in module velocities.
134 # We are assuming that each module is on a square robot.
135 module_center_of_mass_angle = i * numpy.pi / 2.0 + numpy.pi / 4.0
136 for theta in [-module_center_of_mass_angle, 0.0, 1.0, -1.0, 100.0]:
137 for omega in [0.65, -0.1]:
138 # Point each module to the center to make the math easier.
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]139 patch_velocity = contact_patch_velocity(
140 state_vector(velocity=numpy.array([[0.0], [0.0]]),
141 theta=theta,
142 omega=omega,
143 module_angle=module_center_of_mass_angle),
144 self.I)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]145
146 assert_array_almost_equal(
147 patch_velocity,
Austin Schuh6927bc32024-07-14 17:24:56 -0700[diff] [blame]148 (dynamics.ROBOT_WIDTH / numpy.sqrt(2.0) -
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]149 dynamics.CASTER) * omega * numpy.array([[
150 -numpy.sin(theta + module_center_of_mass_angle)
151 ], [numpy.cos(theta + module_center_of_mass_angle)]]))
152
153 # Point the wheel along +x, rotate it by theta, then spin it.
154 # Confirm the velocities come out right.
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]155 patch_velocity = contact_patch_velocity(
156 state_vector(
157 velocity=numpy.array([[0.0], [0.0]]),
158 theta=-module_center_of_mass_angle,
159 module_omega=omega,
160 module_angle=(theta +
161 module_center_of_mass_angle)),
162 self.I)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]163
164 assert_array_almost_equal(
Austin Schuh6927bc32024-07-14 17:24:56 -0700[diff] [blame]165 patch_velocity, -dynamics.CASTER * omega *
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]166 numpy.array([[-numpy.sin(theta)], [numpy.cos(theta)]]))
167
168 # Now, test that the rotation back to wheel coordinates works.
169 # The easiest way to do this is to point the wheel in a direction,
170 # move in that direction, and confirm that there is no lateral velocity.
171 for robot_angle in [0.0, 1.0, -5.0]:
172 for module_angle in [0.0, 1.0, -5.0]:
173 wheel_patch_velocity = numpy.array(
174 wheel_ground_velocity(
175 state_vector(velocity=numpy.array(
176 [[numpy.cos(robot_angle + module_angle)],
177 [numpy.sin(robot_angle + module_angle)]]),
178 theta=robot_angle,
179 module_angle=module_angle), self.I))
180
181 assert_array_almost_equal(wheel_patch_velocity,
182 numpy.array([[1], [0]]))
183
184 def test_slip_angle(self):
185 """Tests that the slip_angle calculation works."""
186 velocity = numpy.array([[1.5], [0.0]])
187
188 for i in range(4):
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]189 for wrap in range(-1, 2):
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]190 for theta in [0.0, 0.6, -0.4]:
191 module_angle = numpy.pi * wrap + theta
192
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]193 # We have redefined the angle to be the sin of the angle.
194 # That way, when the module flips directions, the slip angle also flips
195 # directions to keep it stable.
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]196 computed_angle = self.slip_angle[i](state_vector(
197 velocity=velocity,
198 module_angle=numpy.pi * wrap + theta), self.I)[0, 0]
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]199
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]200 expected = numpy.sin(numpy.pi * wrap + theta)
201
202 self.assertAlmostEqual(
203 expected,
204 computed_angle,
205 msg=f"Trying wrap {wrap} theta {theta}")
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]206
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]207 def test_wheel_torque(self):
208 """Tests that the per module self aligning forces have the right signs."""
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]209 # Point all the modules in a little bit.
210 X = state_vector(
211 velocity=numpy.array([[1.0], [0.0]]),
212 module_angles=[-0.001, -0.001, 0.001, 0.001],
213 )
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]214 xdot_equal = self.swerve_physics(X, self.I)
215
216 self.assertGreater(xdot_equal[2, 0], 0.0)
217 self.assertAlmostEqual(xdot_equal[3, 0], 0.0, places=1)
218 self.assertGreater(self.Ms[0](X, self.I)[0, 0], 0.0)
219
220 self.assertGreater(xdot_equal[6, 0], 0.0)
221 self.assertAlmostEqual(xdot_equal[7, 0], 0.0, places=1)
222 self.assertGreater(self.Ms[1](X, self.I)[0, 0], 0.0)
223
224 self.assertLess(xdot_equal[10, 0], 0.0)
225 self.assertAlmostEqual(xdot_equal[11, 0], 0.0, places=1)
226 self.assertLess(self.Ms[2](X, self.I)[0, 0], 0.0)
227
228 self.assertLess(xdot_equal[14, 0], 0.0)
229 self.assertAlmostEqual(xdot_equal[15, 0], 0.0, places=1)
230 self.assertLess(self.Ms[3](X, self.I)[0, 0], 0.0)
231
232 # Shouldn't be spinning.
233 self.assertAlmostEqual(xdot_equal[21, 0], 0.0, places=2)
234
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]235 # Now, make the bot want to go left by going to the other side.
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]236 # The wheels will be going too fast based on our calcs, so they should be decelerating.
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]237 X = state_vector(
238 velocity=numpy.array([[1.0], [0.0]]),
239 module_angles=[0.01, 0.01, 0.01, 0.01],
240 )
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]241 xdot_left = self.swerve_physics(X, self.I)
242
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]243 self.assertLess(xdot_left[2, 0], -0.05)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]244 self.assertLess(xdot_left[3, 0], 0.0)
245 self.assertLess(self.Ms[0](X, self.I)[0, 0], 0.0)
246
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]247 self.assertLess(xdot_left[6, 0], -0.05)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]248 self.assertLess(xdot_left[7, 0], 0.0)
249 self.assertLess(self.Ms[1](X, self.I)[0, 0], 0.0)
250
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]251 self.assertLess(xdot_left[10, 0], -0.05)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]252 self.assertLess(xdot_left[11, 0], 0.0)
253 self.assertLess(self.Ms[2](X, self.I)[0, 0], 0.0)
254
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]255 self.assertLess(xdot_left[14, 0], -0.05)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]256 self.assertLess(xdot_left[15, 0], 0.0)
257 self.assertLess(self.Ms[3](X, self.I)[0, 0], 0.0)
258
259 self.assertGreater(xdot_left[19, 0], 0.0001)
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]260 self.assertGreater(xdot_left[20, 0], 0.05)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]261 # Shouldn't be spinning.
262 self.assertAlmostEqual(xdot_left[21, 0], 0.0)
263
264 # And now do it to the right too.
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]265 X = state_vector(
266 velocity=numpy.array([[1.0], [0.0]]),
267 module_angles=[-0.01, -0.01, -0.01, -0.01],
268 )
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]269 xdot_right = self.swerve_physics(X, self.I)
270
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]271 self.assertGreater(xdot_right[2, 0], 0.05)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]272 self.assertLess(xdot_right[3, 0], 0.0)
273 self.assertGreater(self.Ms[0](X, self.I)[0, 0], 0.0)
274
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]275 self.assertGreater(xdot_right[6, 0], 0.05)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]276 self.assertLess(xdot_right[7, 0], 0.0)
277 self.assertGreater(self.Ms[1](X, self.I)[0, 0], 0.0)
278
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]279 self.assertGreater(xdot_right[10, 0], 0.05)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]280 self.assertLess(xdot_right[11, 0], 0.0)
281 self.assertGreater(self.Ms[2](X, self.I)[0, 0], 0.0)
282
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]283 self.assertGreater(xdot_right[14, 0], 0.05)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]284 self.assertLess(xdot_right[15, 0], 0.0)
285 self.assertGreater(self.Ms[3](X, self.I)[0, 0], 0.0)
286
287 self.assertGreater(xdot_right[19, 0], 0.0001)
Austin Schuh27694fa2024-07-20 16:29:49 -0700[diff] [blame]288 self.assertLess(xdot_right[20, 0], -0.05)
289 # Shouldn't be spinning.
290 self.assertAlmostEqual(xdot_right[21, 0], 0.0)
291
292 def test_wheel_torque_backwards_nocaster(self):
293 """Tests that the per module self aligning forces have the right signs when going backwards."""
294 self.wrap(nocaster_dynamics)
295 # Point all the modules in a little bit, going backwards.
296 X = state_vector(
297 velocity=numpy.array([[1.0], [0.0]]),
298 module_angles=[
299 numpy.pi - 0.001,
300 numpy.pi - 0.001,
301 numpy.pi + 0.001,
302 numpy.pi + 0.001,
303 ],
304 drive_wheel_velocity=-1,
305 )
306 xdot_equal = self.swerve_physics(X, self.I)
307
308 self.assertGreater(xdot_equal[2, 0], 0.0, msg="Steering backwards")
309 self.assertAlmostEqual(xdot_equal[3, 0], 0.0, places=1)
310 self.assertGreater(self.Ms[0](X, self.I)[0, 0], 0.0)
311
312 self.assertGreater(xdot_equal[6, 0], 0.0, msg="Steering backwards")
313 self.assertAlmostEqual(xdot_equal[7, 0], 0.0, places=1)
314 self.assertGreater(self.Ms[1](X, self.I)[0, 0], 0.0)
315
316 self.assertLess(xdot_equal[10, 0], 0.0, msg="Steering backwards")
317 self.assertAlmostEqual(xdot_equal[11, 0], 0.0, places=1)
318 self.assertLess(self.Ms[2](X, self.I)[0, 0], 0.0)
319
320 self.assertLess(xdot_equal[14, 0], 0.0, msg="Steering backwards")
321 self.assertAlmostEqual(xdot_equal[15, 0], 0.0, places=1)
322 self.assertLess(self.Ms[3](X, self.I)[0, 0], 0.0)
323
324 # Shouldn't be spinning.
325 self.assertAlmostEqual(xdot_equal[21, 0], 0.0, places=2)
326
327 # Now, make the bot want to go left by going to the other side.
328 # The wheels will be going too fast based on our calcs, so they should be decelerating.
329 X = state_vector(
330 velocity=numpy.array([[1.0], [0.0]]),
331 module_angles=[numpy.pi + 0.01] * 4,
332 drive_wheel_velocity=-1,
333 )
334 xdot_left = self.swerve_physics(X, self.I)
335
336 self.assertLess(xdot_left[2, 0], -0.05)
337 self.assertGreater(xdot_left[3, 0], 0.0)
338 self.assertLess(self.Ms[0](X, self.I)[0, 0], 0.0)
339
340 self.assertLess(xdot_left[6, 0], -0.05)
341 self.assertGreater(xdot_left[7, 0], 0.0)
342 self.assertLess(self.Ms[1](X, self.I)[0, 0], 0.0)
343
344 self.assertLess(xdot_left[10, 0], -0.05)
345 self.assertGreater(xdot_left[11, 0], 0.0)
346 self.assertLess(self.Ms[2](X, self.I)[0, 0], 0.0)
347
348 self.assertLess(xdot_left[14, 0], -0.05)
349 self.assertGreater(xdot_left[15, 0], 0.0)
350 self.assertLess(self.Ms[3](X, self.I)[0, 0], 0.0)
351
352 self.assertGreater(xdot_left[19, 0], 0.0001)
353 self.assertGreater(xdot_left[20, 0], 0.05)
354 # Shouldn't be spinning.
355 self.assertAlmostEqual(xdot_left[21, 0], 0.0)
356
357 # And now do it to the right too.
358 X = state_vector(
359 velocity=numpy.array([[1.0], [0.0]]),
360 drive_wheel_velocity=-1,
361 module_angles=[-0.01 + numpy.pi] * 4,
362 )
363 xdot_right = self.swerve_physics(X, self.I)
364
365 self.assertGreater(xdot_right[2, 0], 0.05)
366 self.assertGreater(xdot_right[3, 0], 0.0)
367 self.assertGreater(self.Ms[0](X, self.I)[0, 0], 0.0)
368
369 self.assertGreater(xdot_right[6, 0], 0.05)
370 self.assertGreater(xdot_right[7, 0], 0.0)
371 self.assertGreater(self.Ms[1](X, self.I)[0, 0], 0.0)
372
373 self.assertGreater(xdot_right[10, 0], 0.05)
374 self.assertGreater(xdot_right[11, 0], 0.0)
375 self.assertGreater(self.Ms[2](X, self.I)[0, 0], 0.0)
376
377 self.assertGreater(xdot_right[14, 0], 0.05)
378 self.assertGreater(xdot_right[15, 0], 0.0)
379 self.assertGreater(self.Ms[3](X, self.I)[0, 0], 0.0)
380
381 self.assertGreater(xdot_right[19, 0], 0.0001)
382 self.assertLess(xdot_right[20, 0], -0.05)
383 # Shouldn't be spinning.
384 self.assertAlmostEqual(xdot_right[21, 0], 0.0)
385
386 def test_wheel_torque_backwards_caster(self):
387 """Tests that the per module self aligning forces have the right signs when going backwards with a lot of caster."""
388 self.wrap(bigcaster_dynamics)
389 # Point all the modules in a little bit, going backwards.
390 X = state_vector(
391 velocity=numpy.array([[1.0], [0.0]]),
392 module_angles=[
393 numpy.pi - 0.001,
394 numpy.pi - 0.001,
395 numpy.pi + 0.001,
396 numpy.pi + 0.001,
397 ],
398 drive_wheel_velocity=-1,
399 )
400 xdot_equal = self.swerve_physics(X, self.I)
401
402 self.assertLess(xdot_equal[2, 0], 0.0, msg="Steering backwards")
403 self.assertAlmostEqual(xdot_equal[3, 0], 0.0, places=1)
404 self.assertLess(self.Ms[0](X, self.I)[0, 0], 0.0)
405
406 self.assertLess(xdot_equal[6, 0], 0.0, msg="Steering backwards")
407 self.assertAlmostEqual(xdot_equal[7, 0], 0.0, places=1)
408 self.assertLess(self.Ms[1](X, self.I)[0, 0], 0.0)
409
410 self.assertGreater(xdot_equal[10, 0], 0.0, msg="Steering backwards")
411 self.assertAlmostEqual(xdot_equal[11, 0], 0.0, places=1)
412 self.assertGreater(self.Ms[2](X, self.I)[0, 0], 0.0)
413
414 self.assertGreater(xdot_equal[14, 0], 0.0, msg="Steering backwards")
415 self.assertAlmostEqual(xdot_equal[15, 0], 0.0, places=1)
416 self.assertGreater(self.Ms[3](X, self.I)[0, 0], 0.0)
417
418 # Shouldn't be spinning.
419 self.assertAlmostEqual(xdot_equal[21, 0], 0.0, places=2)
420
421 # Now, make the bot want to go left by going to the other side.
422 # The wheels will be going too fast based on our calcs, so they should be decelerating.
423 X = state_vector(
424 velocity=numpy.array([[1.0], [0.0]]),
425 module_angles=[numpy.pi + 0.01] * 4,
426 drive_wheel_velocity=-1,
427 )
428 xdot_left = self.swerve_physics(X, self.I)
429
430 self.assertGreater(xdot_left[2, 0], -0.05)
431 self.assertGreater(xdot_left[3, 0], 0.0)
432 self.assertGreater(self.Ms[0](X, self.I)[0, 0], 0.0)
433
434 self.assertGreater(xdot_left[6, 0], -0.05)
435 self.assertGreater(xdot_left[7, 0], 0.0)
436 self.assertGreater(self.Ms[1](X, self.I)[0, 0], 0.0)
437
438 self.assertGreater(xdot_left[10, 0], -0.05)
439 self.assertGreater(xdot_left[11, 0], 0.0)
440 self.assertGreater(self.Ms[2](X, self.I)[0, 0], 0.0)
441
442 self.assertGreater(xdot_left[14, 0], -0.05)
443 self.assertGreater(xdot_left[15, 0], 0.0)
444 self.assertGreater(self.Ms[3](X, self.I)[0, 0], 0.0)
445
446 self.assertGreater(xdot_left[19, 0], 0.0001)
447 self.assertGreater(xdot_left[20, 0], 0.05)
448 # Shouldn't be spinning.
449 self.assertAlmostEqual(xdot_left[21, 0], 0.0)
450
451 # And now do it to the right too.
452 X = state_vector(
453 velocity=numpy.array([[1.0], [0.0]]),
454 drive_wheel_velocity=-1,
455 module_angles=[-0.01 + numpy.pi] * 4,
456 )
457 xdot_right = self.swerve_physics(X, self.I)
458
459 self.assertLess(xdot_right[2, 0], 0.05)
460 self.assertGreater(xdot_right[3, 0], 0.0)
461 self.assertLess(self.Ms[0](X, self.I)[0, 0], 0.0)
462
463 self.assertLess(xdot_right[6, 0], 0.05)
464 self.assertGreater(xdot_right[7, 0], 0.0)
465 self.assertLess(self.Ms[1](X, self.I)[0, 0], 0.0)
466
467 self.assertLess(xdot_right[10, 0], 0.05)
468 self.assertGreater(xdot_right[11, 0], 0.0)
469 self.assertLess(self.Ms[2](X, self.I)[0, 0], 0.0)
470
471 self.assertLess(xdot_right[14, 0], 0.05)
472 self.assertGreater(xdot_right[15, 0], 0.0)
473 self.assertLess(self.Ms[3](X, self.I)[0, 0], 0.0)
474
475 self.assertGreater(xdot_right[19, 0], 0.0001)
476 self.assertLess(xdot_right[20, 0], -0.05)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]477 # Shouldn't be spinning.
478 self.assertAlmostEqual(xdot_right[21, 0], 0.0)
479
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]480 def test_wheel_forces(self):
481 """Tests that the per module forces have the right signs."""
482 for i in range(4):
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]483 wheel_force = wrap_module(dynamics.wheel_force, i)
484
485 X = state_vector()
486 robot_equal = wheel_force(X, self.I)
487 xdot_equal = self.swerve_physics(X, self.I)
488 self.assertEqual(robot_equal[0, 0], 0.0)
489 self.assertEqual(robot_equal[1, 0], 0.0)
490 self.assertEqual(xdot_equal[2 + 4 * i], 0.0)
491 self.assertEqual(xdot_equal[3 + 4 * i], 0.0)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]492
493 # Robot is moving faster than the wheels, it should decelerate.
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]494 X = state_vector(dx=0.01)
495 robot_faster = wheel_force(X, self.I)
496 xdot_faster = self.swerve_physics(X, self.I)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]497 self.assertLess(robot_faster[0, 0], -0.1)
498 self.assertEqual(robot_faster[1, 0], 0.0)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]499 self.assertGreater(xdot_faster[3 + 4 * i], 0.0)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]500
501 # Robot is now going slower than the wheels. It should accelerate.
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]502 X = state_vector(dx=-0.01)
503 robot_slower = wheel_force(X, self.I)
504 xdot_slower = self.swerve_physics(X, self.I)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]505 self.assertGreater(robot_slower[0, 0], 0.1)
506 self.assertEqual(robot_slower[1, 0], 0.0)
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]507 self.assertLess(xdot_slower[3 + 4 * i], 0.0)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]508
509 # Positive lateral velocity -> negative force.
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]510 robot_left = wheel_force(state_vector(dy=0.01), self.I)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]511 self.assertEqual(robot_left[0, 0], 0.0)
512 self.assertLess(robot_left[1, 0], -0.1)
513
514 # Negative lateral velocity -> positive force.
Austin Schuhb8b34be2024-07-14 16:06:19 -0700[diff] [blame]515 robot_right = wheel_force(state_vector(dy=-0.01), self.I)
Austin Schuh2a1abec2024-07-10 20:31:16 -0700[diff] [blame]516 self.assertEqual(robot_right[0, 0], 0.0)
517 self.assertGreater(robot_right[1, 0], 0.1)
518
519
520if __name__ == '__main__':
521 unittest.main()