plannersettings#

Variables

static constexpr int MAX_OUTER_PASSES = 2#

struct InitTrajConfig#

#include <plannersettings.h>

Initial trajectory configuration.

Settings controlling how the initial control/state trajectory is generated before running the ALTRO optimization. The initialization can strongly affect convergence speed and robustness of the solver.

Param initcontroller:: Integer flag selecting which controller or strategy is used to generate the initial trajectory guess. Typical uses include zero-input rollout, PD control, or a custom warm-start controller.

Public Members

int initcontroller = 0#

bool pd_goal_rate_ff_enabled = false#

Goal-rate feedforward toggle (ported from OldPlanner smartbdot).

When true and initcontroller == 3, the PD warm-start feeds forward a desired body rate ω_des computed by finite-differencing the goal trajectory between consecutive knots (OldPlanner wkdes), adding a -kd·(ω - ω_des) damping term toward the goal rate instead of damping ω toward zero. Default off preserves the rate-to-zero behavior.

struct DisturbanceConfig#

#include <plannersettings.h>

Disturbance modeling configuration.

Enables and parameterizes disturbance models that the optimizer accounts for during trajectory optimization. These disturbances are incorporated into the system dynamics or cost to improve robustness and realism.

Enabled disturbances may include:

Aerodynamic drag
Propulsion torques
Solar radiation pressure (SRP)
Gravity-gradient torque
Generic external torques
Residual magnetic dipole effects

Coefficient vectors represent linear scaling parameters used in disturbance models. If enabled, they are applied during forward rollout and linearization.

Param plan_for_aero:: Enable aerodynamic disturbance model
Param plan_for_prop:: Enable propulsion torque disturbance
Param plan_for_srp:: Enable solar radiation pressure disturbance
Param plan_for_gg:: Enable gravity-gradient disturbance
Param plan_for_gendist:: Enable generic disturbance torque
Param plan_for_resdipole:: Enable residual dipole torque
Param srp_coeff:: Solar radiation pressure coefficients
Param drag_coeff:: Aerodynamic drag coefficients
Param coeff_N:: Number of disturbance coefficients used in estimation
Param res_dipole:: Residual dipole torque vector
Param prop_torque:: Constant propulsion torque vector
Param gendist_torque:: Generic disturbance torque vector
Param J_est:: Estimated inertia matrix used for disturbance modeling

Public Members

bool plan_for_aero = false#

bool plan_for_prop = false#

bool plan_for_srp = false#

bool plan_for_gg = false#

bool plan_for_gendist = false#

bool plan_for_resdipole = false#

Eigen::Vector3d srp_coeff = Eigen::Vector3d::Zero()#

Eigen::Vector3d drag_coeff = Eigen::Vector3d::Zero()#

int coeff_N = 0#

Eigen::Vector3d res_dipole = Eigen::Vector3d::Zero()#

Eigen::Vector3d prop_torque = Eigen::Vector3d::Zero()#

Eigen::Vector3d gendist_torque = Eigen::Vector3d::Zero()#

Eigen::Matrix3d J_est = Eigen::Matrix3d::Zero()#

struct ConstraintConfig#

#include <plannersettings.h>

Constraint configuration.

Defines hard and soft constraints enforced during optimization. These are typically handled via the augmented Lagrangian framework inside ALTRO.

Constraints may include actuator limits, angular velocity bounds, and environmental pointing constraints (e.g., sun avoidance).

