traj_opt

Introduction

Polynomial Spline (MINCO etc.) trajectory optimization. It generates a trajectory from the start state to the end state. It typically works as the back end of a navigation planner, which uses a path searcher (A* etc.) as the front end.

This is a pure c++ project independent of ROS.

Author: Peixuan Shu (shupeixuan@qq.com), Beihang University, China

Date: Nov 2025

Date: June 2026

Header files

• include/traj_opt/minco_optimizer.hpp: header-only trajectory optimizer (MINCO) for 2D/3D, independent of ROS. Provides the high-level API such as optimize_v1() (free total time) and optimize_v2() (fixed total time).

• include/traj_opt/gcopter/minco.hpp: core MINCO implementation and utilities used by the optimizer.

• include/traj_opt/gcopter/trajectory.hpp: trajectory data structures and helpers for conversion/serialization.

• include/traj_opt/gcopter/lbfgs.hpp: L-BFGS optimizer wrapper used for unconstrained optimization steps.

• include/traj_opt/gcopter/root_finder.hpp: root-finding utilities for boundary/consistency checks.

• include/traj_opt/gcopter/flatness.hpp: differential-flatness related helpers for system modeling.

• include/traj_opt/gcopter/banded_system.hpp: banded linear system solver used in internal linear algebra.

Key APIs

• optimize_v1(): Optimize without a fixed total time. Cost = time cost (weighted total time) + energy cost. Optimization variables are inner waypoints, terminal PVA (position, velocity, acceleration), and individual segment times (each segment time optimized). Use this when you want to trade trajectory time versus energy and allow time allocation per segment.

• optimize_v2(): Optimize with a fixed total time. Cost = terminal position error + energy cost. Optimization variables are inner waypoints, terminal PVA, and time allocation (N-1 variables) under a fixed total duration. Use this when the overall trajectory duration must be constrained.

• optimize_v3(): Optimize without a fixed total time but include a terminal position cost. Cost = energy cost + time cost + terminal position error. Optimization variables are inner waypoints, terminal PVA (tail position is directly optimized), and individual segment times. Use this when you need to balance time minimization and terminal accuracy simultaneously.

Library

traj_opt::traj_opt: an interface library (header-only) that exposes the optimizer headers for easy inclusion in downstream projects.

Build and Test

Build as a standalone project:

mkdir build
cd build
cmake .. -DBUILD_TESTING=ON # set BUILD_TESTING=ON to build test exetuables
make

Or you can use the script:

./compile.sh # make
./compile.sh -c # clean and make

test:

# 必须进入plot_minco_result.py所在文件夹，结果才会自动正确绘制，否则需要将生成的json结果手动粘贴到plot_minco_result.py所在文件夹
cd traj_opt/test

./../build/test/test_minco_optimizer_v1 # 3d测试，结果导出在当前文件夹json，并python绘图
./../build/test/test_minco_optimizer_v1 2d # 2d测试

./../build/test/test_minco_optimizer_v2 # 3d测试，结果导出在当前文件夹json，并python绘图
./../build/test/test_minco_optimizer_v2 2d # 2d测试

python plot_minco_result.py minco_result_2d.json 2d # 2d结果绘制
python plot_minco_result.py minco_result_3d.json 3d # 3d结果绘制

Use this lib in your project

The example nodes are in test folder, which you can refer to.

Method 1: Build in your project (recommended)

If you want to use this library, it is strongly recommended to copy this folder under your project, and link this project in your own project using add_subdirectory(${path to this project}) in your CMakeLists.txt.

your_project/
├── CMakeLists.txt              # Your CMakeLists
│── traj_opt/                   # put this lib here
│   ├── CMakeLists.txt
│   ├── xxx

In your CMakeLists.txt, add:

add_subdirectory(traj_opt) # relative path to this project

add_executable(your_target 
    your_target.cpp
)
target_link_libraries(your_target PRIVATE 
    traj_opt::traj_opt  # link this lib
)

Method 2: Install and find_package

In case you just want to install this project and use find_package in your own project (but you should specify the install path by yourself, which is not recommended):

cd traj_opt/
mkdir build
cd build
cmake .. -DCMAKE_INSTALL_PREFIX=../install  # install to the local install folder
make install # will install the lib in traj_opt/install folder

Find package in your own project CMakeLists.txt:

find_package(traj_opt REQUIRED PATHS /path/to/traj_opt/install)

Or declare the path in cmake command:

cmake .. -DCMAKE_PREFIX_PATH=/path/to/traj_opt/install

Then you can simply find_package in your CMakeLists.txt:

find_package(traj_opt REQUIRED)

Then you can link the library of this project to your target in your CMakeLists.txt:

add_executable(your_target 
    your_target.cpp
)
target_link_libraries(your_target PRIVATE 
    traj_opt::traj_opt  # link this lib
)