Updating to work with Python 3.14 and CVXPY 1.7.3; added new model

.python-version

@@ -0,0 +1 @@
+3.14

FixedFinalTime/parameters.py

@@ -3,10 +3,11 @@
 # Weight constants
 w_nu = 1e5  # virtual control
 # initial trust region radius
-tr_radius = 5
+tr_radius = 1.0
 # trust region variables
 rho_0 = 0.0
 rho_1 = 0.25
-rho_2 = 0.9
+rho_2 = 0.7
 alpha = 2.0
 beta = 3.2
+tol = 1e-3

FixedFinalTime/scproblem.py

@@ -38,9 +38,9 @@ def __init__(self, m, K):
         # Dynamics:
         constraints += [
             self.var['X'][:, k + 1] ==
-            cvx.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) * self.var['X'][:, k]
-            + cvx.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) * self.var['U'][:, k]
-            + cvx.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) * self.var['U'][:, k + 1]
+            cvx.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x), order='F') @ self.var['X'][:, k]
+            + cvx.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u), order='F') @ self.var['U'][:, k]
+            + cvx.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u), order='F') @ self.var['U'][:, k + 1]
             + self.par['z_bar'][:, k]
             + self.var['nu'][:, k]
             for k in range(K - 1)

FreeFinalTime/scproblem.py

@@ -42,9 +42,9 @@ def __init__(self, m, K):
         # Dynamics:
         constraints += [
             self.var['X'][:, k + 1] ==
-            cvx.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) * self.var['X'][:, k]
-            + cvx.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) * self.var['U'][:, k]
-            + cvx.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) * self.var['U'][:, k + 1]
+            cvx.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x), order='F') @ self.var['X'][:, k]
+            + cvx.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u), order='F') @ self.var['U'][:, k]
+            + cvx.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u), order='F') @ self.var['U'][:, k + 1]
             + self.par['S_bar'][:, k] * self.var['sigma']
             + self.par['z_bar'][:, k]
             + self.var['nu'][:, k]

Models/diffdrive_2d.py

@@ -76,8 +76,8 @@ def redimensionalize(self):
         self.lower_bound *= self.r_scale
         self.robot_radius *= self.r_scale
 
-        self.x_init = self.x_nondim(self.x_init)
-        self.x_final = self.x_nondim(self.x_final)
+        self.x_init = self.x_redim(self.x_init)
+        self.x_final = self.x_redim(self.x_final)
 
         for j in range(len(self.obstacles)):
             self.obstacles[j][0][0] *= self.r_scale
@@ -192,7 +192,7 @@ def get_constraints(self, X_v, U_v, X_last_p, U_last_p):
             p = obst[0]
             r = obst[1] + self.robot_radius
 