Param control_limit_scale:: Scaling factor applied to actuator limits
Param u_max:: Maximum control input vector
Param wmax:: Maximum allowable angular velocity magnitude (rad/s)
Param sun_limit_angle:: Minimum allowed angle to sun direction (rad)
Param rw_stic_torque_theta:: Torque floor θ ∈ [0, 1] of the per-RW stiction torque-floor constraint c = θ − |u|/u_lim − |h|/h_c ≤ 0 with u_lim the (control_limit_scale-scaled) RW torque limit and h_c = rw_stic_band_mult · h_lim, where h_lim = rw_momentum_limit_scale · h_max is the enforced (scaled) momentum limit. Semantics: at h = 0 the wheel must hold at least θ·u_lim of torque; the requirement fades linearly and vanishes for |h| ≥ θ·h_c (c ≤ 0 with u = 0 iff |h| ≥ θ·h_c). Default 0.0 disables the floor — the row is then always satisfied, behavior-identical to the historical dead −(u·h)² row. Recommended opt-in value: 0.9. High-torque zero crossings satisfy the row throughout (|u|/u_lim ≥ θ alone suffices); chatter is handled by the dwell cost, not this constraint. ALIGNMENT RULE with bias-momentum parking (CostConfig::RWh_desat_mult recipe, parked at h* = h_stic/2): a parked wheel must satisfy the floor at zero torque, i.e. θ·h_c ≤ h*. Equivalently rw_stic_torque_theta · rw_stic_band_mult · rw_momentum_limit_scale ≤ h* / h_max. With defaults (rw_momentum_limit_scale = 1, so h_c = 0.005·h_max, θ = 0.9 when enabled, h_stic = 0.01·h_max, h* = 0.005·h_max): θ·h_c = 0.0045·h_max < h* = 0.005·h_max (11% margin). A margin config (rw_momentum_limit_scale < 1) only tightens this inequality.
Param rw_stic_band_mult:: Momentum half-width of the torque-floor band as a fraction of the enforced momentum limit h_lim = rw_momentum_limit_scale · h_max: h_c = rw_stic_band_mult · h_lim. Default 0.005 — half of the default cost-side stiction band (CostConfig::RWh_stiction_mult = 0.01). Deliberately independent of CostConfig; see the alignment rule above.

Public Members

double control_limit_scale = 0.75#

double rw_momentum_limit_scale = 1.0#: Margin on the hard RW-momentum constraint: |h| <= rw_momentum_limit_scale * h_max. <1 leaves slack below saturation (the momentum analog of control_limit_scale for torque). Default 1.0 = bind exactly at h_max (no margin).

Eigen::Matrix<double, Eigen::Dynamic, 1, 0, saltro::limits::MAX_CTRL_DIM, 1> u_max#: Stack-allocated bounded control limit vector (no heap allocation).

double wmax = 20.0 * M_PI / 180.0#

double sun_limit_angle = 20.0 * M_PI / 180.0#

double rw_stic_torque_theta = 0.0#

double rw_stic_band_mult = 0.005#

struct CostConfig#

#include <plannersettings.h>

Cost function configuration.

Defines the weights and structure of the stage and terminal cost functions used in the ALTRO optimization. Costs typically penalize:

Orientation error
Angular velocity error
Control effort
Reaction wheel momentum
Actuator stiction or saturation

Stage costs are applied at each timestep:

\[ \ell_k = w_\theta e_\theta^2 + w_\omega e_\omega^2 + w_u \|\mathbf{u}_k\|^2 \]

Terminal costs are applied at the final timestep:

\[ \ell_N = w_\theta^N e_\theta^2 + w_\omega^N e_\omega^2 \]

Param angle:

Weight on orientation error

Param ang_vel:

Weight on angular velocity error

Param ang_vel_mag:

Weight on angular velocity magnitude

Param ang_vel_err_dir:

Weight on angular velocity direction error

Param control_mult:

Global multiplier on control cost

Param mtq_control_weight:

Magnetorquer control cost weight

Param rw_control_weight:

Reaction wheel control cost weight

Param magic_control_weight:

Additional actuator control weight

Param rw_AM_weight:

Reaction wheel angular momentum penalty

Param rw_stic_weight:

Reaction wheel stiction penalty

Param RWh_knee_frac:

Knee of the momentum soft cost, as a fraction of h_max (default 0.5: free* region below 50% of saturation, steep quadratic above it; *free up to the desat term below). The hard ceiling does NOT live in this cost — enforce it with ConstraintConfig::rw_momentum_limit_scale (the AL momentum constraint), tuned per-family via AugLagConfig. Cost shapes, constraint enforces.

