Repository: Simple-Robotics/aligator
Pull request #274: Allow customization of the initial solution.
Author: login: abussy-aldebaran
Labels: name: pr status ready
description: To merge
Created: 2025-03-05T14:28:03Z | Merged: 2025-03-14T13:26:01Z

By default, if TrajOptProblem is given an initial state, the initial solution is initialized with this initial state for the first element, and with the neutral state for the others. Both these states can have very different values, yielding poor starting values for xs, prev_xs and trial_xs. In the classical case where all the states are in the same stage space, one might want to start with a constant solution. Instead of trying to detect if this is a desirable behavior, or adding a configuration parameter, an initialization callback is added to the problem, which can be set by the user before calling the solver setup(). If it is not set, the previous default behavior applies.


Diff:
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 7017b38b4..77a9c01b6 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Only link against needed pinocchio libraries ([#260](https://github.com/Simple-Robotics/aligator/pull/260))
 - Use Pinocchio instantiated functions ([#261](https://github.com/Simple-Robotics/aligator/pull/261))
 - Link to pinocchio collision
+- Some internal code now uses `TrajOptProblemTpl::initializeSolution()` to initialize state-control trajectories ([#274](https://github.com/Simple-Robotics/aligator/pull/274))
 
 ### Fixed
 
@@ -23,6 +24,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Added
 
 - Add MPC test/example ([#272](https://github.com/Simple-Robotics/aligator/pull/272))
+- Allow customization of the initial solution, introduce initialization strategies ([#274](https://github.com/Simple-Robotics/aligator/pull/274))
 - Add a collision distance residual for collision pair
 - Add a relaxed log-barrier cost function
 - Add Nix support ([#268](https://github.com/Simple-Robotics/aligator/pull/268))
diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in
index bff8db6ca..34b4c8b27 100644
--- a/doc/Doxyfile.extra.in
+++ b/doc/Doxyfile.extra.in
@@ -25,7 +25,7 @@ PREDEFINED              += EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
                            ALIGATOR_RAISE_IF_NAN
 
 FULL_PATH_NAMES         = YES
-EXCLUDE_SYMBOLS         = *::internal, internal::*, *::internal::*
+EXCLUDE_SYMBOLS         = *::internal, internal::*, *::internal::*, boost::*, fmt::*
 # Do not show source files
 EXCLUDE_PATTERNS        = *.hxx
 
@@ -62,7 +62,7 @@ SOURCE_BROWSER          = YES
 
 ALPHABETICAL_INDEX      = YES
 
-USE_MDFILE_AS_MAINPAGE  = README.md
+USE_MDFILE_AS_MAINPAGE  = @PROJECT_SOURCE_DIR@/README.md
 
 BUILTIN_STL_SUPPORT     = YES
 HAVE_DOT                = YES
diff --git a/include/aligator/core/solver-util.hpp b/include/aligator/core/solver-util.hpp
index fd1b462b1..4219f739e 100644
--- a/include/aligator/core/solver-util.hpp
+++ b/include/aligator/core/solver-util.hpp
@@ -1,13 +1,13 @@
 /// @file solver-util.hpp
 /// @brief Common utilities for all solvers.
-/// @copyright Copyright (C) 2022 LAAS-CNRS, INRIA
+/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2025 INRIA
 #pragma once
 
 #include "aligator/core/traj-opt-problem.hpp"
 
 namespace aligator {
 
-/// @brief Default-intialize a trajectory to the neutral states for each state
+/// @brief Default-initialize a trajectory to the neutral states for each state
 /// space at each stage.
 template <typename Scalar>
 void xs_default_init(const TrajOptProblemTpl<Scalar> &problem,
@@ -44,31 +44,6 @@ void us_default_init(const TrajOptProblemTpl<Scalar> &problem,
   }
 }
 
-template <typename Scalar>
-auto problemInitializeSolution(const TrajOptProblemTpl<Scalar> &problem) {
-  using VectorXs = typename math_types<Scalar>::VectorXs;
-  std::vector<VectorXs> xs, us, vs, lbdas;
-  const size_t nsteps = problem.numSteps();
-  xs_default_init(problem, xs);
-  us_default_init(problem, us);
-  // initialize multipliers...
-  vs.resize(nsteps + 1);
-  lbdas.resize(nsteps + 1);
-  lbdas[0].setZero(problem.init_constraint_->nr);
-  for (size_t i = 0; i < nsteps; i++) {
-    const StageModelTpl<Scalar> &sm = *problem.stages_[i];
-    lbdas[i + 1].setZero(sm.ndx2());
-    vs[i].setZero(sm.nc());
-  }
-
-  if (!problem.term_cstrs_.empty()) {
-    vs[nsteps].setZero(problem.term_cstrs_.totalDim());
-  }
-
-  return std::make_tuple(std::move(xs), std::move(us), std::move(vs),
-                         std::move(lbdas));
-}
-
 /// @brief Assign a vector of Eigen types into another, ensure there is no
 /// resize
 template <typename T1, typename T2>
@@ -93,28 +68,34 @@ template <typename T1, typename T2>
   return true;
 }
 
+namespace detail {
 /// @brief Check the input state-control trajectory is a consistent warm-start
 /// for the output.
+///
+/// @details If the state trajectory @p xs_in is empty, then both states and
+/// controls will be reinitialized using the @p problem object's set
+/// initialization strategy. Otherwise, if the controls container is empty, they
+/// (**only** the controls) will be default-initialized. Finally, if neither
+/// are empty, we attempt to assign the given @p xs_in and
+/// @p us_in values.
 template <typename Scalar>
-void check_trajectory_and_assign(
+void check_initial_guess_and_assign(
     const TrajOptProblemTpl<Scalar> &problem,
-    const typename math_types<Scalar>::VectorOfVectors &xs_init,
-    const typename math_types<Scalar>::VectorOfVectors &us_init,
+    const typename math_types<Scalar>::VectorOfVectors &xs_in,
+    const typename math_types<Scalar>::VectorOfVectors &us_in,
     typename math_types<Scalar>::VectorOfVectors &xs_out,
     typename math_types<Scalar>::VectorOfVectors &us_out) {
-  const std::size_t nsteps = problem.numSteps();
-  xs_out.reserve(nsteps + 1);
-  us_out.reserve(nsteps);
-  if (xs_init.empty()) {
-    xs_default_init(problem, xs_out);
-  } else if (!assign_no_resize(xs_init, xs_out)) {
-    ALIGATOR_RUNTIME_ERROR("warm-start for xs has wrong size!");
-  }
-  if (us_init.empty()) {
+  if (xs_in.empty()) {
+    problem.initializeSolution(xs_out, us_out);
+  } else if (us_in.empty()) {
     us_default_init(problem, us_out);
-  } else if (!assign_no_resize(us_init, us_out)) {
-    ALIGATOR_RUNTIME_ERROR("warm-start for us has wrong size!");
+  } else {
+    if (!assign_no_resize(xs_in, xs_out))
+      ALIGATOR_RUNTIME_ERROR("warm-start for xs has wrong size!");
+    if (!assign_no_resize(us_in, us_out))
+      ALIGATOR_RUNTIME_ERROR("warm-start for us has wrong size!");
   }
 }
+} // namespace detail
 
 } // namespace aligator
diff --git a/include/aligator/core/traj-opt-problem.hpp b/include/aligator/core/traj-opt-problem.hpp
index 3fabfd5c5..9cab43a50 100644
--- a/include/aligator/core/traj-opt-problem.hpp
+++ b/include/aligator/core/traj-opt-problem.hpp
@@ -1,5 +1,5 @@
 /// @file
-/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, INRIA
+/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2025 INRIA
 #pragma once
 
 #include "aligator/core/stage-model.hpp"
@@ -21,7 +21,11 @@ namespace aligator {
  * \f]
  */
 template <typename _Scalar> struct TrajOptProblemTpl {
+  using Self = TrajOptProblemTpl;
   using Scalar = _Scalar;
+
+  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
+
   using StageModel = StageModelTpl<Scalar>;
   using StageFunction = StageFunctionTpl<Scalar>;
   using UnaryFunction = UnaryFunctionTpl<Scalar>;
@@ -34,6 +38,8 @@ template <typename _Scalar> struct TrajOptProblemTpl {
 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
   using StageConstraint = StageConstraintTpl<Scalar>;
 #pragma GCC diagnostic pop
+  using InitializationStrategy =
+      std::function<void(const Self &, std::vector<VectorXs> &)>;
 
   /**
    * @page trajoptproblem Trajectory optimization problems
@@ -89,8 +95,6 @@ template <typename _Scalar> struct TrajOptProblemTpl {
    *
    */
 
-  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
-
   /// Initial condition
   xyz::polymorphic<UnaryFunction> init_constraint_;
   /// Stages of the control problem.
@@ -162,7 +166,7 @@ template <typename _Scalar> struct TrajOptProblemTpl {
   /// @brief Remove all terminal constraints.
   void removeTerminalConstraints() { term_cstrs_.clear(); }
 
-  std::size_t numSteps() const;
+  [[nodiscard]] std::size_t numSteps() const;
 
   /// @brief Rollout the problem costs, constraints, dynamics, stage per stage.
   Scalar evaluate(const std::vector<VectorXs> &xs,
@@ -189,7 +193,60 @@ template <typename _Scalar> struct TrajOptProblemTpl {
 
   bool checkIntegrity() const;
 
-protected:
+  /// @brief Set a function to initialize the state trajectory.
+  //
+  /// The class constructor defaults the strategy function to xs_default_init.
+  /// @warning Call this set before the solver's setup().
+  /// @tparam Callable Functional type convertible to InitializationStrategy.
+  ///
+  /// @sa initializeSolution()
+  template <typename Callable> void setInitializationStrategy(Callable &&func) {
+    static_assert(std::is_convertible_v<Callable, InitializationStrategy>);
+    this->xs_init_strategy_ = std::forward<Callable>(func);
+  }
+
+  /// @brief Execute the initialization strategy to generate an initial
+  /// candidate solution to the problem.
+  ///
+  /// @sa setInitializationStrategy()
+  void initializeSolution(std::vector<VectorXs> &xs,
+                          std::vector<VectorXs> &us) const {
+    xs_init_strategy_(*this, xs);
+    us_default_init(*this, us);
+  }
+
+  /// @copydoc initializeSolution()
+  void initializeSolution(std::vector<VectorXs> &xs, std::vector<VectorXs> &us,
+                          std::vector<VectorXs> &vs,
+                          std::vector<VectorXs> &lbdas) const {
+    const size_t nsteps = numSteps();
+    initializeSolution(xs, us);
+    // initialize multipliers...
+    vs.resize(nsteps + 1);
+    lbdas.resize(nsteps + 1);
+    lbdas[0].setZero(init_constraint_->nr);
+    for (size_t i = 0; i < nsteps; i++) {
+      const StageModelTpl<Scalar> &sm = *stages_[i];
+      lbdas[i + 1].setZero(sm.ndx2());
+      vs[i].setZero(sm.nc());
+    }
+
+    if (!term_cstrs_.empty()) {
+      vs[nsteps].setZero(term_cstrs_.totalDim());
+    }
+  }
+
+  /// @copydoc initializeSolution()
+  [[nodiscard]] auto initializeSolution() const {
+    std::vector<VectorXs> xs, us, vs, lbdas;
+    initializeSolution(xs, us, vs, lbdas);
+    return std::make_tuple(std::move(xs), std::move(us), std::move(vs),
+                           std::move(lbdas));
+  }
+
+private:
+  InitializationStrategy xs_init_strategy_;
+
   // Check if the initial state is a StateErrorResidual.
   // Since this is a costly operation (dynamic_cast), we cache the result.
   bool checkInitCondIsStateError() const;
diff --git a/include/aligator/core/traj-opt-problem.hxx b/include/aligator/core/traj-opt-problem.hxx
index 9a14fcc9a..d6207a62f 100644
--- a/include/aligator/core/traj-opt-problem.hxx
+++ b/include/aligator/core/traj-opt-problem.hxx
@@ -1,9 +1,10 @@
 /// @file
-/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, INRIA
+/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2025 INRIA
 /// @brief Template definitions header.
 #pragma once
 
 #include "aligator/core/traj-opt-problem.hpp"
+#include "aligator/core/solver-util.hpp"
 #include "aligator/utils/mpc-util.hpp"
 #include "aligator/tracy.hpp"
 
@@ -18,6 +19,7 @@ TrajOptProblemTpl<Scalar>::TrajOptProblemTpl(
     xyz::polymorphic<CostAbstract> term_cost)
     : init_constraint_(std::move(init_constraint)), stages_(stages),
       term_cost_(std::move(term_cost)), unone_(term_cost_->nu),
+      xs_init_strategy_(xs_default_init<Scalar>),
       init_cond_is_state_error_(checkInitCondIsStateError()) {
   unone_.setZero();
 }
diff --git a/include/aligator/core/workspace-base.hpp b/include/aligator/core/workspace-base.hpp
index 18dd69492..8c0df60e7 100644
--- a/include/aligator/core/workspace-base.hpp
+++ b/include/aligator/core/workspace-base.hpp
@@ -60,10 +60,7 @@ template <typename Scalar>
 WorkspaceBaseTpl<Scalar>::WorkspaceBaseTpl(
     const TrajOptProblemTpl<Scalar> &problem)
     : m_isInitialized(true), nsteps(problem.numSteps()), problem_data(problem) {
-  trial_xs.resize(nsteps + 1);
-  trial_us.resize(nsteps);
-  xs_default_init(problem, trial_xs);
-  us_default_init(problem, trial_us);
+  problem.initializeSolution(trial_xs, trial_us);
 }
 
 template <typename Scalar> void WorkspaceBaseTpl<Scalar>::cycleLeft() {
diff --git a/include/aligator/solvers/fddp/results.hpp b/include/aligator/solvers/fddp/results.hpp
index 7a3e6b9cd..99ad4f9b6 100644
--- a/include/aligator/solvers/fddp/results.hpp
+++ b/include/aligator/solvers/fddp/results.hpp
@@ -29,8 +29,7 @@ ResultsFDDPTpl<Scalar>::ResultsFDDPTpl(
   xs.resize(nsteps + 1);
   us.resize(nsteps);
 
-  xs_default_init(problem, xs);
-  us_default_init(problem, us);
+  problem.initializeSolution(xs, us);
 
   gains_.resize(nsteps);
 
diff --git a/include/aligator/solvers/fddp/solver-fddp.hxx b/include/aligator/solvers/fddp/solver-fddp.hxx
index 5db40f119..c76add6b0 100644
--- a/include/aligator/solvers/fddp/solver-fddp.hxx
+++ b/include/aligator/solvers/fddp/solver-fddp.hxx
@@ -285,8 +285,8 @@ bool SolverFDDPTpl<Scalar>::run(const Problem &problem,
         "Either results or workspace not allocated. Call setup() first!");
   }
 
-  check_trajectory_and_assign(problem, xs_init, us_init, results_.xs,
-                              results_.us);
+  detail::check_initial_guess_and_assign(problem, xs_init, us_init, results_.xs,
+                                         results_.us);
   // optionally override xs[0]
   if (force_initial_condition_) {
     workspace_.trial_xs[0] = problem.getInitState();
diff --git a/include/aligator/solvers/proxddp/results.hxx b/include/aligator/solvers/proxddp/results.hxx
index e5b246df7..8bd590fea 100644
--- a/include/aligator/solvers/proxddp/results.hxx
+++ b/include/aligator/solvers/proxddp/results.hxx
@@ -1,5 +1,5 @@
 /// @file
-/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, INRIA
+/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2025 INRIA
 /// @brief Implementation file. Include when template definitions required.
 #pragma once
 
@@ -16,10 +16,7 @@ ResultsTpl<Scalar>::ResultsTpl(const TrajOptProblemTpl<Scalar> &problem)
     ALIGATOR_RUNTIME_ERROR("Problem failed integrity check.");
 
   const std::size_t nsteps = problem.numSteps();
-  std::tie(xs, us, vs, lams) = problemInitializeSolution(problem);
-
-  assert(xs.size() == nsteps + 1);
-  assert(us.size() == nsteps);
+  problem.initializeSolution(xs, us, vs, lams);
 
   gains_.resize(nsteps + 1);
   for (std::size_t i = 0; i < nsteps; i++) {
diff --git a/include/aligator/solvers/proxddp/solver-proxddp.hxx b/include/aligator/solvers/proxddp/solver-proxddp.hxx
index 42667eaba..b76d82cfc 100644
--- a/include/aligator/solvers/proxddp/solver-proxddp.hxx
+++ b/include/aligator/solvers/proxddp/solver-proxddp.hxx
@@ -470,8 +470,8 @@ bool SolverProxDDPTpl<Scalar>::run(const Problem &problem,
     setAlmPenalty(mu_init);
   }
 
-  check_trajectory_and_assign(problem, xs_init, us_init, results_.xs,
-                              results_.us);
+  detail::check_initial_guess_and_assign(problem, xs_init, us_init, results_.xs,
+                                         results_.us);
   const bool v_was_resized =
       !vs_init.empty() && !assign_no_resize(vs_init, results_.vs);
   const bool l_was_resized =
diff --git a/include/aligator/solvers/proxddp/workspace.hxx b/include/aligator/solvers/proxddp/workspace.hxx
index 30b18ef41..c47550fa6 100644
--- a/include/aligator/solvers/proxddp/workspace.hxx
+++ b/include/aligator/solvers/proxddp/workspace.hxx
@@ -1,5 +1,5 @@
 /// @file
-/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, INRIA
+/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2025 INRIA
 /// @brief Implementation file, to be included when necessary.
 #pragma once
 
@@ -18,8 +18,7 @@ WorkspaceTpl<Scalar>::WorkspaceTpl(const TrajOptProblemTpl<Scalar> &problem)
   if (!problem.checkIntegrity())
     ALIGATOR_RUNTIME_ERROR("Problem failed integrity check.");
 
-  std::tie(trial_xs, trial_us, trial_vs, trial_lams) =
-      problemInitializeSolution(problem);
+  problem.initializeSolution(trial_xs, trial_us, trial_vs, trial_lams);
   std::tie(prev_xs, prev_us, prev_vs, prev_lams) = {trial_xs, trial_us,
                                                     trial_vs, trial_lams};
 
diff --git a/tests/problem.cpp b/tests/problem.cpp
index b7727b73c..e3f381f15 100644
--- a/tests/problem.cpp
+++ b/tests/problem.cpp
@@ -1,6 +1,7 @@
 #include "aligator/core/traj-opt-problem.hpp"
 #include "aligator/core/traj-opt-data.hpp"
 #include "aligator/core/stage-data.hpp"
+#include "aligator/solvers/proxddp/solver-proxddp.hpp"
 #include "aligator/solvers/proxddp/results.hpp"
 #include "aligator/solvers/proxddp/workspace.hpp"
 #include "aligator/core/explicit-dynamics.hpp"
@@ -171,4 +172,66 @@ BOOST_AUTO_TEST_CASE(test_copy) {
   BOOST_CHECK_EQUAL(f.problem.getInitState()[0], 0.);
 }
 
+BOOST_AUTO_TEST_CASE(test_default_init) {
+  MyFixture f;
+
+  Eigen::VectorXd state = f.problem.getInitState();
+  BOOST_CHECK_EQUAL(state, f.space.neutral());
+
+  state[0] += 1.;
+  f.problem.setInitState(state);
+
+  auto ddp = SolverProxDDPTpl<double>();
+  ddp.setup(f.problem);
+
+  const auto nsteps = f.problem.numSteps();
+  for (auto i = 0u; i < nsteps + 1; i++) {
+    const auto &xs = ddp.results_.xs;
+    const auto &trial_xs = ddp.workspace_.trial_xs;
+    const auto &prev_xs = ddp.workspace_.prev_xs;
+
+    BOOST_REQUIRE_EQUAL(xs.size(), nsteps + 1);
+    BOOST_REQUIRE_EQUAL(trial_xs.size(), nsteps + 1);
+    BOOST_REQUIRE_EQUAL(prev_xs.size(), nsteps + 1);
+
+    const auto expected = (i == 0) ? state : f.space.neutral();
+
+    BOOST_CHECK_EQUAL(xs[i], expected);
+    BOOST_CHECK_EQUAL(trial_xs[i], expected);
+    BOOST_CHECK_EQUAL(prev_xs[i], expected);
+  }
+}
+
+BOOST_AUTO_TEST_CASE(test_constant_init) {
+  MyFixture f;
+
+  Eigen::VectorXd state = f.problem.getInitState();
+  BOOST_CHECK_EQUAL(state, f.space.neutral());
+
+  state[0] += 1.;
+  f.problem.setInitState(state);
+
+  f.problem.setInitializationStrategy([](const auto &problem, auto &xs) {
+    xs.assign(problem.numSteps() + 1, problem.getInitState());
+  });
+
+  auto ddp = SolverProxDDPTpl<double>();
+  ddp.setup(f.problem);
+
+  const auto nsteps = f.problem.numSteps();
+  for (auto i = 0u; i < nsteps + 1; i++) {
+    const auto &xs = ddp.results_.xs;
+    const auto &trial_xs = ddp.workspace_.trial_xs;
+    const auto &prev_xs = ddp.workspace_.prev_xs;
+
+    BOOST_REQUIRE_EQUAL(xs.size(), nsteps + 1);
+    BOOST_REQUIRE_EQUAL(trial_xs.size(), nsteps + 1);
+    BOOST_REQUIRE_EQUAL(prev_xs.size(), nsteps + 1);
+
+    BOOST_CHECK_EQUAL(xs[i], state);
+    BOOST_CHECK_EQUAL(trial_xs[i], state);
+    BOOST_CHECK_EQUAL(prev_xs[i], state);
+  }
+}
+
 BOOST_AUTO_TEST_SUITE_END()


Reviews:
user: login: ManifoldFR
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-03-10T10:38:19Z
user: login: ManifoldFR
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-03-10T10:39:07Z
user: login: abussy-aldebaran
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-12T07:55:31Z
user: login: ManifoldFR
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-03-13T15:59:40Z
user: login: ManifoldFR
state: APPROVED
author_association: MEMBER
submitted_at: 2025-03-14T13:25:00Z

Review comments:
diff_hunk: @@ -101,6 +101,8 @@ template <typename _Scalar> struct TrajOptProblemTpl {
   ConstraintStackTpl<Scalar> term_cstrs_;
   /// Dummy, "neutral" control value.
   VectorXs unone_;
+  /// Function to initialize the state trajectory. If not set, uses xs_default_init.
+  std::function<void(const TrajOptProblemTpl &, std::vector<VectorXs> &)> xs_init_;
path: include/aligator/core/traj-opt-problem.hpp
user: login: ManifoldFR
body: You should maybe introduce a typedef for this function type (if we decide to keep using the type-erased `std::function`)
created_at: 2025-03-10T10:38:19Z
author_association: MEMBER
diff_hunk: @@ -12,6 +12,8 @@ namespace aligator {
 template <typename Scalar>
 void xs_default_init(const TrajOptProblemTpl<Scalar> &problem,
                      std::vector<typename math_types<Scalar>::VectorXs> &xs) {
+  if (problem.xs_init_)
+    return problem.xs_init_(problem, xs);
path: include/aligator/core/solver-util.hpp
user: login: ManifoldFR
body: Not a fan of this, the function is called `default`. Maybe this function should be the default value of `TrajOptProblemTpl::xs_init_` ?
created_at: 2025-03-10T10:39:07Z
author_association: MEMBER
diff_hunk: @@ -12,6 +12,8 @@ namespace aligator {
 template <typename Scalar>
 void xs_default_init(const TrajOptProblemTpl<Scalar> &problem,
                      std::vector<typename math_types<Scalar>::VectorXs> &xs) {
+  if (problem.xs_init_)
+    return problem.xs_init_(problem, xs);
path: include/aligator/core/solver-util.hpp
user: login: abussy-aldebaran
body: I agree. I went for this solution because it introduces the minimum amount of change.
created_at: 2025-03-12T07:55:31Z
author_association: CONTRIBUTOR
diff_hunk: @@ -12,6 +12,8 @@ namespace aligator {
 template <typename Scalar>
 void xs_default_init(const TrajOptProblemTpl<Scalar> &problem,
                      std::vector<typename math_types<Scalar>::VectorXs> &xs) {
+  if (problem.xs_init_)
+    return problem.xs_init_(problem, xs);
path: include/aligator/core/solver-util.hpp
user: login: ManifoldFR
body: I think my initial design of how trajectories are initialized is rather crap actually, this could be the opportunity for a better rewrite 😆
I'll get more involved in this PR if you don't mind!
created_at: 2025-03-13T15:59:40Z
author_association: MEMBER

PR comments:
user: login: ManifoldFR
created_at: 2025-03-10T10:21:57Z
body: Hey, sorry for not getting back to you sooner. I'll take a look.
author_association: MEMBER
user: login: ManifoldFR
created_at: 2025-03-13T16:47:46Z
body: I've rebased on `devel` after merging #272.
author_association: MEMBER
user: login: ManifoldFR
created_at: 2025-03-13T18:32:18Z
body: @abussy-aldebaran I've redone some of the design and centralized trajectory initialization in the code around a new member function `problem.executeInitialization()`, which calls the state-initialization strategy internally. What do you think?
author_association: MEMBER
user: login: ManifoldFR
created_at: 2025-03-13T18:43:48Z
body: With respect to exposing this stuff to Python, we'll wait until we transition to nanobind, which actually has casters and a [sane way of handling `std::function<R(T...)` parameters](https://nanobind.readthedocs.io/en/latest/functions.html#higher-order-functions).

I looked at what Boost.Python has, and as expected it's a big _nothing_ unless we write something convoluted.

cc @jorisv 
author_association: MEMBER
user: login: abussy-aldebaran
created_at: 2025-03-14T08:22:31Z
body: Looks good to me !

I'd like to add a bit of context to this PR, that I forgot to mention. The solution I first considered was to pass `xs`, `us` and all to `setup()`. That would probably be the straightforward solution. But it would require a lot of overloading, and other changes. It looked a bit too much to me, when using a strategy was way simpler to implement.
author_association: CONTRIBUTOR
user: login: ManifoldFR
created_at: 2025-03-14T13:16:00Z
body: > Looks good to me !
> 
> I'd like to add a bit of context to this PR, that I forgot to mention. The solution I first considered was to pass `xs`, `us` and all to `setup()`. That would probably be the straightforward solution. But it would require a lot of overloading, and other changes. It looked a bit too much to me, when using a strategy was way simpler to implement.

You can provide initial guesses for `xs`, `us` (and even the multipliers) to `SolverProxDDPTpl::run()`, though?
author_association: MEMBER
user: login: abussy-aldebaran
created_at: 2025-03-14T14:06:38Z
body: It is really necessary to init `prev_xs` as well, to have meaningful `dxs`, if I'm not mistaken.
author_association: CONTRIBUTOR