fix time trajectory for interpolation

Author: mayataka

ocs2_ipm/src/IpmSolver.cpp

@@ -408,7 +408,7 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>&
       if (cachedEventIndex < slackDualTrajectory_.preJumps.size()) {
         ipm::toSlackDual(std::move(slackDualTrajectory_.preJumps[cachedEventIndex]), slackStateIneq[i], dualStateIneq[i]);
       } else {
-        const auto time = newTimeTrajectory[i];
+        const auto time = getIntervalStart(timeDiscretization[i]);
         const auto& state = x[i];
         constexpr auto request = Request::Constraint;
         ocpDefinition.preComputationPtr->requestPreJump(request, time, state);
@@ -428,7 +428,7 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>&
       dualStateInputIneq[i].resize(0);
       ++eventIdx;
     } else {
-      const auto time = newTimeTrajectory[i];
+      const auto time = getIntervalStart(timeDiscretization[i]);
       if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) {
         ipm::toSlackDual(getIntermediateDualSolutionAtTime(slackDualTrajectory_, time), slackStateIneq[i], dualStateIneq[i],
                          slackStateInputIneq[i], dualStateInputIneq[i]);

ocs2_oc/src/oc_data/TimeDiscretization.cpp

@@ -123,9 +123,13 @@ scalar_array_t toTime(const std::vector<AnnotatedTime>& annotatedTime) {
 }
 
 scalar_array_t toInterpolationTime(const std::vector<AnnotatedTime>& annotatedTime) {
+  if (annotatedTime.empty()) {
+    return scalar_array_t();
+  }
   scalar_array_t timeTrajectory;
   timeTrajectory.reserve(annotatedTime.size());
-  for (size_t i = 0; i < annotatedTime.size() - 1; i++) {
+  timeTrajectory.push_back(annotatedTime.back().time - numeric_traits::limitEpsilon<scalar_t>());
+  for (int i = 1; i < annotatedTime.size() - 1; i++) {
     if (annotatedTime[i].event == AnnotatedTime::Event::PostEvent) {
       timeTrajectory.push_back(getInterpolationTime(annotatedTime[i]));
     } else {