Param RWh_desat_mult:

Gentle desaturation quadratic applied over the whole momentum range: 0.5*rw_AM_weight*RWh_desat_mult*(h/h_max)^2. Default 0.05, NOT 0: a perfectly flat free band (zero value, gradient AND curvature below the knee) lets the first inner solve exploit the wheel and over-commit before the AL penalties ramp, which was observed to grind the outer loop to MaxOuterIterations on the 3MTQ+1RW vector-slew case. A small desat keeps the wheel direction informed without strangling it. Together with the stiction cost this implements bias-momentum parking: the net potential has stable minima at h* = (w_stic/h_stic) / (rw_AM_weight*desat/h_max^2 + w_stic/h_stic^2), with h_stic = RWh_stiction_mult*h_max, so the wheel idles at a bias speed instead of crossing zero. The special case rw_stic_weight = rw_AM_weight*RWh_desat_mult*(h_stic/h_max)^2 parks at h* = h_stic/2. All weights stay independent — this is a tuning recipe, not a coupling.

Param RWh_stiction_mult:

Stiction band as a fraction of h_max: below RWh_stiction_mult*h_max the kinked-quadratic stiction cost pushes |h| away from zero (wheels at rest may not restart). Deliberately kinked at h = 0: a smoothed peak would inject genuine negative curvature and create a zero-gradient point a wheel can sit on; the kink contributes no curvature to the quadratic model, and the subgradient at exactly h = 0 is 0.

Param angle_N:

Terminal orientation weight

Param ang_vel_N:

Terminal angular velocity weight

Param ang_vel_mag_N:

Terminal angular velocity magnitude weight

Param ang_vel_err_dir_N:

Terminal angular velocity direction weight

Param ang_cost_func_type:

Type of orientation error cost function used, as a shape f(d) of the inner alignment scalar d — in quaternion mode d = q_goal . q (post hemisphere-alignment, so d in [0, 1]); in vector-pointing mode d = c = bs . R(q)^T r_hat (cosine of the boresight-to-target angle):

0: 1 - d (linear)
1: 0.5 * (1 - d)^2 (quadratic, convex)
3: 0.5 * acos(d)^2 (squared angle, Taylor-protected at d = +1)
5: pseudo-Huber in the angle theta = acos(d): delta^2*(sqrt(1 + (theta/delta)^2) - 1), delta = ang_cost_huber_delta. Quadratic near the goal (matches type 3’s 0.5*theta^2 to O((theta/delta)^2)), linear at large angle (bounded urgency: the demanded descent rate saturates at delta instead of growing with the error, so large slews don’t demand instant acquisition), with a non-vanishing antipodal escape gradient (~delta), unlike type 0’s plateau. Taylor-protected at d = +1 and antipode-clamped at d = -1 like type 3. Default is 3 (the preferred well-conditioned shape). Type 2 (acos(d), raw angle) was REMOVED: it is concave (f’’ < 0, anti-PSD under Gauss-Newton) and has a genuine gradient singularity at BOTH poles, including perfect alignment (f’ = -1/sqrt(1-d^2) -> -inf at d = +1, an unremovable singularity unlike type 3’s). Migrate to type 3 (same acos family, convex-usable and Taylor-protected at the aligned pole) or type 0 for a linear-in-d cost. Type 4 ((1 - d)^2) was REMOVED: it is exactly type 1 with the constant 2 absorbed into the angle weight. Migrate by using type 1 with doubled angle weights (2 * 0.5*(1-d)^2 == (1-d)^2).

Param use_cost_hess:

If true, use analytic Hessians of cost

Public Functions

inline void setTerminalEmphasis(double k)#: Scale all terminal weights by k, preserving ratios with their stage counterparts. k=1 matches stage; k=100 is strong terminal emphasis.

Public Members

double angle = 1e3#

double ang_vel = 1e4#

double ang_vel_mag = 0.0#

double ang_vel_err_dir = 0.0#

double control_mult = 1.0#

