How to Add Custom Trajectory Planners to the Peng Framework: A Step-by-Step Guide

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.

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 at line 820, any planner must implement two methods:

• plan: Returns a tuple of (desired_position, desired_velocity, desired_yaw) as Vector3<f32> values and a yaw angle.
• is_finished: Returns a Result<bool, SimulationError> indicating whether the trajectory is complete.

The PlannerType Enum

The PlannerType enum (located at 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 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, 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 or create a separate module like src/custom_planners.rs.

/// 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.

impl Planner for SinusoidalPlanner {
    fn plan(
        &self,
        _current_position: Vector3<f32>,
        _current_velocity: Vector3<f32>,
        time: f32,
    ) -> (Vector3<f32>, Vector3<f32>, 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<f32>,
        time: f32,
    ) -> Result<bool, SimulationError> {
        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 line 705. Place the new variant after the existing entries to maintain the enum structure.

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.

impl PlannerType {
    pub fn plan(
        &self,
        current_position: Vector3<f32>,
        current_velocity: Vector3<f32>,
        time: f32,
    ) -> (Vector3<f32>, Vector3<f32>, 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<f32>,
        time: f32,
    ) -> Result<bool, SimulationError> {
        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.

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 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 to add a custom trajectory planner?

No, all required changes are contained within 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.

What are the exact method signatures required for the Planner trait?

The Planner trait requires two methods: plan which takes current_position: Vector3<f32>, current_velocity: Vector3<f32>, and time: f32, returning (Vector3<f32>, Vector3<f32>, f32) for position, velocity, and yaw; and is_finished which takes current_position: Vector3<f32> and time: f32, returning Result<bool, SimulationError>.

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<bool, SimulationError>, 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.

{
  "mcpServers": {
    "instagit": {
      "command": "npx",
      "args": ["-y", "instagit@latest"]
    }
  }
}