core: have TrajOptProblem::replaceStageCircular() return ejected first stage

CHANGELOG.md

@@ -22,6 +22,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Changed
 
+- core : have `replaceStageCircular()` return ejected knot (https://github.com/Simple-Robotics/aligator/pull/376)
 - python : do not import utils.plotting automatically (https://github.com/Simple-Robotics/aligator/pull/369)
 - ci: display ccache statistics (https://github.com/Simple-Robotics/aligator/pull/369)
 - pixi: update environments (https://github.com/Simple-Robotics/aligator/pull/369)

include/aligator/core/traj-opt-problem.hpp

@@ -210,9 +210,11 @@ template <typename _Scalar> struct TrajOptProblemTpl {
                           std::size_t num_threads = 1,
                           bool compute_second_order = true) const;
 
-  /// @brief Pop out the first StageModel and replace by the supplied one;
-  /// updates the supplied problem data (TrajOptDataTpl) object.
-  void replaceStageCircular(const xyz::polymorphic<StageModel> &model);
+  /// @brief Advance each stage model by one slot after inserting a new stage,
+  /// pop out the first model.
+  /// @returns The first StageModel.
+  xyz::polymorphic<StageModel>
+  replaceStageCircular(const xyz::polymorphic<StageModel> &model);
 
   bool checkIntegrity() const;
 

include/aligator/core/traj-opt-problem.hxx

@@ -164,11 +164,13 @@ inline std::size_t TrajOptProblemTpl<Scalar>::numSteps() const {
 }
 
 template <typename Scalar>
-void TrajOptProblemTpl<Scalar>::replaceStageCircular(
-    const xyz::polymorphic<StageModel> &model) {
+auto TrajOptProblemTpl<Scalar>::replaceStageCircular(
+    const xyz::polymorphic<StageModel> &model) -> xyz::polymorphic<StageModel> {
   addStage(model);
   rotate_vec_left(stages_);
+  auto last = std::move(stages_.back());
   stages_.pop_back();
+  return last;
 }
 
 template <typename Scalar>