double ang_vel_roll_ratio = 1.0#: Roll-axis weight fraction for the axis-aware ω cost in vector-pointing mode (0 < ratio ≤ 1). 1.0 reduces W_ω to uniform c.ang_vel · I, matching the current isotropic cost. Smaller values down-weight rotation about the boresight, freeing the optimizer to spend control on the 2-DOF off-axis pointing error. Ignored in quaternion-goal mode (all 3 DOF are constrained there). Default is current behavior.

double ang_vel_err_dir_ratio = 0.0#

PSD-fraction knob β ∈ [0, 2) for the Lyapunov α · err_dir · ω crossterm. Realized scale is α = β · √(c.angle · λ_min(W_ω)), keeping the (q_e_v, ω_e) block-quadratic PSD by construction. 0 (default) disables the crossterm entirely. If ang_vel_err_dir is set nonzero, the back-compat path overrides α with that raw value and ignores this ratio.

Derivation (Schur complement bound): The total (q_e_v, ω_e) block-quadratic with the angle cost, ω cost, and this crossterm is

½ [q_e_v; ω_e]^T · [ w_ang · I ½α · D^T ] · [q_e_v] [ ½α · D W_ω ] [ω_e]

where D = ∂err_dir/∂q_e_v and W_ω is the ω cost matrix (w_av · I in quat mode; w_av · (roll·bs·bs^T + (I − bs·bs^T)) in vec mode with ang_vel_roll_ratio reduction). The Schur complement of the bottom-right block is PSD iff

w_ang · I − ¼ α² · D^T · W_ω^{-1} · D ≽ 0 ⟺ α² · λ_max(D^T · W_ω^{-1} · D) ≤ 4 · w_ang.

Since err_dir is a cross-product of unit vectors, ‖D‖ ≤ 1, so λ_max(D^T · W_ω^{-1} · D) ≤ 1 / λ_min(W_ω). The conservative bound that always holds is therefore

α ≤ 2 · √(w_ang · λ_min(W_ω)).

Setting α = β · √(w_ang · λ_min(W_ω)) with β ∈ [0, 2) is PSD by construction. FD-tested in test_satellite_cost_omega_ff.py at β=0.5.

double mtq_control_weight = 1e3#

double rw_control_weight = 1e8#

double magic_control_weight = 0.0001#

double rw_AM_weight = 1e4#

double rw_stic_weight = 1.0#

double RWh_stiction_mult = 0.01#

double RWh_knee_frac = 0.5#

double RWh_desat_mult = 0.05#

double angle_N = 1e4#: Terminal weights. Principle: preserve the stage ratios. If you set angle_N high without matching ang_vel_N, the optimizer chases the target angle at max torque with no penalty for arriving at high ω. Use setTerminalEmphasis(k) to scale all terminal weights uniformly instead of editing fields individually.

double ang_vel_N = 1e5#

double ang_vel_mag_N = 0.0#

double ang_vel_err_dir_N = 0.0#

int ang_cost_func_type = 3#

double ang_cost_huber_delta = 0.35#: Pseudo-Huber crossover angle δ (rad) for ang_cost_func_type = 5: the pointing-angle scale at which the type-5 cost transitions from quadratic (θ ≪ δ, matches type 3’s ½θ²) to linear (θ ≫ δ, slope saturates at δ — bounded urgency on large slews). Default 0.35 rad ≈ 20°. Must be finite and > 0 (validated); ignored by the other shapes. Smaller δ ⇒ urgency saturates earlier (gentler large-angle demands); δ → ∞ recovers type 3 exactly.

bool use_cost_hess = false#

bool cost_hess_gauss_newton = false#

Gauss-Newton mode for the angle-cost (q,q) Hessian block. When true, drop the second-order chain-rule term f(c)·d²c/dq²(which can be indefinite in vec mode wherec = bs·R^T·r̂is degree-2 in q). Keep the PwA manifold-curvature correction−grad_dot_q · I_4— it's the sphere-tangent projection and is PSD whenf’·c < 0`, which holds for our cost shapes in the aligned hemisphere.

