From a7a4762cea2d996ea4dc13577722c1788439a683 Mon Sep 17 00:00:00 2001 From: Purushotham Mani Date: Mon, 16 Mar 2026 13:59:19 -0700 Subject: [PATCH] Fix min-snap QP solver robustness for ill-conditioned trajectories - Fix None vs False check in vehicle_rate_mpc.py for solve() return value - Add P matrix regularization (1e-8 * I) for strict positive-definiteness - Add solver fallback chain (clarabel > proxqp > osqp > scs) with per-solver kwargs - Print actual exceptions instead of bare except swallowing errors - Fixes extended_traj_track and other numerically challenging courses Made-with: Cursor --- src/figs/control/vehicle_rate_mpc.py | 2 +- src/figs/simulator.py | 7 ++- src/figs/tsplines/min_snap.py | 71 ++++++++++++++++++---------- 3 files changed, 51 insertions(+), 29 deletions(-) diff --git a/src/figs/control/vehicle_rate_mpc.py b/src/figs/control/vehicle_rate_mpc.py index a1efe69..e29de55 100644 --- a/src/figs/control/vehicle_rate_mpc.py +++ b/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.") diff --git a/src/figs/simulator.py b/src/figs/simulator.py index 14a39ab..282853b 100644 --- a/src/figs/simulator.py +++ b/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 \ No newline at end of file + return Tro,Xro,Uro,Iro,Tsol,Adv,Tco \ No newline at end of file diff --git a/src/figs/tsplines/min_snap.py b/src/figs/tsplines/min_snap.py index c933a6e..02c2b1a 100644 --- a/src/figs/tsplines/min_snap.py +++ b/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