trajopt 0.0.2

Trajectory optimization research and development framework

TrajOpt

TrajOpt is a self-contained Python library for multi-segment trajectory optimization using Sequential Convex Programming (SCP). The software is built to facilitate the design of models that can be effectively reused across different mission scenarios.

Features

• Configurable Missions: Configuring missions using existing models is fast and efficient due to the config.yaml structure.
• Simple Model Design Framework: Building new models only requires providing their continuous-time functions f_k(x, u, t, params, fcns).
• Robust Base Algorithm: The base AutoSCvx algorithm includes autotuning of penalty weights and second-order information for robust convergence on most problems with minimal hyperparameter tuning.
• Self Contained: The base algorithm for solving the nonconvex problems is fully written in this package. The only black-boxes we accept are calls to the convex solvers due to their guaranteed reliability and provable convergence.
• Easy Algorithm Design: Researchers can quickly design their own algorithms using the powerful modeling languages CVXPY and JAX.

Segments

A trajectory segment is defined by a set of (Costs, Constraints, Parameters, Functions):

\begin{aligned}
J_i &= \sum_j \int_{t_{I,i}}^{t_{F,i}} 
J_{r,j}(x_i,u_i,t,\mathrm{params}_i,\mathrm{fcns}_i)\,dt 
+ \sum_j J_{F,j}\!\left(
x_i(t_{F,i}),u_i(t_{F,i}),t_{F,i},
\mathrm{params}_i,\mathrm{fcns}_i
\right) \\
\mathbb{C}_i 
&= \left\{
(x_i(\cdot),u_i(\cdot),t_{I,i},t_{F,i})
\;\middle|\;
\begin{aligned}
x_i(t_{I,i}) &= x_{I,i}, \\
x_i(t_{F,i}) &= x_{F,i}, \\
\dot{x}_i &= f_i(x_i,u_i,t_i,\mathrm{params}_i,\mathrm{fcns}_i), \\
lb_{g,i} &\le g_i(x_i,u_i,t_i,\mathrm{params}_i,\mathrm{fcns}_i) \le ub_{g,i}, \\
lb_{g_{\mathrm{cvx}},i} &\le g_{\mathrm{cvx},i}(x_i,u_i,t_i,\mathrm{params}_i,\mathrm{fcns}_i) 
\le ub_{g_{\mathrm{cvx}},i}
\end{aligned}
\right\}. \\
\mathrm{params}_i 
&= \text{a set of user-defined variables} \\
\mathrm{fcns}_i 
&= \text{a set of user-defined functions of the form } 
f_k(x_i,u_i,t_i,\mathrm{params}_i,\mathrm{fcns}_i)
\end{aligned}

Trajectory

The trajectory optimal control problem (OCP) is defined by summing the cost contributions and enforcing the constraints from each segment:

\begin{aligned}
\underset{\{x_i,u_i,t_{I,i},t_{F,i}\}_{i=1}^{N}}{\mathrm{minimize}} 
\quad & \sum_{i=1}^{N} J_i \\
\mathrm{subject\;to} 
\quad & (z_1,\ldots,z_N) \in \mathbb{C}, \\
\mathrm{where} 
\quad \mathbb{C} 
&= \mathbb{C}_1 \times \mathbb{C}_2 \times \cdots \times \mathbb{C}_N
\end{aligned}

Augmented Optimal Control Problem

The OCP above is posed in continuous time with a free final time, which is not directly solvable. We transcribe it to a finite-dimensional problem using a global normalized time, a time-dilation control, and a multiple-shooting discretization.

Time is normalized to $\tau \in [0,1]$ on a fixed mesh,

0 = \tau_1 < \tau_2 < \cdots < \tau_N = 1,

and physical time is carried as a state through the time-dilation control $s(\tau) \triangleq \mathrm{d}t/\mathrm{d}\tau$. This lets us optimize the duration of each interval while keeping the mesh fixed. We collect the physical state and time into an augmented state, and the physical control and dilation into an augmented control:

y \triangleq \begin{bmatrix} x \\ t \end{bmatrix}, 
\qquad 
\nu \triangleq \begin{bmatrix} u \\ s \end{bmatrix}.

The dynamics in normalized time follow from the chain rule:

