sleipnirgroup-jormungandr 0.6.7.dev21__tar.gz → 0.6.7.dev23__tar.gz

{sleipnirgroup_jormungandr-0.6.7.dev21 → sleipnirgroup_jormungandr-0.6.7.dev23}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: sleipnirgroup-jormungandr
-Version: 0.6.7.dev21
+Version: 0.6.7.dev23
 Summary: Reverse mode autodiff library and NLP solver DSL
 License-Expression: BSD-3-Clause
 License-File: LICENSE.txt

{sleipnirgroup_jormungandr-0.6.7.dev21 → sleipnirgroup_jormungandr-0.6.7.dev23}/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp

@@ -167,11 +167,13 @@ ExitStatus feasibility_restoration(
   DenseVector fr_x{num_vars + 2 * num_eq};
   fr_x << x, p_e_0, n_e_0;
 
-  DenseVector fr_s = DenseVector::Ones(2 * num_eq);
+  // Start the constraints pₑ, nₑ ≥ 0 with zero violation
+  DenseVector fr_s{2 * num_eq};
+  fr_s << p_e_0, n_e_0;
 
   DenseVector fr_y = DenseVector::Zero(num_eq);
 
-  // Force the duals to start with perfect complementarity with the slacks
+  // Start the duals with perfect complementarity with respect to the slacks
   DenseVector fr_z{2 * num_eq};
   fr_z << fr_μ * p_e_0.cwiseInverse(), fr_μ * n_e_0.cwiseInverse();
 
@@ -428,13 +430,18 @@ ExitStatus feasibility_restoration(
   DenseVector fr_x{num_vars + 2 * num_eq + 2 * num_ineq};
   fr_x << x, p_e_0, n_e_0, p_i_0, n_i_0;
 
+  // Use the original problem's slacks for cᵢ(x) − pᵢ + nᵢ ≥ 0 since
+  // cᵢ(x) − pᵢ + nᵢ = s by construction.
+  //
+  // Start the constraints pₑ, nₑ, pᵢ, nᵢ ≥ 0 with zero violation.
   DenseVector fr_s{s.rows() + 2 * num_eq + 2 * num_ineq};
   fr_s.segment(0, s.rows()) = s;
-  fr_s.segment(s.rows(), 2 * num_eq + 2 * num_ineq).setOnes();
+  fr_s.segment(s.rows(), 2 * num_eq + 2 * num_ineq) << p_e_0, n_e_0, p_i_0,
+      n_i_0;
 
   DenseVector fr_y = DenseVector::Zero(c_e.rows());
 
-  // Force the duals to start with perfect complementarity with the slacks
+  // Start the duals with perfect complementarity with respect to the slacks
   DenseVector fr_z{c_i.rows() + 2 * num_eq + 2 * num_ineq};
   fr_z << fr_μ * s.cwiseInverse(), fr_μ * p_e_0.cwiseInverse(),
       fr_μ * n_e_0.cwiseInverse(), fr_μ * p_i_0.cwiseInverse(),

{sleipnirgroup_jormungandr-0.6.7.dev21 → sleipnirgroup_jormungandr-0.6.7.dev23}/python/src/sleipnir/__init__.py

@@ -2,4 +2,4 @@
 
 from ._sleipnir import *
 
-__version__ = "0.6.7.dev21"
+__version__ = "0.6.7.dev23"

{sleipnirgroup_jormungandr-0.6.7.dev21 → sleipnirgroup_jormungandr-0.6.7.dev23}/python/test/optimization/cart_pole_ocp_test.py

@@ -6,7 +6,6 @@ from cart_pole_util import (
     cart_pole_dynamics_double,
     cart_pole_dynamics_variable,
 )
-from rk4 import rk4
 from sleipnir.autodiff import ExpressionType, VariableMatrix
 from sleipnir.optimization import (
     OCP,
@@ -86,28 +85,37 @@ def test_cart_pole_ocp():
     assert X.value(2, 0) == pytest.approx(x_initial[2, 0], abs=1e-8)
     assert X.value(3, 0) == pytest.approx(x_initial[3, 0], abs=1e-8)
 
-    # FIXME: Replay diverges
-    if 0:
-        # Verify solution
-        x = np.zeros((4, 1))
-        u = np.zeros((1, 1))
-        for k in range(N):
-            # Cart position constraints
-            assert X[0, k] >= 0.0
-            assert X[0, k] <= d_max
-
-            # Input constraints
-            assert U[0, k] >= -u_max
-            assert U[0, k] <= u_max
-
-            # Verify state
-            assert X.value(0, k) == pytest.approx(x[0, 0], abs=1e-2)
-            assert X.value(1, k) == pytest.approx(x[1, 0], abs=1e-2)
-            assert X.value(2, k) == pytest.approx(x[2, 0], abs=1e-2)
-            assert X.value(3, k) == pytest.approx(x[3, 0], abs=1e-2)
-
-            # Project state forward
-            x = rk4(cart_pole_dynamics_double, x, u, dt)
+    # Verify solution
+    for k in range(N):
+        # Cart position constraints
+        assert X.value(0, k) >= 0.0
+        assert X.value(0, k) <= d_max
+
+        # Input constraints
+        assert U.value(0, k) >= -u_max
+        assert U.value(0, k) <= u_max
+
+        # Dynamics constraints
+        #
+        # Direct collocation constrains the system dynamics at the midpoint of a
+        # cubic Hermite spline through each pair of adjacent states.
+        f = cart_pole_dynamics_double
+        h = problem.dt().value(0, k)
+        x_begin = X[:, k : k + 1].value()
+        x_end = X[:, k + 1 : k + 2].value()
+        u_begin = U[:, k : k + 1].value()
+        u_end = U[:, k + 1 : k + 2].value()
+
+        xdot_begin = f(x_begin, u_begin)
+        xdot_end = f(x_end, u_end)
+        xdot_c = -3.0 / (2.0 * h) * (x_begin - x_end) - 0.25 * (xdot_begin + xdot_end)
+
+        x_c = 0.5 * (x_begin + x_end) + h / 8.0 * (xdot_begin - xdot_end)
+        u_c = 0.5 * (u_begin + u_end)
+
+        expected_xdot_c = f(x_c, u_c)
+        for row in range(xdot_c.shape[0]):
+            assert xdot_c[row, 0] == pytest.approx(expected_xdot_c[row, 0], abs=1e-8)
 
     # Verify final state
     assert X.value(0, N) == pytest.approx(x_final[0, 0], abs=1e-8)
@@ -121,17 +129,20 @@ def test_cart_pole_ocp():
             "Time (s),Cart position (m),Pole angle (rad),Cart velocity (m/s),Pole angular velocity (rad/s)\n"
         )
 
+        time = 0.0
         for k in range(N + 1):
             f.write(
-                f"{k * dt},{X.value(0, k)},{X.value(1, k)},{X.value(2, k)},{X.value(3, k)}\n"
+                f"{time},{X.value(0, k)},{X.value(1, k)},{X.value(2, k)},{X.value(3, k)}\n"
             )
 
+            time += problem.dt().value(0, k)
+
     # Log inputs for offline viewing
     with open("Cart-pole inputs.csv", "w") as f:
         f.write("Time (s),Cart force (N)\n")
 
+        time = 0.0
         for k in range(N + 1):
-            if k < N:
-                f.write(f"{k * dt},{U.value(0, k)}\n")
-            else:
-                f.write(f"{k * dt},0.0\n")
+            f.write(f"{time},{U.value(0, k)}\n")
+
+            time += problem.dt().value(0, k)

{sleipnirgroup_jormungandr-0.6.7.dev21 → sleipnirgroup_jormungandr-0.6.7.dev23}/python/test/optimization/differential_drive_ocp_test.py

@@ -68,29 +68,24 @@ def test_differential_drive_ocp():
     assert X.value(3, 0) == pytest.approx(x_initial[3, 0], abs=1e-8)
     assert X.value(4, 0) == pytest.approx(x_initial[4, 0], abs=1e-8)
 
-    # FIXME: Replay diverges
-    if 0:
-        # Verify solution
-        x = np.zeros((5, 1))
-        u = np.zeros((2, 1))
-        for k in range(N):
-            u = U[:, k : k + 1].value()
-
-            # Input constraints
-            assert U[0, k].value() >= -u_max[0]
-            assert U[0, k].value() <= u_max[0]
-            assert U[1, k].value() >= -u_max[1]
-            assert U[1, k].value() <= u_max[1]
-
-            # Verify state
-            assert X.value(0, k) == pytest.approx(x[0, 0], abs=1e-8)
-            assert X.value(1, k) == pytest.approx(x[1, 0], abs=1e-8)
-            assert X.value(2, k) == pytest.approx(x[2, 0], abs=1e-8)
-            assert X.value(3, k) == pytest.approx(x[3, 0], abs=1e-8)
-            assert X.value(4, k) == pytest.approx(x[4, 0], abs=1e-8)
-
-            # Project state forward
-            x = rk4(differential_drive_dynamics_double, x, u, problem.dt().value(0, k))
+    # Verify solution
+    for k in range(N):
+        # Input constraints
+        assert U.value(0, k) >= -u_max[0, 0]
+        assert U.value(0, k) <= u_max[0, 0]
+        assert U.value(1, k) >= -u_max[1, 0]
+        assert U.value(1, k) <= u_max[1, 0]
+
+        # Dynamics constraints
+        expected_x_k1 = rk4(
+            differential_drive_dynamics_double,
+            X[:, k : k + 1].value(),
+            U[:, k : k + 1].value(),
+            problem.dt().value(0, k),
+        )
+        actual_x_k1 = X[:, k + 1 : k + 2].value()
+        for row in range(actual_x_k1.shape[0]):
+            assert actual_x_k1[row, 0] == pytest.approx(expected_x_k1[row, 0], abs=1e-8)
 
     # Verify final state
     assert X.value(0, N) == pytest.approx(x_final[0, 0], abs=1e-8)