enable MPC test (AtsushiSakai#620)

AtsushiSakai · web-flow · commit a13ef29dc4a2 · 2022-01-08T17:06:29.000+09:00
* enable_mpc_test

* enable_mpc_test

* enable_mpc_test

* enable_mpc_test

* enable_mpc_test

* enable_mpc_test

* enable_mpc_test

* enable_mpc_test
diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
@@ -43,7 +43,7 @@ class Rocket_Model_6DoF:
     A 6 degree of freedom rocket landing problem.
     """
 
-    def __init__(self):
+    def __init__(self, rng):
         """
         A large r_scale for a small scale problem will
         ead to numerical problems as parameters become excessively small
@@ -92,7 +92,7 @@ def __init__(self):
         # Vector from thrust point to CoM
         self.r_T_B = np.array([-1e-2, 0., 0.])
 
-        self.set_random_initial_state()
+        self.set_random_initial_state(rng)
 
         self.x_init = np.concatenate(
             ((self.m_wet,), self.r_I_init, self.v_I_init, self.q_B_I_init, self.w_B_init))
@@ -102,29 +102,32 @@ def __init__(self):
         self.r_scale = np.linalg.norm(self.r_I_init)
         self.m_scale = self.m_wet
 
-    def set_random_initial_state(self):
+    def set_random_initial_state(self, rng):
+        if rng is None:
+            rng = np.random.default_rng()
+
         self.r_I_init = np.array((0., 0., 0.))
-        self.r_I_init[0] = np.random.uniform(3, 4)
-        self.r_I_init[1:3] = np.random.uniform(-2, 2, size=2)
+        self.r_I_init[0] = rng.uniform(3, 4)
+        self.r_I_init[1:3] = rng.uniform(-2, 2, size=2)
 
         self.v_I_init = np.array((0., 0., 0.))
-        self.v_I_init[0] = np.random.uniform(-1, -0.5)
-        self.v_I_init[1:3] = np.random.uniform(
-            -0.5, -0.2, size=2) * self.r_I_init[1:3]
+        self.v_I_init[0] = rng.uniform(-1, -0.5)
+        self.v_I_init[1:3] = rng.uniform(-0.5, -0.2,
+                                         size=2) * self.r_I_init[1:3]
 
         self.q_B_I_init = self.euler_to_quat((0,
-                                              np.random.uniform(-30, 30),
-                                              np.random.uniform(-30, 30)))
+                                              rng.uniform(-30, 30),
+                                              rng.uniform(-30, 30)))
         self.w_B_init = np.deg2rad((0,
-                                    np.random.uniform(-20, 20),
-                                    np.random.uniform(-20, 20)))
+                                    rng.uniform(-20, 20),
+                                    rng.uniform(-20, 20)))
 
     def f_func(self, x, u):
         m, rx, ry, rz, vx, vy, vz, q0, q1, q2, q3, wx, wy, wz = x[0], x[1], x[
             2], x[3], x[4], x[5], x[6], x[7], x[8], x[9], x[10], x[11], x[12], x[13]
         ux, uy, uz = u[0], u[1], u[2]
 