Effect by mode:

Vec mode: drops f'·d²c/dq². Empirically improves convergence dramatically (PE_fin 6-22° → 0.2-6.6° on baseline scenarios).
Quat mode: has no f'·d²d/dq² term (d = q_g·q is linear in q), so this flag is a no-op. Default (false) preserves the current full-Hessian behavior.

double gn_curvature_max = 0.0#

Opt-in curvature cap for the Gauss-Newton vec-pointing angle Hessian (only active when cost_hess_gauss_newton == true). Default 0.0 = disabled: exact GN behavior, zero change.

Why. In vec mode the assembled GN angle-Hessian q-block is the rank-1 PSD term w_ang · f’(c) · dc·dcᵀ (c = bs·Rᵀ·r̂). Because it’s rank-1, its single nonzero eigenvalue is *exactly* λ = w_ang · f’(c) · |dc|². For the ½·acos² shape (ang_cost_func_type == 3) this eigenvalue diverges like 1/sin θ toward the antipode (θ → 180°): measured ≈ 71·w_ang at θ=170°, ≈ 7.2e5·w_ang at θ=179.999°. (The full exact Hessian is not stiff there — its divergence is the negative azimuthal cone, which regularization already handles.) The unbounded GN stiffness collapses step sizes and stalls near-antipodal slews.

What. When > 0 and GN mode is active, the assembled rank-1 eigenvalue is clamped to gn_curvature_max · w_ang by nonnegative scaling of the (PSD) rank-1 term — so PSD-ness and the gradient are untouched, and below the cap behavior is bit-identical. The knob is in units of the angle weight: with the standard boresight parametrization |dc|² = 4 sin²θ, so types 0/1 satisfy f’·|dc|² ≤ 4` everywhere and a cap ≥ 4 leaves them exact.

Cap ⇔ angle. A cap C bounds the effective stiffness; type-3 GN is clamped for all θ inside the angle where f’(cos θ)·|dc|² = C (i.e.4·f’’(cos θ)·sin²θ = C`). Larger C ⇒ cap engages only closer to the antipode. Typical useful values: C ≈ 10–50.

struct AugLagConfig#

#include <plannersettings.h>

Augmented Lagrangian configuration.

Controls the outer-loop constraint handling in ALTRO using an augmented Lagrangian method. Constraints are enforced by iteratively updating penalty parameters and Lagrange multipliers:

\[ \mathcal{L}(x,u,\lambda,\mu) = J(x,u) + \lambda^\top g(x,u) + \frac{\mu}{2}\|g(x,u)\|^2 \]

Param max_outer_iters:: Maximum number of outer iterations
Param lag_mult_init:: Initial Lagrange multiplier value
Param lag_mult_max:: Maximum allowed Lagrange multiplier
Param penalty_init:: Initial penalty parameter
Param penalty_max:: Maximum penalty parameter
Param penalty_scale:: Multiplicative increase per iteration
Param constraint_tol:: Constraint satisfaction tolerance
Param total_cost_tol:: Total cost convergence tolerance

Public Members

int max_outer_iters = 30#

double lag_mult_init = 0.0#

double lag_mult_max = 1e20#

double penalty_init = 1e-1#

double penalty_max = 1e16#

double penalty_scale = 10.0#

double constraint_tol = 0.002#

double total_cost_tol = 1e-2#

struct ILQRConfig#

#include <plannersettings.h>

iLQR inner-loop configuration.

Settings for the inner iterative LQR solver used by ALTRO to optimize trajectories between augmented Lagrangian updates.

The solver iteratively linearizes dynamics and quadratizes costs:

\[ \mathbf{x}_{k+1} \approx f(\mathbf{x}_k,\mathbf{u}_k) \]

Param max_iters:: Maximum number of iLQR iterations
Param grad_tol:: Gradient norm convergence tolerance
Param cost_tol:: Cost improvement tolerance
Param z_count_lim:: Maximum number of zero-improvement steps
Param max_cost:: Abort if cost exceeds this value
Param state_bound:: Maximum allowed state magnitude

