Make swerve C++ physics match python
This is done by inspection right now, when we start exporting solvers
from casadi, we'll need to revisit how a lot of this works.
Change-Id: Ic8468bb48e2cee2469615be54c00d2e067420236
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index bb5a51a..781943c 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -446,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));
@@ -482,7 +482,7 @@
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 = sin_atan2");
+ result_py->emplace_back(" atan2 = casadi.atan2");
result_py->emplace_back(" fmax = casadi.fmax");
result_py->emplace_back(" fabs = casadi.fabs");
@@ -536,9 +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 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");
result_py.emplace_back("# [thetas0, thetad0, omegas0, omegad0,");
@@ -779,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__();
diff --git a/frc971/control_loops/swerve/physics_test.py b/frc971/control_loops/swerve/physics_test.py
index 0a40b22..ecfc1ce 100644
--- a/frc971/control_loops/swerve/physics_test.py
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -186,20 +186,10 @@
velocity = numpy.array([[1.5], [0.0]])
for i in range(4):
- x = casadi.SX.sym("x")
- y = casadi.SX.sym("y")
- sin_atan2 = casadi.Function('sin_atan2', [y, x],
- [dynamics.sin_atan2(y, x)])
-
for wrap in range(-1, 2):
for theta in [0.0, 0.6, -0.4]:
module_angle = numpy.pi * wrap + theta
- self.assertAlmostEqual(
- 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.