add trajectorySpread of primalSolution

Author: mayataka

Diff for: ocs2_slp/src/SlpSolver.cpp

@@ -39,6 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h>
 #include <ocs2_oc/multiple_shooting/Transcription.h>
 #include <ocs2_oc/precondition/Ruzi.h>
+#include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h>
 
 #include "ocs2_slp/Helpers.h"
 
@@ -172,6 +173,11 @@ void SlpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f
     ocpDefinition.targetTrajectoriesPtr = &targetTrajectories;
   }
 
+  // Trajectory spread of primalSolution_
+  if (!primalSolution_.timeTrajectory_.empty()) {
+    std::ignore = trajectorySpread(primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), primalSolution_);
+  }
+
   // Initialize the state and input
   vector_array_t x, u;
   multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u);

Diff for: ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp

@@ -39,6 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h>
 #include <ocs2_oc/multiple_shooting/Transcription.h>
 #include <ocs2_oc/oc_problem/OcpSize.h>
+#include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h>
 
 namespace ocs2 {
 
@@ -170,6 +171,11 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f
     ocpDefinition.targetTrajectoriesPtr = &targetTrajectories;
   }
 
+  // Trajectory spread of primalSolution_
+  if (!primalSolution_.timeTrajectory_.empty()) {
+    std::ignore = trajectorySpread(primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), primalSolution_);
+  }
+
   // Initialize the state and input
   vector_array_t x, u;
   multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u);