Public Members

int max_iters = 250#

double grad_tol = 0.0#

double cost_tol = 1e-1#

int z_count_lim = 10#

double max_cost = 1e40#

double state_bound = 10.0#

bool ls_strict_decrease = false#: Require strict cost decrease in line search (J_new < J_prev). Original ALTRO behavior; prevents accepting cost-increasing steps.

bool conjunctive_convergence = false#: Require BOTH grad_tol AND cost_tol for convergence (true), or allow either alone (false). Original ALTRO uses conjunctive.

bool persistent_reg = false#: Persist regularization across iLQR iterations (true = ALTRO-style), or reset to reg_init each iteration (false = legacy).

struct RegularizationConfig#

#include <plannersettings.h>

Regularization configuration.

Controls Levenberg–Marquardt-style regularization used to ensure numerical stability when solving Riccati equations and backward passes.

Regularization modifies the Hessian:

\[ Q_{uu} \leftarrow Q_{uu} + \rho I \]

Param reg_init:: Initial regularization value
Param reg_min:: Minimum regularization
Param reg_max:: Maximum regularization
Param reg_scale:: Multiplicative scaling factor
Param reg_bump:: Factor used when increasing regularization
Param reg_min_cond:: Minimum condition threshold
Param rand_add_ratio:: Random diagonal perturbation ratio
Param use_dynamics_hess:: Use second derivatives of dynamics
Param use_constraint_hess:: Use second derivatives of constraints
Param psd_clamp_lxx:: TESTING/DIAGNOSTIC aid only — NOT recommended for production. When true, the backward pass eigen-clamps each stage cost Hessian lxx to PSD (negative eigenvalues zeroed). Useful to prove that an indefinite cost Hessian is the culprit when a solve fails, but it runs an eigendecomposition per knot (slow) and masks model problems rather than fixing them. Default false; when false the backward pass is bitwise-identical to the unflagged code.

Public Members

double reg_init = 1e-2#

double reg_min = 1e-8#

double reg_max = 1e30#

double reg_scale = 1.6#

double reg_bump = 10.0#

int reg_min_cond = 2#

double rand_add_ratio = 0.0#

bool use_dynamics_hess = false#

bool use_constraint_hess = false#

bool psd_clip_quu_ddp = false#

bool psd_clamp_lxx = false#

struct LineSearchConfig#

#include <plannersettings.h>

Line search configuration.

Parameters for backtracking line search used during forward rollout. The step size \(\alpha\) is reduced until sufficient decrease is achieved:

\[ J(\alpha) \le J(0) + \beta_1 \alpha \nabla J^\top d \]

Param max_iters:: Maximum line search iterations
Param beta1:: Sufficient decrease parameter
Param beta2:: Maximum allowable cost increase factor

Public Members

int max_iters = 20#

double beta1 = 1e-10#

double beta2 = 500.0#

struct PassConfig#

#include <plannersettings.h>

Per-pass optimization configuration.

Each ALTRO outer pass may use different cost weights, regularization, or time discretization. This allows coarse-to-fine optimization strategies.

Param cost:: Cost configuration for this pass
Param auglag:: Augmented Lagrangian configuration
Param ilqr:: iLQR configuration
Param reg:: Regularization configuration
Param linesearch:: Line search configuration
Param dt:: Timestep used for discretization

Public Members

CostConfig cost#

AugLagConfig auglag#

ILQRConfig ilqr#

RegularizationConfig reg#

LineSearchConfig linesearch#

double dt = 1.0#

struct TVLQRSettings#

#include <plannersettings.h>

TVLQR gain-generation configuration.

Controls the backward-pass discretization and chunking window used when generating tracking gains from an optimized trajectory.

Param dt_tvlqr:: Fixed TVLQR gain discretization step. SALTRO currently uses the planner pass dt and keeps this at 0.0.
Param tvlqr_len:: Chunk duration in seconds for gain computation.
Param tvlqr_overlap:: Overlap duration in seconds between consecutive chunks.

Public Functions