How to Switch Between Trajectory Planners at Runtime in Peng

You can switch between trajectory planners at runtime in Peng by defining a schedule in the YAML configuration file or by calling PlannerManager::set_planner() directly in your Rust code.

Peng is a Rust-based quadrotor simulation framework that provides a flexible trajectory-planning subsystem. The ability to switch between trajectory planners at runtime enables complex flight missions where the vehicle transitions from hovering to line tracking, Lissajous curves, or obstacle avoidance without restarting the simulation.

Understanding the Trajectory Planning Architecture

The runtime switching mechanism relies on four core components defined in src/lib.rs:

• PlannerStepConfig (lines 2327-2334) – Represents a single scheduled switch, holding the step number, planner type name, and parameters.
• PlannerManager (lines 1212-1246) – Maintains the active planner and provides set_planner() for immediate switches.
• create_planner() (lines 2425-2500) – Factory function that maps string identifiers like "MinimumJerkLine" to concrete Rust structs.
• update_planner() (lines 723-730) – Scheduler called each simulation tick to check if a switch should occur.

Configuring Planner Switches in YAML

The planner_schedule field in config/quad.yaml defines when and how to switch between trajectory planners at runtime.

Structure of the planner_schedule

Each entry in the schedule requires three fields:

• step – Simulation time in milliseconds when the planner becomes active.
• planner_type – String identifier that must match a variant in create_planner().
• params – Planner-specific configuration object.

Supported Planner Types and Parameters

Peng supports multiple trajectory primitives that you can sequence in the YAML schedule:

planner_schedule:
  # Hover to Minimum-Jerk line at 1 second

  - step: 1000
    planner_type: MinimumJerkLine
    params:
      end_position: [0.0, 0.0, 1.0]
      end_yaw: 0.0
      duration: 2.5

  # Switch to Lissajous curve at 5 seconds

  - step: 5000
    planner_type: Lissajous
    params:
      center: [0.5, 0.5, 1.0]
      amplitude: [0.5, 0.5, 0.2]
      frequency: [1.0, 2.0, 3.0]
      phase: [0.0, 1.570796, 0.0]
      duration: 20.0
      end_yaw: 6.283185307179586
      ramp_time: 5.0

  # Circular trajectory at 27 seconds

  - step: 27000
    planner_type: Circle
    params:
      center: [0.5, 0.5, 1.0]
      radius: 0.5
      angular_velocity: 1.0
      duration: 5.0
      ramp_time: 2.0

Available planner types include: Hover, MinimumJerkLine, Lissajous, Circle, Landing, ObstacleAvoidance, MinimumSnapWaypoint, and QPpolyTraj.

How Runtime Switching Works

The automatic switching mechanism relies on the simulation loop calling update_planner() each tick.

The update_planner Function

Located at lines 723-730 in src/lib.rs, this function checks if the current simulation step matches any entry in the schedule:

update_planner(
    &mut planner_manager,
    i,                                      // Current simulation step count
    time,                                   // Current simulation time in seconds
    config.simulation.simulation_frequency,
    &quad,                                  // Current quadrotor state
    &maze.obstacles,                        // Obstacles for avoidance planners
    &planner_config,                        // Vec<PlannerStepConfig> from YAML
)?;

The function converts the YAML step (milliseconds) to simulation ticks by comparing step * simulation_frequency against current_step * 1000. When a match occurs, it logs the switch and calls create_planner() to instantiate the new trajectory generator.

The PlannerManager API

The PlannerManager struct maintains the active planner state. When update_planner() detects a schedule match, it invokes:

planner_manager.set_planner(new_planner);

This immediately replaces the active planner. The next call to planner_manager.update() (typically in the control loop) queries the new planner for desired position, velocity, and yaw.

Manual Planner Switching in Code

For reactive behaviors that cannot be predefined in YAML, use the PlannerManager API directly:

use peng_quad::{PlannerManager, PlannerType, CirclePlanner};
use nalgebra::Vector3;

// Create a new circular trajectory planner
let circle_planner = CirclePlanner {
    center: Vector3::new(1.0, 1.0, 1.0),
    radius: 0.75,
    angular_velocity: 1.5,
    start_position: quad.position,
    start_time: current_time,
    duration: 8.0,
    start_yaw: quad.orientation.euler_angles().2,
    end_yaw: quad.orientation.euler_angles().2,
    ramp_time: 1.0,
};

// Immediately switch to the new planner
planner_manager.set_planner(PlannerType::Circle(circle_planner));

The PlannerType enum wraps all concrete planner implementations. Calling set_planner() bypasses the YAML schedule and immediately activates the specified trajectory generator.

Adding Custom Planners

To extend Peng with a new trajectory planner:

1. Implement the Planner trait for your struct (see HoverPlanner in the source for a minimal example).
2. Add a match arm in create_planner() (lines 2425-2500 in src/lib.rs) that maps a string identifier to your struct.
3. Document YAML parameters and use helper functions like parse_vector3() or parse_f32() to extract configuration values.

Once added, you can reference the new planner via planner_type: MyNewPlanner in the YAML schedule.

Summary

• Configuration-driven switching is defined in config/quad.yaml using the planner_schedule array with step (ms), planner_type, and params.
• Automatic runtime switching occurs via update_planner() in src/lib.rs, which checks simulation ticks against the schedule and calls PlannerManager::set_planner().
• Manual switching is available through the PlannerManager API by calling set_planner() with a PlannerType variant.
• Supported planners include Hover, MinimumJerkLine, Lissajous, Circle, Landing, ObstacleAvoidance, MinimumSnapWaypoint, and QPpolyTraj.
• Extensibility is achieved by implementing the Planner trait and adding a case to create_planner() in src/lib.rs.

Frequently Asked Questions

Can I switch planners mid-flight without restarting the simulation?

Yes. Peng is designed specifically for this use case. You can either define multiple entries in the planner_schedule YAML array with different step values, or call planner_manager.set_planner() programmatically at any point during the simulation loop. The switch takes effect immediately on the next control cycle.

What happens if two planner schedule entries have the same step value?

If multiple entries share the same step value in the YAML configuration, update_planner() will process them in the order they appear in the array. Since each call to set_planner() immediately replaces the active planner, the last entry with that step value will ultimately determine which planner is active. To avoid ambiguity, ensure each step value is unique or order entries carefully.

How do I implement a custom trajectory planner in Peng?

To add a custom planner, implement the Planner trait for your new struct, which requires methods for updating state and querying desired position, velocity, and yaw. Then add a match arm in the create_planner() function (around line 2425 in src/lib.rs) that maps a string identifier to your planner constructor. Finally, document the YAML parameters so users can reference your planner in the planner_schedule configuration.

Is there a performance penalty for switching planners at runtime?

The overhead is negligible. Switching planners involves only updating a pointer to the trait object inside PlannerManager and instantiating the new planner struct with its parameters. The update_planner() check runs once per simulation tick and performs simple integer comparisons to determine if a switch is required. The actual trajectory computation happens inside the active planner's update() method, so performance depends on the complexity of the specific planner, not the switching mechanism itself.

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