From 03e1d0e8ea8ebc01d639f527d420e7da0a093605 Mon Sep 17 00:00:00 2001
From: Lorenzo <lore.moretti94@gmail.com>
Date: Wed, 18 Dec 2024 16:27:48 +0100
Subject: [PATCH] fix merging of zmp initial state

---
 src/Planners/src/UnicycleTrajectoryGenerator.cpp | 1 +
 1 file changed, 1 insertion(+)

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