KO | EN

GitLite

Minimalist Open Source Repository Search Engine

gitlite — search

#dsh #dsh-plugin #ai-agent #developer-tools #codex #typescript #cli #deepseek-harness #claude-code #ai-agents #mcp #rust

IPN_MPC

★ 159 Open GitHub ↗

Joinied Positioning and Control Model Based on Factor Graph Optimization (FGO)

// repository documentation

Was this content helpful?

★ 0 (0 ratings)

# IPN_MPC

IPN_MPC is a C++17 research implementation of factor-graph model predictive control for
quadrotor navigation. It combines state estimation, nonlinear dynamics, trajectory tracking,
control limits, and obstacle avoidance in a GTSAM factor graph.

The current codebase targets GTSAM 4.3 and uses Pangolin for interactive 3D visualization.

## Features

- Joint positioning and model predictive control with factor-graph optimization.
- Quadrotor dynamics with thrust, body-rate commands, drag, and simulated measurements.
- Circular, figure-eight, and back-and-forth reference trajectories.
- Moving spherical and cylindrical obstacle simulation.
- Soft Control Barrier Function (CBF) obstacle factors.
- Optional experimental hard-inequality SQP using GTSAM's active-set QP solver.
- A discrete-time safety boundary as a final collision guard.
- Timestamped, severity-based application logging.
- Automated GTSAM compatibility and hard-constraint QP tests.

## Requirements

- CMake 3.16 or newer
- A C++17 compiler
- Eigen3
- GTSAM 4.3 or newer, including `gtsam_unstable`
- Pangolin 0.8 or newer
- yaml-cpp

ROS and hardware integration dependencies are optional and disabled by default.

## Build and test

```bash
cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
cmake --build build --parallel
ctest --test-dir build --output-on-failure
```

Useful CMake options:

```text
IPN_MPC_BUILD_APPS       Build simulation applications (default: ON)
IPN_MPC_BUILD_TESTS      Build automated tests (default: ON)
IPN_MPC_BUILD_ROS        Build ROS integration (default: OFF)
IPN_MPC_BUILD_HARDWARE   Build hardware integration (default: OFF)
```

For example:

```bash
cmake -S . -B build \
  -DIPN_MPC_BUILD_ROS=ON \
  -DIPN_MPC_BUILD_HARDWARE=ON
```

## Running a simulation

Applications load configuration files using paths relative to the build directory. Run them from
`build/`:

```bash
cd build
./jpcm_thrust_gyro_cbf
```

Available executables:

```text
circle_trajectory
constrained_joint_estimation_control
joint_estimation_control
joint_estimation_control_isam
joint_positioning_control
jpcm_thrust_gyro
jpcm_thrust_gyro_cbf
jpcm_thrust_gyro_wall
sliding_window_joint_estimation_control
```

The Pangolin window displays the vehicle, historical trajectory, prediction horizon, reference
trajectory, coordinate frame, and configured obstacles. Closing the window exits the simulation.

## Logging

Project logs include a timestamp, severity, source file, and line number. The default level is
`info`. Set `IPN_MPC_LOG_LEVEL` to `debug`, `info`, `warning`, `error`, or `off`:

```bash
IPN_MPC_LOG_LEVEL=debug ./jpcm_thrust_gyro_cbf
```

GTSAM optimizer iteration output is disabled by default so application diagnostics remain
readable.

## Obstacle avoidance

The main CBF example uses:

```text
apps/jpcm_thrust_gyro_cbf.cpp
config/factor_graph_thrust_gyro_cbf.yaml
config/quadrotor_thrust_gyro_cbf.yaml
```

Soft CBF factors guide the optimized trajectory away from obstacles. The following parameters
control the safety behavior:

```yaml
POINT_OBS_SIGMA: 0.05
CBF_ALPHA: 1.0
CBF_BETA: 0.8
SAFE_D: 0.15
UAV_SIZE: 0.20
```

The effective required clearance is:

```text
obstacle radius + SAFE_D + UAV_SIZE / 2
```

### Experimental hard constraints

GTSAM's standard nonlinear optimizers do not directly support nonlinear inequality constraints.
This project includes an experimental sequential quadratic programming pass that linearizes each
obstacle boundary and solves hard inequalities with `gtsam_unstable::QPSolver`.

Enable it in `config/factor_graph_thrust_gyro_cbf.yaml`:

```yaml
USE_HARD_CONSTRAINTS: true
HARD_CONSTRAINT_ITERATIONS: 3
```

The active-set solver can be too slow for real-time visualization on some systems, so hard
constraints are disabled by default. Soft CBF factors and the runtime safety boundary remain
enabled.

## Project layout

```text
apps/                 Simulation and research executables
cmake/                Dependency and target definitions
config/               Runtime YAML configuration
docs/                 Mathematical notes and documentation images
include/ipn_mpc/      Public headers grouped by subsystem
src/                  Implementations grouped by subsystem
tests/                Compatibility and regression tests
tools/matlab/         MATLAB analysis tools
tools/python/         Python analysis and plotting tools
```

Public headers and implementations mirror the same subsystem organization, including `control`,
`dynamics`, `simulation`, `trajectory`, and `visualization`.

## Code style

The repository uses `.clang-format` with four-space indentation and a 100-column target. Format
changed C++ files before committing:

```bash
clang-format -i path/to/file.cpp path/to/file.h
```

## References

- [VeCBFPdFactor mathematical formulation](docs/vecbf_pd_factor_mathematical_formulation.pdf)
- [Simulation video](https://youtu.be/QBPwTr4mFy4)

## Authors

- Peiwen Yang
- Weisong Wen
- Shiyu Bai
- Li-Ta Hsu

Department of Aeronautical and Aviation Engineering, The Hong Kong Polytechnic University.

For questions, contact [peiwen1.yang@connect.polyu.hk](mailto:peiwen1.yang@connect.polyu.hk).

IPN_MPC

Explore Similar Repositories

Was this content helpful?

Recent Feedback

Download README