Fix min-snap QP solver robustness for ill-conditioned trajectories

src/figs/control/vehicle_rate_mpc.py

@@ -102,7 +102,7 @@ def __init__(self,
 
         # Solve Padded Trajectory
         output = ms.solve(traj_config_pd)
-        if output is not False:
+        if output is not None:
             Tpi, CPi = output
         else:
             raise ValueError("Padded trajectory (for VehicleRateMPC) not feasible. Aborting.")

src/figs/simulator.py

@@ -174,7 +174,7 @@ def load_frame(self, frame:Union[str,dict]):
 
     def simulate(self,policy:Type[BaseController],
                  t0:float,tf:int,x0:np.ndarray,obj:Union[None,np.ndarray]=None
-                 ) -> Tuple[np.ndarray,np.ndarray,np.ndarray,np.ndarray,np.ndarray,np.ndarray]:
+                 ) -> Tuple[np.ndarray,np.ndarray,np.ndarray,np.ndarray,np.ndarray,np.ndarray,np.ndarray]:
         """
         Simulates the flight.
 
@@ -216,6 +216,7 @@ def simulate(self,policy:Type[BaseController],
         # Rollout Variables
         Tro,Xro,Uro = np.zeros(Nctl+1),np.zeros((nx,Nctl+1)),np.zeros((nu,Nctl))
         Iro = np.zeros((Nctl,height,width,channels),dtype=np.uint8)
+        Tco = np.zeros((Nctl,4,4))
         Xro[:,0] = x0
 
         # Diagnostics Variables
@@ -241,6 +242,7 @@ def simulate(self,policy:Type[BaseController],
                 # Get current image
                 Tb2w = th.xv_to_T(xcr)
                 T_c2w = Tb2w@T_c2b
+                T_c2g = self.gsplat.T_w2g@T_c2w@self.gsplat.T_w2g
                 icr = self.gsplat.render_rgb(camera,T_c2w)
 
                 # Add sensor noise and syncronize estimated state
@@ -278,6 +280,7 @@ def simulate(self,policy:Type[BaseController],
                 k = i//n_sim2ctl
 
                 Iro[k,:,:,:] = icr
+                Tco[k,:,:] = T_c2g
                 Tro[k] = tcr
                 Xro[:,k+1] = xcr
                 Uro[:,k] = ucm
@@ -287,4 +290,4 @@ def simulate(self,policy:Type[BaseController],
         # Log final time
         Tro[Nctl] = t0+Nsim/hz_sim
 
-        return Tro,Xro,Uro,Iro,Tsol,Adv
+        return Tro,Xro,Uro,Iro,Tsol,Adv,Tco

src/figs/tsplines/min_snap.py

@@ -39,33 +39,52 @@ def solve(fout_wps:Dict[str,Union[int,Tuple[np.float64,np.ndarray]]],Natt=5) ->
     P,q = Pq_gen(Tp,Nco)                                           # Min Snap Cost
     A,b = Ab_gen(Tp,FOp,Nco)                                       # Keyframe Constraints
 
+    # Regularize P to ensure strict positive-definiteness (helps ill-conditioned problems)
+    n = P.shape[0]
+    P += 1e-8 * np.eye(n)
+
     # Convert to Sparse
-    P = sps.csc_matrix(P)
-    A = sps.csc_matrix(A)
-
-    # Solve QP to get coefficient solution (spline variables)
-    for attempt in range(Natt):
-        try:
-            sigma = qpsolvers.solve_qp(P,q,G=None,h=None,A=A,b=b,
-                                    solver="osqp")       # Solve QP
-            SM = sigma.reshape((-1,Nfo,Nco))                                # Reshape to match keyframes
-
-            Nsm = SM.shape[0]
-            TT = np.zeros((Nsm,Nco))
-            for i in range(0,Nsm):
-                TT[i,:] = np.linspace(Tp[i],Tp[i+1],Nco)
-
-            Tps = np.array(Tp)
-            CPs = SM2CP(SM,TT,Nco)
-
-            return Tps,CPs
-
-        except:
-            print(f"Minimum Snap Trajectory Solve Failed (Attempt {attempt + 1}) failed. Retrying...")
-            if attempt == Natt - 1:
-                print("All attempts failed.")
-
-                return None
+    P_sp = sps.csc_matrix(P)
+    A_sp = sps.csc_matrix(A)
+
+    # Solver-specific keyword arguments
+    solver_kwargs = {
+        "clarabel": {},
+        "proxqp":   {"max_iter": 20000, "eps_abs": 1e-7},
+        "osqp":     {"max_iter": 20000, "eps_abs": 1e-7, "eps_rel": 1e-7, "polish": True},
+        "scs":      {"max_iters": 20000, "eps_abs": 1e-7, "eps_rel": 1e-7},
+    }
+
+    solver_priority = ["clarabel", "proxqp", "osqp", "scs"]
+    solvers_to_try = [s for s in solver_priority if s in qpsolvers.available_solvers]
+
+    for solver_name in solvers_to_try:
+        kwargs = solver_kwargs.get(solver_name, {})
+        for attempt in range(Natt):
+            try:
+                sigma = qpsolvers.solve_qp(P_sp,q,G=None,h=None,A=A_sp,b=b,
+                                        solver=solver_name, **kwargs)
+                if sigma is None:
+                    raise RuntimeError(f"{solver_name} returned None")
+                SM = sigma.reshape((-1,Nfo,Nco))
+
+                Nsm = SM.shape[0]
+                TT = np.zeros((Nsm,Nco))
+                for i in range(0,Nsm):
+                    TT[i,:] = np.linspace(Tp[i],Tp[i+1],Nco)
+
+                Tps = np.array(Tp)
+                CPs = SM2CP(SM,TT,Nco)
+
+                return Tps,CPs
+
+            except Exception as e:
+                print(f"Min-snap solve failed ({solver_name}, attempt {attempt + 1}/{Natt}): {e}")
+
+        print(f"All {Natt} attempts exhausted for solver '{solver_name}', trying next...")
+
+    print("All solvers failed.")
+    return None
 
 def Pq_gen(Tp:List[np.float64],Nco:int) -> Tuple[np.ndarray,np.ndarray]:
     # Unpack some stuff