Merged in fix/includeFinalEvent (pull request #615)

fix trajectory spreading - final time can be post-event

Approved-by: Farbod Farshidian

Author: Huoleit

ocs2_core/src/Types.cpp

@@ -189,7 +189,7 @@ std::string checkSize(int stateDim, int inputDim, const ScalarFunctionQuadraticA
   if (data.dfdux.rows() != inputDim) {
     errorDescription << dataName << ".dfdux.rows() != " << inputDim << "\n";
   }
-  if (data.dfdux.cols() != stateDim) {
+  if (data.dfdux.cols() != stateDim && inputDim > 0) {
     errorDescription << dataName << ".dfdux.cols() != " << stateDim << "\n";
   }
 

ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h

@@ -135,6 +135,26 @@ void retrieveActiveNormalizedTime(const std::pair<int, int>& partitionInterval,
                                   const size_array_t& postEventIndices, scalar_array_t& normalizedTimeTrajectory,
                                   size_array_t& normalizedPostEventIndices);
 
+/**
+ * Get the Partition Intervals From Time Trajectory. Intervals are defined as [start, end).
+ *
+ * Pay attention, the rightmost index of the end partition is (..., timeArray.size() - 1) , as the last value function is filled manually.
+ * The reason is though we don’t write to the end index, we do have to read it. Adding the last index to the final partition will
+ * cause a segmentation fault. There is no trivial method to distinguish the final partition from other partitions because, by design,
+ * partitions should be treated equally.
+ *
+ * Every time point that is equal or larger to the desiredPartitionPoint should be included in that partition. This logic here is the same
+ * as the event times.
+ *
+ * The last time of desiredPartitionPoints is filled manually. There is no round-off error involved. So it is safe to use == for
+ * floating-point numbers. The last time point is naturally included by using std::lower_bound.
+ *
+ * @param [in] timeTrajectory: time trajectory that will be divided
+ * @param [in] numWorkers: number of worker i.e. number of partitions
+ * @return array of index pairs indicating the start and end of each partition
+ */
+std::vector<std::pair<int, int>> computePartitionIntervals(const scalar_array_t& timeTrajectory, int numWorkers);
+
 /**
  * Gets a reference to the linear controller from the given primal solution.
  */

ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h

@@ -229,26 +229,6 @@ class GaussNewtonDDP : public SolverBase {
     return getValueFunctionImpl(time, state, cachedPrimalData_.primalSolution, cachedDualData_.valueFunctionTrajectory);
   }
 
-  /**
-   * Get the Partition Intervals From Time Trajectory. Intervals are defined as [start, end).
-   *
-   * Pay attention, the rightmost index of the end partition is (..., timeArray.size() - 1) , as the last value function is filled manually.
-   * The reason is though we don’t write to the end index, we do have to read it. Adding the last index to the final partition will
-   * cause a segmentation fault. There is no trivial method to distinguish the final partition from other partitions because, by design,
-   * partitions should be treated equally.
-   *
-   * Every time point that is equal or larger to the desiredPartitionPoint should be included in that partition. This logic here is the same
-   * as the event times.
-   *
-   * The last time of desiredPartitionPoints is filled manually. There is no round-off error involved. So it is safe to use == for
-   * floating-point numbers. The last time point is naturally included by using std::lower_bound.
-   *
-   * @param [in] timeTrajectory: time trajectory that will be divided
-   * @param [in] numWorkers: number of worker i.e. number of partitions
-   * @return array of index pairs indicating the start and end of each partition
-   */
-  std::vector<std::pair<int, int>> getPartitionIntervalsFromTimeTrajectory(const scalar_array_t& timeTrajectory, int numWorkers);
-
   /**
    * Forward integrate the system dynamics with given controller in primalSolution and operating trajectories. In general, it uses
    * the given control policies and initial state, to integrate the system dynamics in the time period [initTime, finalTime].

ocs2_ddp/src/DDP_HelperFunctions.cpp

@@ -300,4 +300,33 @@ void retrieveActiveNormalizedTime(const std::pair<int, int>& partitionInterval,
                  [N, &partitionInterval](size_t i) -> size_t { return N - i + partitionInterval.first; });
 }
 
+/******************************************************************************************************/
+/******************************************************************************************************/
+/******************************************************************************************************/
+std::vector<std::pair<int, int>> computePartitionIntervals(const scalar_array_t& timeTrajectory, int numWorkers) {
+  const scalar_t increment = (timeTrajectory.back() - timeTrajectory.front()) / static_cast<scalar_t>(numWorkers);
+
+  scalar_array_t desiredPartitionPoints(numWorkers + 1);
+  desiredPartitionPoints.front() = timeTrajectory.front();
+  for (size_t i = 1u; i < desiredPartitionPoints.size() - 1; i++) {
+    desiredPartitionPoints[i] = desiredPartitionPoints[i - 1] + increment;
+  }
+  desiredPartitionPoints.back() = timeTrajectory.back();
+
+  std::vector<std::pair<int, int>> partitionIntervals;
+  partitionIntervals.reserve(desiredPartitionPoints.size());
+
+  int endPos, startPos = 0;
+  for (size_t i = 1u; i < desiredPartitionPoints.size(); i++) {
+    const auto itr = std::upper_bound(timeTrajectory.begin(), timeTrajectory.end(), desiredPartitionPoints[i]);
+    endPos = (itr != timeTrajectory.end()) ? std::distance(timeTrajectory.begin(), itr) : (timeTrajectory.size() - 1);
+    if (endPos != startPos) {
+      partitionIntervals.emplace_back(startPos, endPos);
+      startPos = endPos;
+    }
+  }
+
+  return partitionIntervals;
+}
+
 }  // namespace ocs2

ocs2_ddp/src/GaussNewtonDDP.cpp

@@ -369,35 +369,6 @@ vector_t GaussNewtonDDP::getStateInputEqualityConstraintLagrangianImpl(scalar_t
   return DmDagger.transpose() * temp;
 }
 
-/******************************************************************************************************/
-/******************************************************************************************************/
-/******************************************************************************************************/
-std::vector<std::pair<int, int>> GaussNewtonDDP::getPartitionIntervalsFromTimeTrajectory(const scalar_array_t& timeTrajectory,
-                                                                                         int numWorkers) {
-  scalar_array_t desiredPartitionPoints(numWorkers + 1);
-  desiredPartitionPoints.front() = timeTrajectory.front();
-
-  const scalar_t increment = (timeTrajectory.back() - timeTrajectory.front()) / static_cast<scalar_t>(numWorkers);
-  for (size_t i = 1u; i < desiredPartitionPoints.size() - 1; i++) {
-    desiredPartitionPoints[i] = desiredPartitionPoints[i - 1] + increment;
-  }
-  desiredPartitionPoints.back() = timeTrajectory.back();
-
-  std::vector<std::pair<int, int>> partitionIntervals;
-  partitionIntervals.reserve(desiredPartitionPoints.size());
-
-  int endPos, startPos = 0;
-  for (size_t i = 1u; i < desiredPartitionPoints.size(); i++) {
-    const scalar_t& time = desiredPartitionPoints[i];
-    endPos = std::distance(timeTrajectory.begin(), std::lower_bound(timeTrajectory.begin(), timeTrajectory.end(), time));
-    if (endPos != startPos) {
-      partitionIntervals.emplace_back(startPos, endPos);
-      startPos = endPos;
-    }
-  }
-
-  return partitionIntervals;
-}
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
@@ -534,8 +505,7 @@ scalar_t GaussNewtonDDP::solveSequentialRiccatiEquationsImpl(const ScalarFunctio
     riccatiEquationsWorker(0, partitionInterval, finalValueFunction);
   } else {  // solve it in parallel
     // do equal-time partitions based on available thread resource
-    std::vector<std::pair<int, int>> partitionIntervals =
-        getPartitionIntervalsFromTimeTrajectory(nominalPrimalData_.primalSolution.timeTrajectory_, ddpSettings_.nThreads_);
+    const auto partitionIntervals = computePartitionIntervals(nominalPrimalData_.primalSolution.timeTrajectory_, ddpSettings_.nThreads_);
 
     // hold the final value function of each partition
     std::vector<ScalarFunctionQuadraticApproximation> finalValueFunctionOfEachPartition(partitionIntervals.size());
@@ -559,7 +529,14 @@ scalar_t GaussNewtonDDP::solveSequentialRiccatiEquationsImpl(const ScalarFunctio
   if (ddpSettings_.checkNumericalStability_) {
     const int N = nominalPrimalData_.primalSolution.timeTrajectory_.size();
     for (int k = N - 1; k >= 0; k--) {
-      const auto errorDescription = checkBeingPSD(nominalDualData_.valueFunctionTrajectory[k], "ValueFunction");
+      // check size
+      auto errorDescription = checkSize(nominalPrimalData_.primalSolution.stateTrajectory_[k].size(), 0,
+                                        nominalDualData_.valueFunctionTrajectory[k], "ValueFunction");
+      if (!errorDescription.empty()) {
+        throw std::runtime_error(errorDescription);
+      }
+      // check PSD
+      errorDescription = checkBeingPSD(nominalDualData_.valueFunctionTrajectory[k], "ValueFunction");
       if (!errorDescription.empty()) {
         std::stringstream throwMsg;
         throwMsg << "at time " << nominalPrimalData_.primalSolution.timeTrajectory_[k] << ":\n";

ocs2_ddp/src/SLQ.cpp

@@ -239,7 +239,6 @@ void SLQ::riccatiEquationsWorker(size_t workerIndex, const std::pair<int, int>&
    *  SsNormalized = [-10.0, ..., -2.0, -1.0, -0.0]
    */
   vector_array_t& allSsTrajectory = allSsTrajectoryStock_[workerIndex];
-  allSsTrajectory.clear();
   integrateRiccatiEquationNominalTime(*riccatiIntegratorPtrStock_[workerIndex], *riccatiEquationsPtrStock_[workerIndex], partitionInterval,
                                       nominalTimeTrajectory, nominalEventsPastTheEndIndices, std::move(allSsFinal), SsNormalizedTime,
                                       SsNormalizedPostEventIndices, allSsTrajectory);
@@ -266,7 +265,6 @@ void SLQ::integrateRiccatiEquationNominalTime(IntegratorBase& riccatiIntegrator,
   const int nominalTimeSize = SsNormalizedTime.size();
   const int numEvents = SsNormalizedPostEventIndices.size();
   auto partitionDuration = nominalTimeTrajectory[partitionInterval.second] - nominalTimeTrajectory[partitionInterval.first];
-  const auto numTimeSteps = static_cast<size_t>(settings().maxNumStepsPerSecond_ * std::max(1.0, partitionDuration));
 
   // Normalized switching time indices, start and end of the partition are added at the beginning and end
   using iterator_t = scalar_array_t::const_iterator;
@@ -281,15 +279,16 @@ void SLQ::integrateRiccatiEquationNominalTime(IntegratorBase& riccatiIntegrator,
 
   // integrating the Riccati equations
   allSsTrajectory.clear();
-  allSsTrajectory.reserve(numTimeSteps);
+  allSsTrajectory.reserve(nominalTimeSize);
   for (int i = 0; i <= numEvents; i++) {
     iterator_t beginTimeItr = SsNormalizedSwitchingTimesIndices[i].first;
     iterator_t endTimeItr = SsNormalizedSwitchingTimesIndices[i].second;
 
-    Observer observer(&allSsTrajectory);
     // solve Riccati equations
+    Observer observer(&allSsTrajectory);
+    const auto maxNumTimeSteps = static_cast<size_t>(settings().maxNumStepsPerSecond_ * std::max(1.0, partitionDuration));
     riccatiIntegrator.integrateTimes(riccatiEquation, observer, allSsFinal, beginTimeItr, endTimeItr, settings().timeStep_,
-                                     settings().absTolODE_, settings().relTolODE_, numTimeSteps);
+                                     settings().absTolODE_, settings().relTolODE_, maxNumTimeSteps);
 
     if (i < numEvents) {
       allSsFinal = riccatiEquation.computeJumpMap(*endTimeItr, allSsTrajectory.back());

ocs2_oc/include/ocs2_oc/trajectory_adjustment/TrajectorySpreading.h

@@ -123,8 +123,13 @@ class TrajectorySpreading final {
    */
   size_array_t findPostEventIndices(const scalar_array_t& eventTimes, const scalar_array_t& timeTrajectory) const {
     size_array_t postEventIndices(eventTimes.size());
-    std::transform(eventTimes.cbegin(), eventTimes.cend(), postEventIndices.begin(),
-                   [this, &timeTrajectory](scalar_t time) -> int { return upperBoundIndex(timeTrajectory, time); });
+    for (std::size_t i = 0; i < eventTimes.size(); i++) {
+      if (i == eventTimes.size() - 1 && eventTimes[i] == timeTrajectory.back()) {
+        postEventIndices[i] = timeTrajectory.size() - 1;
+      } else {
+        postEventIndices[i] = upperBoundIndex(timeTrajectory, eventTimes[i]);
+      }
+    }
     return postEventIndices;
   }
 

ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp

@@ -170,7 +170,7 @@ auto TrajectorySpreading::set(const ModeSchedule& oldModeSchedule, const ModeSch
   // if last mode of the new mode sequence is NOT matched
   else if (!isLastActiveModeOfNewModeSequenceMatched) {
     const auto mismatchEventTime = newModeSchedule.eventTimes[newStartIndexOfMatchedSequence + w - 1];
-    eraseFromIndex_ = upperBoundIndex(oldTimeTrajectory, mismatchEventTime);
+    eraseFromIndex_ = lowerBoundIndex(oldTimeTrajectory, mismatchEventTime);
   }
 
   // computes the index of the spreading values and intervals

ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp

@@ -52,6 +52,16 @@ struct Result {
   ocs2::size_array_t preEventModeTrajectory;
 };
 
+std::size_t modeAtTime(const ocs2::ModeSchedule& modeSchedule, ocs2::scalar_t time, bool isFinalTime) {
+  if (isFinalTime) {
+    auto timeItr = std::upper_bound(modeSchedule.eventTimes.begin(), modeSchedule.eventTimes.end(), time);
+    int modeIndex = std::distance(modeSchedule.eventTimes.begin(), timeItr);
+    return modeSchedule.modeSequence[modeIndex];
+  } else {
+    return modeSchedule.modeAtTime(time);
+  }
+}
+
 class TrajectorySpreadingTest : public testing::Test {
  protected:
   static constexpr size_t STATE_DIM = 2;
@@ -99,8 +109,9 @@ class TrajectorySpreadingTest : public testing::Test {
                    [&out](size_t eventIndex) { return out.stateTrajectory[eventIndex - 1]; });
 
     out.modeTrajectory.resize(out.timeTrajectory.size());
-    std::transform(out.timeTrajectory.begin(), out.timeTrajectory.end(), out.modeTrajectory.begin(),
-                   [&modeSchedule](ocs2::scalar_t time) { return modeSchedule.modeAtTime(time); });
+    for (int i = 0; i < out.modeTrajectory.size(); i++) {
+      out.modeTrajectory[i] = modeAtTime(modeSchedule, out.timeTrajectory[i], i == out.modeTrajectory.size() - 1 ? true : false);
+    }
 
     out.preEventModeTrajectory.resize(out.postEventsIndeces.size());
     std::transform(out.postEventsIndeces.begin(), out.postEventsIndeces.end(), out.preEventModeTrajectory.begin(),
@@ -194,17 +205,22 @@ class TrajectorySpreadingTest : public testing::Test {
       const ocs2::scalar_t finalTime = spreadResult.timeTrajectory.back();
 
       const auto startEventItr = std::upper_bound(updatedModeSchedule.eventTimes.begin(), updatedModeSchedule.eventTimes.end(), initTime);
-      // If a time point is aligned with(the same as) an event time, it belongs to the pre-mode.
-      const auto endEventItr = std::lower_bound(updatedModeSchedule.eventTimes.begin(), updatedModeSchedule.eventTimes.end(), finalTime);
-
-      ocs2::size_array_t postEventIndice(endEventItr - startEventItr);
-      std::transform(startEventItr, endEventItr, postEventIndice.begin(), [&spreadResult](ocs2::scalar_t time) {
-        auto timeItr = std::upper_bound(spreadResult.timeTrajectory.begin(), spreadResult.timeTrajectory.end(), time);
-        return std::distance(spreadResult.timeTrajectory.begin(), timeItr);
-      });
+      // If the final time is aligned with(the same as) an event time, it belongs to the post-mode.
+      const auto endEventItr = std::upper_bound(updatedModeSchedule.eventTimes.begin(), updatedModeSchedule.eventTimes.end(), finalTime);
+
+      ocs2::size_array_t postEventIndices(endEventItr - startEventItr);
+      for (auto itr = startEventItr; itr != endEventItr; itr++) {
+        int index = std::distance(startEventItr, itr);
+        if (itr == endEventItr - 1 && *itr == spreadResult.timeTrajectory.back()) {
+          postEventIndices[index] = spreadResult.timeTrajectory.size() - 1;
+        } else {
+          auto timeItr = std::upper_bound(spreadResult.timeTrajectory.begin(), spreadResult.timeTrajectory.end(), *itr);
+          postEventIndices[index] = std::distance(spreadResult.timeTrajectory.begin(), timeItr);
+        }
+      }
 
-      EXPECT_TRUE(spreadResult.postEventsIndeces.size() == postEventIndice.size());
-      EXPECT_TRUE(std::equal(postEventIndice.begin(), postEventIndice.end(), spreadResult.postEventsIndeces.begin()));
+      EXPECT_TRUE(spreadResult.postEventsIndeces.size() == postEventIndices.size());
+      EXPECT_TRUE(std::equal(postEventIndices.begin(), postEventIndices.end(), spreadResult.postEventsIndeces.begin()));
 
     } else {
       EXPECT_EQ(spreadResult.postEventsIndeces.size(), 0);
@@ -220,9 +236,10 @@ class TrajectorySpreadingTest : public testing::Test {
     auto eventIndexActualItr = spreadResult.postEventsIndeces.begin();
     auto eventTimeReferenceInd = ocs2::lookup::findIndexInTimeArray(updatedModeSchedule.eventTimes, period.first);
     for (size_t k = 0; k < spreadResult.timeTrajectory.size(); k++) {
-      // time should be monotonic sequence
-      if (k > 0) {
-        EXPECT_TRUE(spreadResult.timeTrajectory[k - 1] < spreadResult.timeTrajectory[k]);
+      // Time should be monotonic sequence except the final time. It is possible that the last two time points have the same time, but one
+      // stands for pre-mode and the other stands for post-mode.
+      if (k > 0 && k < spreadResult.timeTrajectory.size() - 1) {
+        EXPECT_TRUE(spreadResult.timeTrajectory[k - 1] < spreadResult.timeTrajectory[k]) << "TimeIndex: " << k;
       }
 
       // Pre-event time should be equal to the event time
@@ -238,7 +255,9 @@ class TrajectorySpreadingTest : public testing::Test {
         eventIndexActualItr++;
       }
       // mode should match the given modeSchedule
-      EXPECT_TRUE(updatedModeSchedule.modeAtTime(spreadResult.timeTrajectory[k]) == spreadResult.modeTrajectory[k]);
+      auto updatedMode =
+          modeAtTime(updatedModeSchedule, spreadResult.timeTrajectory[k], k == spreadResult.timeTrajectory.size() - 1 ? true : false);
+      EXPECT_TRUE(updatedMode == spreadResult.modeTrajectory[k]);
     }  // end of k loop
 
     // test postEventsIndeces
@@ -319,6 +338,48 @@ TEST_F(TrajectorySpreadingTest, partially_matching_modes) {
   EXPECT_TRUE(status.willPerformTrajectorySpreading);
 }
 
+TEST_F(TrajectorySpreadingTest, final_time_is_the_same_as_event_time_1) {
+  const ocs2::scalar_array_t eventTimes{1.1, 1.3};
+  const ocs2::size_array_t modeSequence{0, 1, 2};
+
+  const ocs2::scalar_array_t updatedEventTimes{1.1, 2.1};
+  const ocs2::size_array_t updatedModeSequence{0, 1, 2};
+
+  const std::pair<ocs2::scalar_t, ocs2::scalar_t> period{0.2, 2.1};
+  const auto status = checkResults({eventTimes, modeSequence}, {updatedEventTimes, updatedModeSequence}, period);
+
+  EXPECT_FALSE(status.willTruncate);
+  EXPECT_TRUE(status.willPerformTrajectorySpreading);
+}
+
+TEST_F(TrajectorySpreadingTest, final_time_is_the_same_as_event_time_2) {
+  const ocs2::scalar_array_t eventTimes{1.1, 2.1};
+  const ocs2::size_array_t modeSequence{0, 1, 2};
+
+  const ocs2::scalar_array_t updatedEventTimes{1.1, 1.3};
+  const ocs2::size_array_t updatedModeSequence{0, 1, 2};
+
+  const std::pair<ocs2::scalar_t, ocs2::scalar_t> period{0.2, 2.1};
+  const auto status = checkResults({eventTimes, modeSequence}, {updatedEventTimes, updatedModeSequence}, period);
+
+  EXPECT_FALSE(status.willTruncate);
+  EXPECT_TRUE(status.willPerformTrajectorySpreading);
+}
+
+TEST_F(TrajectorySpreadingTest, erase_trajectory) {
+  const ocs2::scalar_array_t eventTimes{1.1, 1.3};
+  const ocs2::size_array_t modeSequence{0, 1, 2};
+
+  const ocs2::scalar_array_t updatedEventTimes{1.1, 1.3};
+  const ocs2::size_array_t updatedModeSequence{0, 1, 3};
+
+  const std::pair<ocs2::scalar_t, ocs2::scalar_t> period{0.2, 2.1};
+  const auto status = checkResults({eventTimes, modeSequence}, {updatedEventTimes, updatedModeSequence}, period);
+
+  EXPECT_TRUE(status.willTruncate);
+  EXPECT_FALSE(status.willPerformTrajectorySpreading);
+}
+
 TEST_F(TrajectorySpreadingTest, fully_matched_modes) {
   const ocs2::scalar_array_t eventTimes{1.1, 1.3};
   const ocs2::size_array_t modeSequence{0, 1, 2};