fix merging of zmp initial state

ami-iit · Dec 18, 2024 · 03e1d0e · 03e1d0e
1 parent c65fff0
commit 03e1d0e
Showing 1 changed file with 1 addition and 0 deletions.
diff --git 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