Fix initial launch velocity constraint in FRC 2022 shooter example (#578

)
SleipnirGroup · Jun 29, 2024 · 2123b4f · 2123b4f
1 parent 19234a7
commit 2123b4f
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 22 deletions.
diff --git a/examples/FRC2022Shooter/main.py b/examples/FRC2022Shooter/main.py
@@ -63,9 +63,9 @@ def lerp(a, b, t):
     p_x = X[0, :]
     p_y = X[1, :]
     p_z = X[2, :]
-    v_x = X[3, :] + np.full((1, N), robot_initial_v_x)
-    v_y = X[4, :] + np.full((1, N), robot_initial_v_y)
-    v_z = X[5, :] + np.full((1, N), robot_initial_v_z)
+    v_x = X[3, :]
+    v_y = X[4, :]
+    v_z = X[5, :]
 
     # Position initial guess is linear interpolation between start and end position
     for k in range(N):
@@ -77,9 +77,9 @@ def lerp(a, b, t):
     uvec_shooter_to_target = target - shooter
     uvec_shooter_to_target /= norm(uvec_shooter_to_target)
     for k in range(N):
-        X[3, k].set_value(max_launch_velocity * uvec_shooter_to_target[0, 0])
-        X[4, k].set_value(max_launch_velocity * uvec_shooter_to_target[1, 0])
-        X[5, k].set_value(max_launch_velocity * uvec_shooter_to_target[2, 0])
+        v_x[k].set_value(max_launch_velocity * uvec_shooter_to_target[0, 0])
+        v_y[k].set_value(max_launch_velocity * uvec_shooter_to_target[1, 0])
+        v_z[k].set_value(max_launch_velocity * uvec_shooter_to_target[2, 0])
 
     # Shooter initial position
     problem.subject_to(X[:3, 0] == shooter)
@@ -115,9 +115,9 @@ def f(x):
         m = 2.0  # kg
         a_D = lambda v: 0.5 * rho * v**2 * C_D * A / m
 
-        v_x = x[3, 0]
-        v_y = x[4, 0]
-        v_z = x[5, 0]
+        v_x = x[3, 0] + robot_initial_v_x
+        v_y = x[4, 0] + robot_initial_v_y
+        v_z = x[5, 0] + robot_initial_v_z
         return VariableMatrix(
             [[v_x], [v_y], [v_z], [-a_D(v_x)], [-a_D(v_y)], [-g - a_D(v_z)]]
         )

diff --git a/examples/FRC2022Shooter/src/Main.cpp b/examples/FRC2022Shooter/src/Main.cpp
@@ -56,12 +56,9 @@ int main() {
   auto p_x = X.Row(0);
   auto p_y = X.Row(1);
   auto p_z = X.Row(2);
-  auto v_x =
-      X.Row(3) + Eigen::RowVector<double, N>::Constant(robot_initial_v_x);
-  auto v_y =
-      X.Row(4) + Eigen::RowVector<double, N>::Constant(robot_initial_v_y);
-  auto v_z =
-      X.Row(5) + Eigen::RowVector<double, N>::Constant(robot_initial_v_z);
+  auto v_x = X.Row(3);
+  auto v_y = X.Row(4);
+  auto v_z = X.Row(5);
 
   // Position initial guess is linear interpolation between start and end
   // position
@@ -74,9 +71,9 @@ int main() {
   // Velocity initial guess is max launch velocity toward goal
   Eigen::Vector3d uvec_shooter_to_target = (target - shooter).normalized();
   for (int k = 0; k < N; ++k) {
-    X(3, k).SetValue(max_launch_velocity * uvec_shooter_to_target(0, 0));
-    X(4, k).SetValue(max_launch_velocity * uvec_shooter_to_target(1, 0));
-    X(5, k).SetValue(max_launch_velocity * uvec_shooter_to_target(2, 0));
+    v_x(k).SetValue(max_launch_velocity * uvec_shooter_to_target(0, 0));
+    v_y(k).SetValue(max_launch_velocity * uvec_shooter_to_target(1, 0));
+    v_z(k).SetValue(max_launch_velocity * uvec_shooter_to_target(2, 0));
   }
 
   // Shooter initial position
@@ -97,7 +94,7 @@ int main() {
   // Require the final velocity is down
   problem.SubjectTo(v_z(N - 1) < 0.0);
 
-  auto f = [](sleipnir::VariableMatrix x) {
+  auto f = [&](sleipnir::VariableMatrix x) {
     // x' = x'
     // y' = y'
     // z' = z'
@@ -112,9 +109,9 @@ int main() {
     constexpr double m = 2.0;  // kg
     auto a_D = [](auto v) { return 0.5 * rho * v * v * C_D * A / m; };
 
-    auto v_x = x(3, 0);
-    auto v_y = x(4, 0);
-    auto v_z = x(5, 0);
+    auto v_x = x(3, 0) + robot_initial_v_x;
+    auto v_y = x(4, 0) + robot_initial_v_y;
+    auto v_z = x(5, 0) + robot_initial_v_z;
     return sleipnir::VariableMatrix{{v_x},       {v_y},       {v_z},
                                     {-a_D(v_x)}, {-a_D(v_y)}, {-g - a_D(v_z)}};
   };