# How to Add Custom Trajectory Planners to the Peng Framework: A Step-by-Step Guide > Learn how to add custom trajectory planners to the Peng framework. Implement the Planner trait, register your planner in the enum, and extend dispatch match arms for seamless integration. - Repository: [Yang Zhou/peng](https://github.com/makeecat/peng) - Tags: how-to-guide - Published: 2026-03-06 --- **To add a custom trajectory planner to the Peng framework, implement the `Planner` trait for your struct, register it as a variant in the `PlannerType` enum, and extend the dispatch match arms in `PlannerType::plan` and `PlannerType::is_finished` within [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs).** The `makeecat/peng` repository provides a modular quadrotor simulation framework where trajectory planning is centralized through a trait-based architecture. Adding custom trajectory planners allows you to define specialized flight behaviors while maintaining compatibility with the existing control pipeline. All necessary modifications are contained within the main library file, making the integration process straightforward and type-safe. ## Understanding the Peng Trajectory Planning Architecture Peng’s trajectory-planning subsystem is built around three core components that work together to generate position, velocity, and yaw references for the quadrotor controller. ### The Planner Trait The **Planner** trait defines the required API for all trajectory planners in the system. According to the source code in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) at line 820, any planner must implement two methods: - `plan`: Returns a tuple of `(desired_position, desired_velocity, desired_yaw)` as `Vector3` values and a yaw angle. - `is_finished`: Returns a `Result` indicating whether the trajectory is complete. ### The PlannerType Enum The **PlannerType** enum (located at [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) line 705) acts as a type-safe registry for all available planners. This enum uses variant types to wrap concrete planner structs, enabling the framework to switch between different planning algorithms at runtime through a single interface. ### The PlannerManager The **PlannerManager** struct (found at [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) line 1212) holds the active planner and handles the runtime switching logic. It provides the `set_planner` method to swap between different `PlannerType` variants during simulation execution. ## Step-by-Step Implementation Guide Follow these steps to integrate a custom planner into the Peng framework. All changes occur within [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs), though you may optionally create a dedicated module for organization. ### Step 1: Create the Planner Struct Define a struct with the parameters required for your trajectory calculation. You can place this directly in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) or create a separate module like [`src/custom_planners.rs`](https://github.com/makeecat/peng/blob/main/src/custom_planners.rs). ```rust /// Example: a simple sinusoidal planner that moves the quadrotor up and down. pub struct SinusoidalPlanner { /// Amplitude of the vertical motion (meters) pub amplitude: f32, /// Frequency of the sinusoid (Hz) pub frequency: f32, /// Start time of the trajectory pub start_time: f32, /// Duration after which the planner is considered finished pub duration: f32, } ``` ### Step 2: Implement the Planner Trait Implement the `Planner` trait for your struct, providing concrete logic for the `plan` and `is_finished` methods. The `plan` method receives the current state and time, while `is_finished` checks trajectory completion. ```rust impl Planner for SinusoidalPlanner { fn plan( &self, _current_position: Vector3, _current_velocity: Vector3, time: f32, ) -> (Vector3, Vector3, f32) { // Normalise time relative to the start of the planner let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0); // Desired vertical position follows a sine wave let z = self.amplitude * (2.0 * std::f32::consts::PI * self.frequency * t).sin(); // Derivative gives the vertical velocity let vz = self.amplitude * (2.0 * std::f32::consts::PI * self.frequency).cos() * (2.0 * std::f32::consts::PI * self.frequency); let desired_pos = Vector3::new(0.0, 0.0, z); let desired_vel = Vector3::new(0.0, 0.0, vz); // Yaw stays constant for this example (desired_pos, desired_vel, 0.0) } fn is_finished( &self, _current_position: Vector3, time: f32, ) -> Result { Ok(time >= self.start_time + self.duration) } } ``` ### Step 3: Register the Variant in PlannerType Add your planner as a new variant in the `PlannerType` enum at [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) line 705. Place the new variant after the existing entries to maintain the enum structure. ```rust pub enum PlannerType { // … existing variants … QPpolyTraj(QPpolyTrajPlanner), /// Your custom sinusoidal planner Sinusoidal(SinusoidalPlanner), } ``` ### Step 4: Extend the Dispatch Logic Update the `match` expressions in `PlannerType::plan` (line 725) and `PlannerType::is_finished` (line 785) to forward calls to your new variant. This dispatch pattern maintains the enum's role as a universal interface. ```rust impl PlannerType { pub fn plan( &self, current_position: Vector3, current_velocity: Vector3, time: f32, ) -> (Vector3, Vector3, f32) { match self { // … existing arms … PlannerType::QPpolyTraj(p) => p.plan(current_position, current_velocity, time), PlannerType::Sinusoidal(p) => p.plan(current_position, current_velocity, time), } } pub fn is_finished( &self, current_position: Vector3, time: f32, ) -> Result { match self { // … existing arms … PlannerType::QPpolyTraj(p) => p.is_finished(current_position, time), PlannerType::Sinusoidal(p) => p.is_finished(current_position, time), } } } ``` ### Step 5: Use the Custom Planner Instantiate your planner and activate it through the `PlannerManager` using the `set_planner` method. The manager will now route all planning calls to your custom implementation. ```rust use peng_quad::{PlannerManager, PlannerType, SinusoidalPlanner}; use nalgebra::Vector3; // Initialise the manager with a hover (required by the constructor) let mut manager = PlannerManager::new(Vector3::new(0.0, 0.0, 1.0), 0.0); // Create an instance of the custom planner let sinusoidal = SinusoidalPlanner { amplitude: 0.5, frequency: 0.2, start_time: 0.0, duration: 20.0, }; // Switch the manager to use the custom planner manager.set_planner(PlannerType::Sinusoidal(sinusoidal)); // Now each simulation step can query the manager: let (pos, vel, yaw) = manager .update( current_position, current_orientation, current_velocity, sim_time, ) .unwrap(); ``` ## Summary Adding custom trajectory planners to Peng requires modifications to only one file while maintaining type safety throughout the architecture: - **Implement the `Planner` trait** for your struct with `plan` and `is_finished` methods - **Add a variant** to the `PlannerType` enum in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) at line 705 - **Extend the match arms** in `PlannerType::plan` (line 725) and `PlannerType::is_finished` (line 785) to dispatch to your planner - **Switch planners at runtime** using `PlannerManager::set_planner` (line 1212) This design keeps the framework modular and allows instant integration of new flight behaviors without modifying the core simulation loop or controller logic. ## Frequently Asked Questions ### Do I need to modify any files other than [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) to add a custom trajectory planner? No, all required changes are contained within [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs). You only need to implement the `Planner` trait, add your struct to the `PlannerType` enum, and extend the dispatch methods. However, for better code organization, you may choose to place your planner implementation in a separate module and import it into [`lib.rs`](https://github.com/makeecat/peng/blob/main/lib.rs). ### What are the exact method signatures required for the `Planner` trait? The `Planner` trait requires two methods: `plan` which takes `current_position: Vector3`, `current_velocity: Vector3`, and `time: f32`, returning `(Vector3, Vector3, f32)` for position, velocity, and yaw; and `is_finished` which takes `current_position: Vector3` and `time: f32`, returning `Result`. ### Can I switch between multiple custom planners during a single simulation? Yes, the `PlannerManager` supports runtime switching through the `set_planner` method. You can instantiate multiple planner variants and call `manager.set_planner(PlannerType::YourCustomVariant(instance))` at any point during the simulation to change trajectory generation mid-flight. ### How do I handle errors in my custom planner's `is_finished` method? The `is_finished` method returns a `Result`, allowing you to propagate errors using the `?` operator or return `Ok(true/false)` for normal completion checks. If your planner encounters an invalid state, you can return `Err(SimulationError::YourErrorVariant)` to signal failure to the simulation manager.