How to Add New Planners to the Existing Schedule in Peng

You can add new planners to Peng by implementing the Planner trait for your custom struct, extending the PlannerType enum, and updating the dispatch logic in src/lib.rs to route calls through PlannerManager::set_planner.

Adding custom motion primitives to the existing schedule in Peng (the Rust-based quadrotor simulation framework by makeecat) requires extending the core planning architecture. All trajectory generation logic lives in src/lib.rs, where the Planner trait defines the contract for motion primitives and the PlannerType enum provides type erasure for the scheduler.

Understanding the Planner Architecture in Peng

Peng’s scheduling system relies on three core components defined in src/lib.rs:

• Planner trait (lines 796–822): Defines the interface with two required methods—plan() for trajectory generation and is_finished() for completion checking.
• PlannerType enum (lines 945–979): A wrapper enum that holds concrete planner variants like HoverPlanner and MinimumJerkLinePlanner.
• PlannerManager (lines ~1241–1245): Maintains the active PlannerType and switches between planners via set_planner().

To integrate a new planner, you must bridge your custom implementation into this existing enum-based dispatch system.

Step-by-Step Guide to Adding a Custom Planner

1. Define the Planner Struct

Create a new pub struct in src/lib.rs alongside existing planners (lines 908–939). Include fields for all parameters your algorithm requires, such as start time, duration, waypoints, or amplitude settings.

pub struct SineWavePlanner {
    pub amplitude: f32,
    pub frequency: f32,
    pub altitude: f32,
    pub y: f32,
    pub start_time: f32,
    pub duration: f32,
    pub yaw: f32,
}

2. Implement the Planner Trait

Implement the Planner trait for your struct using the exact signatures defined in the source. The plan method receives current position, velocity, and time, returning desired position, velocity, and yaw. The is_finished method returns a Result<bool, SimulationError> indicating whether the trajectory segment is complete.

impl Planner for SineWavePlanner {
    fn plan(
        &self,
        _current_position: Vector3<f32>,
        _current_velocity: Vector3<f32>,
        time: f32,
    ) -> (Vector3<f32>, Vector3<f32>, f32) {
        let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0);
        let x = self.amplitude * (2.0 * std::f32::consts::PI * self.frequency * t).sin();
        let position = Vector3::new(x, self.y, self.altitude);
        let vx = self.amplitude * (2.0 * std::f32::consts::PI * self.frequency) * 
                 (2.0 * std::f32::consts::PI * self.frequency * t).cos() / self.duration;
        let velocity = Vector3::new(vx, 0.0, 0.0);
        (position, velocity, self.yaw)
    }

    fn is_finished(
        &self,
        _current_position: Vector3<f32>,
        time: f32,
    ) -> Result<bool, SimulationError> {
        Ok(time >= self.start_time + self.duration)
    }
}

3. Extend the PlannerType Enum

Add a new variant to the PlannerType enum (lines 945–979) to hold your planner instance. This enables the scheduler to store your planner in the same collection as built-in types.

pub enum PlannerType {
    // ... existing variants ...
    SineWave(SineWavePlanner),
}

4. Update the Dispatch Logic

Modify the impl PlannerType block (lines ~1010–1070) to forward calls to your new variant. Update both the plan and is_finished methods with match arms that delegate to your struct’s implementation.

impl PlannerType {
    pub fn plan(&self, cp: Vector3<f32>, cv: Vector3<f32>, time: f32) 
        -> (Vector3<f32>, Vector3<f32>, f32) {
        match self {
            // ... existing arms ...
            PlannerType::SineWave(p) => p.plan(cp, cv, time),
        }
    }

    pub fn is_finished(&self, cp: Vector3<f32>, time: f32) 
        -> Result<bool, SimulationError> {
        match self {
            // ... existing arms ...
            PlannerType::SineWave(p) => p.is_finished(cp, time),
        }
    }
}

5. Instantiate and Activate the Planner

Create an instance of your planner and pass it to PlannerManager::set_planner() (lines 1241–1245) to add it to the existing schedule. The manager immediately begins routing trajectory calls through your implementation.

let sine_planner = SineWavePlanner {
    amplitude: 2.0,
    frequency: 0.5,
    altitude: 1.0,
    y: 0.0,
    start_time: 0.0,
    duration: 20.0,
    yaw: 0.0,
};
planner_manager.set_planner(PlannerType::SineWave(sine_planner));

Key Implementation Details

• Statelessness: Planners should store all necessary state in their struct fields, as the PlannerManager calls methods without maintaining context between invocations.
• Time normalization: Use the start_time and duration fields to normalize simulation time within your plan method, as shown in the SineWavePlanner example.
• Error handling: The is_finished method returns Result<bool, SimulationError>—always wrap boolean returns in Ok() to satisfy the trait contract.

Summary

• Implement the Planner trait (plan and is_finished) for your custom struct in src/lib.rs.
• Add a variant to the PlannerType enum to enable type erasure.
• Update match arms in PlannerType::plan and PlannerType::is_finished to route calls to your implementation.
• Activate via PlannerManager::set_planner to insert your planner into the existing schedule.
• Maintain state exclusively within your planner struct fields to ensure compatibility with the manager’s dispatch pattern.

Frequently Asked Questions

What is the Planner trait in Peng?

The Planner trait defined in src/lib.rs (lines 796–822) is the core interface for all motion primitives in Peng. It requires two methods: plan() for computing desired position, velocity, and yaw from current state and time, and is_finished() for determining when a trajectory segment ends. All built-in planners like HoverPlanner and MinimumJerkLinePlanner implement this trait.

Where is the planner dispatch logic located?

The dispatch logic resides in the impl PlannerType block in src/lib.rs (around lines 1010–1070). This block contains the match statements that forward plan() and is_finished() calls to the concrete implementations based on which enum variant is currently active in the PlannerManager.

Can I add multiple custom planners to the same schedule?

Yes. You can add multiple custom planners by creating distinct structs for each motion primitive, adding separate variants to PlannerType, and updating the corresponding match arms. Switch between them at runtime by calling planner_manager.set_planner() with different PlannerType variants as needed.

Do I need to modify Cargo.toml to add a new planner?

No. Adding a new planner requires changes only to src/lib.rs since planners use the existing dependencies (nalgebra for vector math) already declared in Cargo.toml. You only need to modify dependencies if your custom planner requires external crates not already included in the Peng project.

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