Add sigmoid to replace abs(x) in atan2(y, x) to keep it smooth on the x-axis
Signed-off-by: justinT21 <jjturcot@gmail.com>
Change-Id: Iff4891607ad77d82716a2825f03b4ee086f0eb9f
diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
index 5adef05..cbc4e66 100644
--- a/frc971/control_loops/swerve/BUILD
+++ b/frc971/control_loops/swerve/BUILD
@@ -265,3 +265,16 @@
"@pip//scipy",
],
)
+
+py_binary(
+ name = "smooth_function_graph",
+ srcs = [
+ "smooth_function_graph.py",
+ ],
+ deps = [
+ "@pip//matplotlib",
+ "@pip//numpy",
+ "@pip//pygobject",
+ "@pip//scipy",
+ ],
+)
diff --git a/frc971/control_loops/swerve/casadi_velocity_mpc.py b/frc971/control_loops/swerve/casadi_velocity_mpc.py
index 5d1f582..f5e746b 100644
--- a/frc971/control_loops/swerve/casadi_velocity_mpc.py
+++ b/frc971/control_loops/swerve/casadi_velocity_mpc.py
@@ -252,8 +252,8 @@
mpc = MPC(solver='fatrop') if not full_debug else MPC(solver='ipopt')
R_goal = numpy.zeros((3, 1))
-R_goal[0, 0] = 1.0
-R_goal[1, 0] = 1.0
+R_goal[0, 0] = 0.0
+R_goal[1, 0] = -1.0
R_goal[2, 0] = 0.0
module_velocity = 0.0
diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index e712a39..d647d4b 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -567,12 +567,15 @@
result_py.emplace_back(" ])");
result_py.emplace_back("");
constexpr double kLogGain = 1.0 / 0.05;
+ constexpr double kAbsGain = 1.0 / 0.05;
result_py.emplace_back("def soft_atan2(y, x):");
result_py.emplace_back(" return casadi.arctan2(");
result_py.emplace_back(" y,");
result_py.emplace_back(" casadi.logsumexp(casadi.SX(numpy.array(");
- result_py.emplace_back(absl::Substitute(
- " [1.0, casadi.fabs(x) * $0.0]))) / $0.0)", kLogGain));
+ result_py.emplace_back(
+ absl::Substitute(" [1.0, x * (1.0 - 2.0 / (1 + "
+ "casadi.exp($1.0 * x))) * $0.0]))) / $0.0)",
+ kLogGain, kAbsGain));
result_py.emplace_back("");
result_py.emplace_back("# Returns the derivative of our state vector");
diff --git a/frc971/control_loops/swerve/smooth_function_graph.py b/frc971/control_loops/swerve/smooth_function_graph.py
new file mode 100644
index 0000000..570b3a6
--- /dev/null
+++ b/frc971/control_loops/swerve/smooth_function_graph.py
@@ -0,0 +1,111 @@
+#!/usr/bin/python3
+
+import numpy
+import scipy
+import matplotlib.pyplot as plt
+import matplotlib
+from scipy.special import logsumexp
+
+x = numpy.linspace(-10, 10, 1000)
+y0 = numpy.zeros((1000, 1))
+y1 = numpy.zeros((1000, 1))
+
+# add more detail near 0
+X = numpy.sort(
+ numpy.hstack(
+ [numpy.arange(-1, 1, 0.005),
+ numpy.arange(-0.005, 0.005, 0.0001)]))
+Y = numpy.sort(
+ numpy.hstack(
+ [numpy.arange(-1, 1, 0.005),
+ numpy.arange(-0.005, 0.005, 0.0001)]))
+X, Y = numpy.meshgrid(X, Y)
+
+
+def single_slip_force(vy, vx):
+ return numpy.arctan2(vy, numpy.abs(vx))
+
+
+def single_new_slip_force(vy, vx):
+ loggain = 1 / 0.05
+ return numpy.arctan2(vy,
+ logsumexp([1.0, numpy.abs(vx) * loggain]) / loggain)
+
+
+def single_new_new_slip_force(vy, vx):
+ loggain = 1 / 0.05
+ loggain2 = 1 / 0.05
+ return numpy.arctan2(
+ vy,
+ logsumexp(
+ [1.0, vx * (1.0 - 2.0 /
+ (1 + numpy.exp(loggain2 * vx))) * loggain]) / loggain)
+
+
+velocity = 0.1
+
+acc_val = single_new_new_slip_force(velocity, velocity)
+expt_val = single_new_slip_force(velocity, velocity)
+
+print("Percent Error: ", (acc_val - expt_val) / expt_val * 100)
+
+slip_force = scipy.vectorize(single_slip_force)
+new_slip_force = scipy.vectorize(single_new_slip_force)
+new_new_slip_force = scipy.vectorize(single_new_new_slip_force)
+
+Y0 = slip_force(Y, X)
+Y1 = new_slip_force(Y, X)
+Y2 = new_new_slip_force(Y, X)
+Y3 = Y2 - Y1
+
+matplotlib.rcParams['figure.figsize'] = (15, 15)
+
+fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
+surf = ax.plot_surface(X,
+ Y,
+ Y0,
+ cmap=matplotlib.cm.coolwarm,
+ linewidth=0,
+ antialiased=False)
+ax.set_xlabel('X')
+ax.set_ylabel('Y')
+ax.set_zlabel('atan2(y, x)')
+fig.suptitle("Atan2")
+
+fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
+surf = ax.plot_surface(X,
+ Y,
+ Y1,
+ cmap=matplotlib.cm.coolwarm,
+ linewidth=0,
+ antialiased=False)
+ax.set_xlabel('X')
+ax.set_ylabel('Y')
+ax.set_zlabel('atan2(y, x)')
+fig.suptitle("Softened y atan2")
+
+fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
+surf = ax.plot_surface(X,
+ Y,
+ Y2,
+ cmap=matplotlib.cm.coolwarm,
+ linewidth=0,
+ antialiased=False)
+ax.set_xlabel('X')
+ax.set_ylabel('Y')
+ax.set_zlabel('atan2(y, x)')
+fig.suptitle("Softened x and y atan2")
+
+fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
+surf = ax.plot_surface(X,
+ Y,
+ Y3,
+ cmap=matplotlib.cm.coolwarm,
+ linewidth=0,
+ antialiased=False)
+ax.set_xlabel('X')
+ax.set_ylabel('Y')
+ax.set_zlabel('Error')
+fig.suptitle("Error between Soft_atan2 and the new one")
+
+plt.show()