Merged main into feature/perception

Author: farbod-farshidian

ocs2_ddp/CMakeLists.txt

@@ -191,3 +191,13 @@ target_link_libraries(testContinuousTimeLqr
   ${PROJECT_NAME}
   gtest_main
 )
+
+catkin_add_gtest(testDdpHelperFunction
+  test/testDdpHelperFunction.cpp
+)
+target_link_libraries(testDdpHelperFunction
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${PROJECT_NAME}
+  gtest_main
+)

ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h

@@ -80,6 +80,20 @@ PerformanceIndex computeRolloutPerformanceIndex(const scalar_array_t& timeTrajec
 scalar_t rolloutTrajectory(RolloutBase& rollout, scalar_t initTime, const vector_t& initState, scalar_t finalTime,
                            PrimalSolution& primalSolution);
 
+/**
+ * Extract a primal solution for the range [initTime, finalTime] from a given primal solution. It assumes that the
+ * given range is within the solution time of input primal solution.
+ *
+ * @note: The controller field is ignored.
+ * @note: The extracted primal solution can have an event time at final time but ignores it at initial time.
+ *
+ * @param [in] timePeriod: The time period for which the solution should be extracted.
+ * @param [in] inputPrimalSolution: The input PrimalSolution
+ * @param [out] outputPrimalSolution: The output PrimalSolution.
+ */
+void extractPrimalSolution(const std::pair<scalar_t, scalar_t>& timePeriod, const PrimalSolution& inputPrimalSolution,
+                           PrimalSolution& outputPrimalSolution);
+
 /**
  * Computes the integral of the squared (IS) norm of the controller update.
  *

ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h

@@ -250,19 +250,15 @@ class GaussNewtonDDP : public SolverBase {
   std::vector<std::pair<int, int>> getPartitionIntervalsFromTimeTrajectory(const scalar_array_t& timeTrajectory, int numWorkers);
 
   /**
-   * Forward integrate the system dynamics with given controller 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].
-   * However, if the provided controller does not cover the period [initTime, finalTime], it extrapolates (zero-order)
-   * the controller until the next event time where after it uses the operating trajectories.
+   * 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].
+   * However, if the provided controller does not cover the period [initTime, finalTime], it will use the controller till the
+   * final time of the controller and after it uses the operating trajectories.
    *
-   * Attention: Do NOT pass the controllerPtr of the same primalData used for the first parameter to the second parameter, as all
-   * member variables(including controller) of primal data will be cleared.
-   *
-   * @param [out] primalData: primalData
-   * @param [in] controller: nominal controller used to rollout (time, state, input...) trajectories
-   * @param [in] workerIndex: working thread (default is 0).
+   * @param [in, out] primalSolution: The resulting state-input trajectory. The primal solution is initialized with the controller
+   *                                  and the modeSchedule. However, for StateTriggered Rollout the modeSchedule can be overwritten.
    */
-  void rolloutInitialTrajectory(PrimalDataContainer& primalData, ControllerBase* controller, size_t workerIndex = 0);
+  void rolloutInitialTrajectory(PrimalSolution& primalSolution);
 
   /**
    * Calculates the controller. This method uses the following variables. The method modifies unoptimizedController_.
@@ -364,9 +360,7 @@ class GaussNewtonDDP : public SolverBase {
   std::pair<bool, std::string> checkConvergence(bool isInitalControllerEmpty, const PerformanceIndex& previousPerformanceIndex,
                                                 const PerformanceIndex& currentPerformanceIndex) const;
 
-  void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override {
-    runImpl(initTime, initState, finalTime, nullptr);
-  }
+  void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override;
 
   void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const ControllerBase* externalControllerPtr) override;
 
@@ -407,8 +401,8 @@ class GaussNewtonDDP : public SolverBase {
   PerformanceIndex performanceIndex_;
   std::vector<PerformanceIndex> performanceIndexHistory_;
 
+  std::unique_ptr<RolloutBase> initializerRolloutPtr_;
   std::vector<std::unique_ptr<RolloutBase>> dynamicsForwardRolloutPtrStock_;
-  std::vector<std::unique_ptr<RolloutBase>> initializerRolloutPtrStock_;
 
   // optimized data
   DualSolution optimizedDualSolution_;

ocs2_ddp/include/ocs2_ddp/SLQ.h

@@ -32,8 +32,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_core/integration/Integrator.h>
 #include <ocs2_core/integration/SystemEventHandler.h>
 
-#include "GaussNewtonDDP.h"
-#include "riccati_equations/ContinuousTimeRiccatiEquations.h"
+#include "ocs2_ddp/GaussNewtonDDP.h"
+#include "ocs2_ddp/riccati_equations/ContinuousTimeRiccatiEquations.h"
 
 namespace ocs2 {
 
@@ -89,23 +89,6 @@ class SLQ final : public GaussNewtonDDP {
                                            scalar_array_t& SsNormalizedTime, size_array_t& SsNormalizedPostEventIndices,
                                            vector_array_t& allSsTrajectory);
 
-  /**
-   * Integrates the riccati equation and freely selects the time nodes for the value function.
-   *
-   * @param riccatiIntegrator [in] : Riccati integrator object
-   * @param riccatiEquation [in] : Riccati equation object
-   * @param nominalTimeTrajectory [in] : time trajectory produced in the forward rollout.
-   * @param nominalEventsPastTheEndIndices [in] : Indices into nominalTimeTrajectory to point to times right after event times
-   * @param allSsFinal [in] : Final value of the value function.
-   * @param SsNormalizedTime [out] : Time trajectory of the value function.
-   * @param SsNormalizedPostEventIndices [out] : Indices into SsNormalizedTime to point to times right after event times
-   * @param allSsTrajectory [out] : Value function in vector format.
-   */
-  void integrateRiccatiEquationAdaptiveTime(IntegratorBase& riccatiIntegrator, ContinuousTimeRiccatiEquations& riccatiEquation,
-                                            const scalar_array_t& nominalTimeTrajectory, const size_array_t& nominalEventsPastTheEndIndices,
-                                            vector_t allSsFinal, scalar_array_t& SsNormalizedTime,
-                                            size_array_t& SsNormalizedPostEventIndices, vector_array_t& allSsTrajectory);
-
   /****************
    *** Variables **
    ****************/

ocs2_ddp/src/DDP_HelperFunctions.cpp

@@ -34,10 +34,29 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <ocs2_core/PreComputation.h>
 #include <ocs2_core/integration/TrapezoidalIntegration.h>
+#include <ocs2_core/misc/LinearInterpolation.h>
 #include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h>
 
 namespace ocs2 {
 
+namespace {
+template <typename DataType>
+void copySegment(const LinearInterpolation::index_alpha_t& indexAlpha0, const LinearInterpolation::index_alpha_t& indexAlpha1,
+                 const std::vector<DataType>& inputTrajectory, std::vector<DataType>& outputTrajectory) {
+  outputTrajectory.clear();
+  outputTrajectory.resize(2 + indexAlpha1.first - indexAlpha0.first);
+
+  if (!outputTrajectory.empty()) {
+    outputTrajectory.front() = LinearInterpolation::interpolate(indexAlpha0, inputTrajectory);
+    if (indexAlpha1.first >= indexAlpha0.first) {
+      std::copy(inputTrajectory.begin() + indexAlpha0.first + 1, inputTrajectory.begin() + indexAlpha1.first + 1,
+                outputTrajectory.begin() + 1);
+    }
+    outputTrajectory.back() = LinearInterpolation::interpolate(indexAlpha1, inputTrajectory);
+  }
+}
+}  // unnamed namespace
+
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
@@ -165,6 +184,74 @@ scalar_t rolloutTrajectory(RolloutBase& rollout, scalar_t initTime, const vector
   return (finalTime - initTime) / static_cast<scalar_t>(primalSolution.timeTrajectory_.size());
 }
 
+/******************************************************************************************************/
+/******************************************************************************************************/
+/******************************************************************************************************/
+void extractPrimalSolution(const std::pair<scalar_t, scalar_t>& timePeriod, const PrimalSolution& inputPrimalSolution,
+                           PrimalSolution& outputPrimalSolution) {
+  // no controller
+  if (outputPrimalSolution.controllerPtr_ != nullptr) {
+    outputPrimalSolution.controllerPtr_->clear();
+  }
+  // for none StateTriggeredRollout initialize modeSchedule
+  outputPrimalSolution.modeSchedule_ = inputPrimalSolution.modeSchedule_;
+
+  // create alias
+  auto& timeTrajectory = outputPrimalSolution.timeTrajectory_;
+  auto& stateTrajectory = outputPrimalSolution.stateTrajectory_;
+  auto& inputTrajectory = outputPrimalSolution.inputTrajectory_;
+  auto& postEventIndices = outputPrimalSolution.postEventIndices_;
+
+  /*
+   * Find the indexAlpha pair for interpolation. The interpolation function uses the std::lower_bound while ignoring the initial
+   * time event, we should use std::upper_bound. Therefore at the first step, we check if for the case where std::upper_bound
+   * would have give a different solution (index_alpha_t::second = 0) and correct the pair. Then, in the second step, we check
+   * whether the index_alpha_t::first is a pre-event index. If yes, we move index_alpha_t::first to the post-event index.
+   */
+  const auto indexAlpha0 = [&]() {
+    const auto lowerBoundIndexAlpha = LinearInterpolation::timeSegment(timePeriod.first, inputPrimalSolution.timeTrajectory_);
+
+    const auto upperBoundIndexAlpha = numerics::almost_eq(lowerBoundIndexAlpha.second, 0.0)
+                                          ? LinearInterpolation::index_alpha_t{lowerBoundIndexAlpha.first + 1, 1.0}
+                                          : lowerBoundIndexAlpha;
+    const auto it = std::find(inputPrimalSolution.postEventIndices_.cbegin(), inputPrimalSolution.postEventIndices_.cend(),
+                              upperBoundIndexAlpha.first + 1);
+    if (it == inputPrimalSolution.postEventIndices_.cend()) {
+      return upperBoundIndexAlpha;
+    } else {
+      return LinearInterpolation::index_alpha_t{upperBoundIndexAlpha.first + 1, 1.0};
+    }
+  }();
+  const auto indexAlpha1 = LinearInterpolation::timeSegment(timePeriod.second, inputPrimalSolution.timeTrajectory_);
+
+  // time
+  copySegment(indexAlpha0, indexAlpha1, inputPrimalSolution.timeTrajectory_, timeTrajectory);
+
+  // state
+  copySegment(indexAlpha0, indexAlpha1, inputPrimalSolution.stateTrajectory_, stateTrajectory);
+
+  // input
+  copySegment(indexAlpha0, indexAlpha1, inputPrimalSolution.inputTrajectory_, inputTrajectory);
+
+  // If the pre-event index is within the range we accept the event
+  postEventIndices.clear();
+  for (const auto& postIndex : inputPrimalSolution.postEventIndices_) {
+    if (postIndex > static_cast<size_t>(indexAlpha0.first) && inputPrimalSolution.timeTrajectory_[postIndex - 1] <= timePeriod.second) {
+      postEventIndices.push_back(postIndex - static_cast<size_t>(indexAlpha0.first));
+    }
+  }
+
+  // If there is an event at final time, it misses its pair (due to indexAlpha1 and copySegment)
+  if (!postEventIndices.empty() && postEventIndices.back() == timeTrajectory.size()) {
+    constexpr auto eps = numeric_traits::weakEpsilon<scalar_t>();
+    const auto indexAlpha2 = LinearInterpolation::timeSegment(timePeriod.second + eps, inputPrimalSolution.timeTrajectory_);
+
+    timeTrajectory.push_back(std::min(timePeriod.second + eps, timePeriod.second));
+    stateTrajectory.push_back(LinearInterpolation::interpolate(indexAlpha2, inputPrimalSolution.stateTrajectory_));
+    inputTrajectory.push_back(LinearInterpolation::interpolate(indexAlpha2, inputPrimalSolution.inputTrajectory_));
+  }
+}
+
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/