Trajectory Optimization in PDDP
Overview
Robotic trajectory optimizers often fail under hard constraints (e.g., joint limits, obstacles). This project strengthens Parallel DDP (PDDP) by evaluating constraint-handling methods to improve feasibility, stability, and speed on CPU/GPU. We implement these methods on both a pendulum and a 7-joint IIWA robot to map and understand findings, using Python as a reference to compare against and validate the CUDA implementation.
Summary
Methods
- Unconstrained (Baseline): Standard PDDP backward/forward passes with adaptive LM-style regularization; no explicit constraint terms.
- Log Barrier: Adds barrier terms to the cost (e.g., −μ ∑log(slack)) to discourage constraint violation; uses a decreasing barrier schedule and safeguarded step sizes.
- Augmented Lagrangian (AL): Penalty + multipliers for state/path constraints with conservative μ updates; enforces feasibility via violation metrics and PD-checked
Quu. - Interior-Point (IP): Primal-dual formulation with slack variables; solves damped/centered subproblems to reduce KKT residuals while maintaining strict feasibility.
- Quadratic Programming (QP): Local QP subproblems from linearized dynamics + quadratic cost; applies explicit state/box/linear constraints via batched QP solves for control updates.
Augmented Lagrangian Constraint-Handling Method
- Developed and implemented the Augmented Lagrangian constraint-handling method within PDDP in both Python and CUDA, combining penalty and multiplier updates to enforce state feasibility.
- Designed a μ-update rule that increased only after consecutive violations and decreased once constraints were satisfied, improving numerical stability and avoiding oscillation.
- Documented the implementation and benchmark results for future lab use in extending PDDP to higher-dimensional robotic systems.
Augmented Lagrangian Method Python Results for IIWA
We are able to see that the Augmented Lagrangian method converges to the solution while staying within the soft constraints set.
Augmented Lagrangian Method CUDA Results for IIWA
We are able to see that the Augmented Lagrangian method converges to the correct position with minimal errors.
Next Steps
- GPU profiling and kernel optimizations for constraint projection and residual evaluation.
- Batched experiments and comparisons for all constraint-handling methods.
- Unit tests for constraint satisfaction and ensure close enough values between CPU and GPU.