\frac{\mathrm{d}y}{\mathrm{d}\tau} = F(y,\nu) \triangleq 
\begin{bmatrix} s\, f(x,u,t,\mathrm{params},\mathrm{fcns}) \\ s \end{bmatrix}.

The augmented control is parameterized with a first-order hold (FOH) between nodes, and continuity is enforced through multiple-shooting defect constraints,

y_{k+1} - y_k - \int_{\tau_k}^{\tau_{k+1}} F\big(y(\tau),\nu(\tau)\big)\,\mathrm{d}\tau = 0, 
\qquad k = 1,\ldots,N-1,

while the nonconvex path constraints and boundary conditions are enforced at the nodes ${y_k,\nu_k}$. A free-final-time objective such as minimum time reduces to integrating the dilation, $\int_0^1 s(\tau),\mathrm{d}\tau$.

Stacking the nodal variables into a single decision vector $z = {y_k,\nu_k}_{k=1}^N$ yields the generic finite-dimensional nonconvex program solved by the algorithm:

\begin{aligned}
\underset{z \in \mathcal{Z}}{\mathrm{minimize}} \quad & J(z) \\
\mathrm{subject\;to} \quad & h(z) = 0, \\
                          & g(z) \le 0,
\end{aligned}

where $h(z)$ collects the dynamics defects, boundary conditions, and nonconvex equality path constraints; $g(z)$ collects the nonconvex inequality path constraints; and the convex state, control, and time-dilation sets are absorbed into $\mathcal{Z}$.

Trajectory Analyzer

The TrajectoryAnalyzer is the top-level entry point. It reads a mission config.yaml, builds the Trajectory (its segments, costs, and constraints) and the Method that solves it, and exposes a simple solve → analyze → plot workflow:

from trajopt.trajectory_analyzer import TrajectoryAnalyzer

traj = TrajectoryAnalyzer("config.yaml")
traj.solve()                 # run the SCP method
data = traj.analyze()        # propagate iterates through the nonlinear dynamics
traj.plot(data)              # re-dimensionalized plots

analyze() propagates each SCP iterate through the true nonlinear dynamics, re-dimensionalizes the result, and packages the data the plots consume. The analysis type set in the config selects what is run:

• standalone: solve and analyze a single mission.
• mc: Monte Carlo dispersion, re-solving while perturbing config values sampled per run.
• method_variation: solve the same mission with different method overrides and compare them.

Method

A method maps the nonconvex augmented OCP into a sequence of convex subproblems. The base algorithm is AutoSCvx: it relaxes the nonconvex constraints with virtual buffers $(p,q)$ and penalizes them, so every subproblem stays feasible while violations are driven to zero:

\begin{aligned}
\underset{z \in \mathcal{Z},\, p,\, q}{\mathrm{minimize}} \quad & 
J(z) + \tfrac{1}{2}p^\top W_h\, p + \tfrac{1}{2}q^\top W_g\, q + \lambda^\top p + \mu^\top q \\
\mathrm{subject\;to} \quad & h(z) = p, \\
                          & g(z) \le q, \quad q \ge 0.
\end{aligned}

The quadratic weights $(W_h, W_g)$ and the dual (linear) weights $(\lambda,\mu)$ are auto-tuned through primal–dual updates, which removes most of the manual penalty-weight tuning that SCP methods typically require. The Method assembles a single CVXPY subproblem from all segments and solves it repeatedly through the SCP loop.

Sequential Convex Programming

SCP solves the nonconvex problem by iterating on convex approximations built around the current reference trajectory $\bar z$. Each nonlinear mapping $c$ is replaced by its first-order linearization,

\ell_c(z,\bar z) \triangleq c(\bar z) + \left.\frac{\partial c}{\partial z}\right|_{\bar z}(z - \bar z).

Starting from an initial guess, each iteration:

1. Discretize & linearize the dynamics and constraints about $\bar z$.
2. Solve the resulting convex subproblem for a candidate step.
3. Line search on a merit function to accept a step length $\alpha$.
4. Update the reference, the virtual buffers, and the penalty weights $(W_h, W_g, \lambda, \mu)$.
5. Test convergence on the state change and constraint/defect residuals; otherwise repeat.

On convergence the buffers vanish, so the converged solution satisfies the original nonconvex problem to tolerance.

Core Developers

Skye Mceowen and Carlos Morales