-        return np.matrix([
+        return np.array([
             [-0.01 * np.sqrt(ux**2 + uy**2 + uz**2)],
             [vx],
             [vy],
@@ -149,7 +152,7 @@ def A_func(self, x, u):
             2], x[3], x[4], x[5], x[6], x[7], x[8], x[9], x[10], x[11], x[12], x[13]
         ux, uy, uz = u[0], u[1], u[2]
 
-        return np.matrix([
+        return np.array([
             [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
             [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
             [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
@@ -177,7 +180,7 @@ def B_func(self, x, u):
             2], x[3], x[4], x[5], x[6], x[7], x[8], x[9], x[10], x[11], x[12], x[13]
         ux, uy, uz = u[0], u[1], u[2]
 
-        return np.matrix([
+        return np.array([
             [-0.01 * ux / np.sqrt(ux**2 + uy**2 + uz**2),
              -0.01 * uy / np.sqrt(ux ** 2 + uy**2 + uz**2),
              -0.01 * uz / np.sqrt(ux**2 + uy**2 + uz**2)],
@@ -219,14 +222,14 @@ def euler_to_quat(self, a):
         return q
 
     def skew(self, v):
-        return np.matrix([
+        return np.array([
             [0, -v[2], v[1]],
             [v[2], 0, -v[0]],
             [-v[1], v[0], 0]
         ])
 
     def dir_cosine(self, q):
-        return np.matrix([
+        return np.array([
             [1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2]
                                                    + q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
             [2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2
@@ -236,7 +239,7 @@ def dir_cosine(self, q):
         ])
 
     def omega(self, w):
-        return np.matrix([
+        return np.array([
             [0, -w[0], -w[1], -w[2]],
             [w[0], 0, w[2], -w[1]],
             [w[1], -w[2], 0, w[0]],
@@ -304,7 +307,7 @@ def get_constraints(self, X_v, U_v, X_last_p, U_last_p):
         ]
 
         # linearized lower thrust constraint
-        rhs = [U_last_p[:, k] / cvxpy.norm(U_last_p[:, k]) * U_v[:, k]
+        rhs = [U_last_p[:, k] / cvxpy.norm(U_last_p[:, k]) @ U_v[:, k]
                for k in range(X_v.shape[1])]
         constraints += [
             self.T_min <= cvxpy.vstack(rhs)
@@ -460,11 +463,11 @@ def __init__(self, m, K):
         # x_t+1 = A_*x_t+B_*U_t+C_*U_T+1*S_*sigma+zbar+nu
         constraints += [
             self.var['X'][:, k + 1] ==
-            cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) *
+            cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) @
             self.var['X'][:, k] +
-            cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) *
+            cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) @
             self.var['U'][:, k] +
-            cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) *
+            cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) @
             self.var['U'][:, k + 1] +
             self.par['S_bar'][:, k] * self.var['sigma'] +
             self.par['z_bar'][:, k] +
@@ -606,9 +609,9 @@ def plot_animation(X, U):  # pragma: no cover
         plt.pause(0.5)
 
 
-def main():
+def main(rng=None):
     print("start!!")
-    m = Rocket_Model_6DoF()
+    m = Rocket_Model_6DoF(rng)
 
     # state and input list
     X = np.empty(shape=[m.n_x, K])
diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
@@ -40,14 +40,14 @@ def prm_planning(start_x, start_y, goal_x, goal_y,
     """
     Run probabilistic road map planning
 
-    :param start_x:
-    :param start_y:
-    :param goal_x:
-    :param goal_y:
-    :param obstacle_x_list:
-    :param obstacle_y_list:
-    :param robot_radius:
-    :param rng:
+    :param start_x: start x position
+    :param start_y: start y position
+    :param goal_x: goal x position
+    :param goal_y: goal y position
+    :param obstacle_x_list: obstacle x positions
+    :param obstacle_y_list: obstacle y positions
+    :param robot_radius: robot radius
+    :param rng: (Optional) Random generator
     :return:
     """
     obstacle_kd_tree = KDTree(np.vstack((obstacle_x_list, obstacle_y_list)).T)
diff --git a/tests/test_model_predictive_speed_and_steer_control.py b/tests/test_model_predictive_speed_and_steer_control.py
@@ -1,18 +1,19 @@
 import sys
 import conftest
 
-if 'cvxpy' in sys.modules:  # pragma: no cover
+from PathTracking.model_predictive_speed_and_steer_control \
+    import model_predictive_speed_and_steer_control as m
 
-    from PathTracking.model_predictive_speed_and_steer_control \
-        import model_predictive_speed_and_steer_control as m
 
-    def test_1():
-        m.show_animation = False
-        m.main()
+def test_1():
+    m.show_animation = False
+    m.main()
+
+
+def test_2():
+    m.show_animation = False
+    m.main2()
 
-    def test_2():
-        m.show_animation = False
-        m.main2()
 
 if __name__ == '__main__':
     conftest.run_this_test(__file__)
diff --git a/tests/test_probabilistic_road_map.py b/tests/test_probabilistic_road_map.py
@@ -5,7 +5,7 @@
 
 def test1():
     probabilistic_road_map.show_animation = False
-    probabilistic_road_map.main(rng=np.random.default_rng(1234))
+    probabilistic_road_map.main(rng=np.random.default_rng(1233))
 
 
 if __name__ == '__main__':
diff --git a/tests/test_rocket_powered_landing.py b/tests/test_rocket_powered_landing.py
@@ -1,13 +1,20 @@
 import conftest  # Add root path to sys.path
 import sys
+import numpy as np
+from numpy.testing import suppress_warnings
 
-if 'cvxpy' in sys.modules:  # pragma: no cover
+from AerialNavigation.rocket_powered_landing import rocket_powered_landing as m
 
-    from AerialNavigation.rocket_powered_landing import rocket_powered_landing as m
 
-    def test1():
-        m.show_animation = False
-        m.main()
+def test1():
+    m.show_animation = False
+    with suppress_warnings() as sup:
+        sup.filter(UserWarning,
+                   "You are solving a parameterized problem that is not DPP"
+                   )
+        sup.filter(UserWarning,
+                   "Solution may be inaccurate")
+        m.main(rng=np.random.default_rng(1234))
 
 
 if __name__ == '__main__':