-            lhs = [(X_last_p[0:2, k] - p) / (cvx.norm((X_last_p[0:2, k] - p)) + 1e-6) * (X_v[0:2, k] - p)
+            lhs = [(X_last_p[0:2, k] - p) / (cvx.norm((X_last_p[0:2, k] - p)) + 1e-6) @ (X_v[0:2, k] - p)
                    for k in range(K)]
             constraints += [r - cvx.vstack(lhs) <= self.s_prime[j]]
         return constraints

Models/diffdrive_2d_plot.py

@@ -1,9 +1,8 @@
 import numpy as np
 import matplotlib.pyplot as plt
 from matplotlib import gridspec
-from matplotlib import collections as mc
-from mpl_toolkits import mplot3d
 from Models.diffdrive_2d import Model
+from utils import make_gif
 
 m = Model()
 
@@ -88,6 +87,8 @@ def plot(X_in, U_in, sigma_in):
     U = U_in
     sigma = sigma_in
 
+    make_gif(my_plot, figures_N)
+
     fig = plt.figure(figsize=(10, 12))
     my_plot(fig, figures_i)
     cid = fig.canvas.mpl_connect('key_press_event', key_press_event)

Models/quadcopter_demo.py

@@ -0,0 +1,198 @@
+import numpy as np
+import cvxpy as cvx
+import sympy as sp
+
+from global_parameters import K
+from FixedFinalTime.parameters import w_nu
+
+class Model:
+    """
+    The Quadcopter demo from the end of the original SCvx paper.
+    """
+
+    n_x = 6
+    n_u = 3
+
+    lambda_s = w_nu
+
+    theta_max = np.deg2rad(45) # max thrust tilt
+    tf = 3.0 # final time
+    t_f_guess = tf # required for solver
+    dt = tf / (K - 1) # dt
+
+    def __init__(self):
+        self.m = 0.3 # mass
+        self.k_D = 0.5 # condensed drag coeff
+        self.g_val = -9.81 # gravity!!!!!!!!!!!!
+        self.T_min = 1.0 # thrust min
+        self.T_max = 4.0 # thrust max
+        self.x_ic = np.array([0, 0,  0, 0, 0.5, 0]) # p_ic, v_ic
+        self.x_fc = np.array([0, 10, 0, 0, 0.5, 0]) # p_fc, v_fc
+        self.u_ic = np.array([-self.m*self.g_val, 0, 0])
+        self.u_fc = np.array([-self.m*self.g_val, 0, 0])
+
+        self.obstacles = [
+            {'p_obs': np.array([0.0, 3.0,  0.45]), 'R_obs': 1.0},
+            {'p_obs': np.array([0.0, 7.0, -0.45]), 'R_obs': 1.0},
+        ]
+        self.n_obs = len(self.obstacles)
+
+        # Constraints slack variable
+        self.s_prime = cvx.Variable((K, self.n_obs), nonneg=True)
+        # Convex Relaxation for Thrust Magnitude (K)
+        self.sigma = cvx.Variable(K, nonneg=True)
+
+        self.m_scale = self.m
+        self.r_scale = np.linalg.norm(self.x_fc[0:3] - self.x_ic[0:3])
+
+    def nondimensionalize(self):
+        """ nondimensionalize all parameters and boundaries """
+        self.m /= self.m_scale
+        self.k_D /= (self.m_scale/self.r_scale)
+        self.g_val /= self.r_scale
+        self.T_min = self.u_nondim(self.T_min)
+        self.T_max = self.u_nondim(self.T_max)
+        self.x_ic = self.x_nondim(self.x_ic)
+        self.x_fc = self.x_nondim(self.x_fc)
+        self.u_ic = self.u_nondim(self.u_ic)
+        self.u_fc = self.u_nondim(self.u_fc)
+        for j in range(self.n_obs):
+            self.obstacles[j]['p_obs'] /= self.r_scale
+            self.obstacles[j]['R_obs'] /= self.r_scale
+
+    def x_nondim(self, x):
+        """ nondimensionalize a single x row """
+        x /= self.r_scale
+        return x
+
+    def u_nondim(self, u):
+        """ nondimensionalize u, or in general any force in Newtons"""
+        return u / (self.m_scale * self.r_scale)
+
+    def redimensionalize(self):
+        self.m *= self.m_scale
+        self.k_D *= (self.m_scale/self.r_scale)
+        self.g_val *= self.r_scale
+        self.T_min = self.u_redim(self.T_min)
+        self.T_max = self.u_redim(self.T_max)
+        self.x_ic = self.x_redim(self.x_ic)
+        self.x_fc = self.x_redim(self.x_fc)
+        self.u_ic = self.u_redim(self.u_ic)
+        self.u_fc = self.u_redim(self.u_fc)
+        for j in range(self.n_obs):
+            self.obstacles[j]['p_obs'] *= self.r_scale
+            self.obstacles[j]['R_obs'] *= self.r_scale
+
+    def x_redim(self, x):
+        """ redimensionalize x, assumed to have the shape of a solution """
+        x *= self.r_scale
+        return x
+
+    def u_redim(self, u):
+        """ redimensionalize u """
+        return u * (self.m_scale * self.r_scale)
+
+    def get_equations(self):
+        """
+        :return: Functions to calculate A, B and f given state x and input u (dynamics only!)
+        """
+        x = sp.Matrix(sp.symbols('r_x, r_y, r_z, v_x, v_y, v_z', real=True))
+        u = sp.Matrix(sp.symbols('T_x, T_y, T_z', real=True))
+        v = sp.Matrix(x[3:6])
+
+        g_vec = sp.Matrix([self.g_val, 0, 0])
+        a = 1/self.m * u - self.k_D * v.norm(2)*v + g_vec
+        f = sp.simplify(sp.Matrix([v, a]))
+
+        A = sp.simplify(f.jacobian(x))
+        B = sp.simplify(f.jacobian(u))
+
+        f_func = sp.lambdify((x, u), f, 'numpy')
+        A_func = sp.lambdify((x, u), A, 'numpy')
+        B_func = sp.lambdify((x, u), B, 'numpy')
+
+        return f_func, A_func, B_func
+
+    def initialize_trajectory(self, X, U):
+        """
+        Initialize the trajectory 
+        X: [nx, K]
+        U: [nu, K]
+        """
+        for k in range(K):
+            alpha = k / (K - 1)
+            X[:, k] = (1 - alpha) * self.x_ic + alpha * self.x_fc
+            U[:, k] = (1 - alpha) * self.u_ic + alpha * self.u_fc
+
+        return X, U
+
+    def get_objective(self, X, U, X_old, U_old):
+        return cvx.Minimize(cvx.sum(self.sigma) * self.dt + self.lambda_s * cvx.sum(self.s_prime))
+
+    def get_constraints(self, X, U, X_old, U_old):
+        constraints = [
+            # Initial and Final State
+            X[:, 0] == self.x_ic,
+            X[:, -1] == self.x_fc,
+            U[:, 0] == self.u_ic,
+            U[:, -1] == self.u_fc,
+
+            # Planar motion
+            X[0, :] == 0,
+
+            # Thrust magnitude
+            cvx.norm(U, p=2, axis=0) <= self.sigma,
+
+            # Thrust limits
+            self.sigma >= self.T_min,
+            self.sigma <= self.T_max,
+
+            # Tilt angle
+            np.cos(self.theta_max)*self.sigma <= U[0, :],
+        ]
+
+        # Obstacles (non-convex constraints)
+        # s_obs = R_obs - ||p_old - p_obs||_2 <= 0
+        # S = [-(p_old-p_obs) / ||p_old-p_obs||_2, 0]
+        # Q = [0 0 0]
+        # Obstacles s(x_k) + S @ (x - x_old) <= s_prime
+        p_old = X_old[0:3, :]
+        p = X[0:3, :]
+
+        for j, obs in enumerate(self.obstacles):
+            p_obs = obs['p_obs']
+            R_obs = obs['R_obs']
+
+            diff = p_old - p_obs[:, None]
+            dist = cvx.norm(diff, p=2, axis=0)
+            s_val = R_obs - dist
+
+            S = -diff/dist
+            linear_term = cvx.sum(
+                cvx.multiply(S, p - p_old), 
+                axis=0
+            )
+
+            constraints += [s_val + linear_term <= self.s_prime[:, j]]
+        return constraints
+
+    def get_linear_cost(self):
+        """
+        Returns the unweighted (for some reason?) cost of everything except dynamics
+        which are handled by the solver
+        """
+        return np.sum(self.sigma.value) * self.dt + np.sum(self.s_prime.value)
+
+    def get_nonlinear_cost(self, X=None, U=None):
+        cost = np.sum(np.linalg.norm(U, ord=2, axis=0)) * self.dt
+        p = X[0:3, :]
+        for j, obs in enumerate(self.obstacles):
+            p_obs = obs['p_obs']
+            R_obs = obs['R_obs']
+
+            cost += np.sum(np.maximum(R_obs - np.linalg.norm(p - p_obs[:, None], ord=2, axis=0), 0))
+
+        return cost
+
+
+