diff --git a/src/Planners/src/UnicycleTrajectoryGenerator.cpp b/src/Planners/src/UnicycleTrajectoryGenerator.cpp index 6f1cc45f6b..ff29c8e7f7 100644 --- a/src/Planners/src/UnicycleTrajectoryGenerator.cpp +++ b/src/Planners/src/UnicycleTrajectoryGenerator.cpp @@ -750,6 +750,7 @@ bool BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::Impl::mergeTrajec trajectory.zmpGeneratorInitialState .assign(unicycleTrajectoryPlanner.getOutput().zmpTrajectory.initialState.begin(), unicycleTrajectoryPlanner.getOutput().zmpTrajectory.initialState.end()); + trajectory.zmpGeneratorInitialState.pop_front(); // get feet cubic spline trajectory