Merge pull request #265 from abussy-aldebaran/topic/fix-traj-opt-pb

Fix copy of TrajOptProblem

CHANGELOG.md

@@ -15,6 +15,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Fixed
 
+- Fixed copy of TrajOptProblem ([#265](https://github.com/Simple-Robotics/aligator/pull/265))
 - `LinesearchVariant::init()` should not be called unless the step accpetance strategy is a linesearch
 - Fixed compilation issues with C++20 (resolving [#246](https://github.com/Simple-Robotics/aligator/issues/246) and [#254](https://github.com/Simple-Robotics/aligator/discussions/254))
 

README.md

@@ -133,6 +133,7 @@ Please also consider citing the reference paper for the ProxDDP algorithm:
 * [Fabian Schramm](https://github.com/fabinsch) (Inria): core developer
 * [Ludovic De Matteïs](https://github.com/LudovicDeMatteis) (LAAS-CNRS/Inria): feature developer
 * [Ewen Dantec](https://edantec.github.io/) (Inria): feature developer
+* [Antoine Bussy](https://github.com/antoine-bussy) (Aldebaran)
 
 ## Acknowledgments
 

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

@@ -127,7 +127,10 @@ template <typename _Scalar> struct TrajOptProblemTpl {
                     xyz::polymorphic<Manifold> space,
                     xyz::polymorphic<CostAbstract> term_cost);
 
-  bool initCondIsStateError() const { return init_state_error_ != nullptr; }
+  bool initCondIsStateError() const {
+    assert(init_cond_is_state_error_ == checkInitCondIsStateError());
+    return init_cond_is_state_error_;
+  }
 
   /// @brief Add a stage to the control problem.
   void addStage(const xyz::polymorphic<StageModel> &stage);
@@ -138,7 +141,7 @@ template <typename _Scalar> struct TrajOptProblemTpl {
       ALIGATOR_RUNTIME_ERROR(
           "Initial condition is not a StateErrorResidual.\n");
     }
-    return init_state_error_->target_;
+    return static_cast<StateErrorResidual const *>(&*init_constraint_)->target_;
   }
 
   /// @brief Set initial state constraint.
@@ -147,7 +150,7 @@ template <typename _Scalar> struct TrajOptProblemTpl {
       ALIGATOR_RUNTIME_ERROR(
           "Initial condition is not a StateErrorResidual.\n");
     }
-    init_state_error_->target_ = x0;
+    static_cast<StateErrorResidual *>(&*init_constraint_)->target_ = x0;
   }
 
   /// @brief Add a terminal constraint for the model.
@@ -187,8 +190,10 @@ template <typename _Scalar> struct TrajOptProblemTpl {
   bool checkIntegrity() const;
 
 protected:
-  /// Pointer to underlying state error residual
-  StateErrorResidual *init_state_error_;
+  // Check if the initial state is a StateErrorResidual.
+  // Since this is a costly operation (dynamic_cast), we cache the result.
+  bool checkInitCondIsStateError() const;
+  bool init_cond_is_state_error_ = false;
 };
 
 namespace internal {

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

@@ -17,9 +17,9 @@ TrajOptProblemTpl<Scalar>::TrajOptProblemTpl(
     const std::vector<xyz::polymorphic<StageModel>> &stages,
     xyz::polymorphic<CostAbstract> term_cost)
     : init_constraint_(std::move(init_constraint)), stages_(stages),
-      term_cost_(std::move(term_cost)), unone_(term_cost_->nu) {
+      term_cost_(std::move(term_cost)), unone_(term_cost_->nu),
+      init_cond_is_state_error_(checkInitCondIsStateError()) {
   unone_.setZero();
-  init_state_error_ = dynamic_cast<StateErrorResidual *>(&*init_constraint_);
 }
 
 template <typename Scalar>
@@ -29,9 +29,7 @@ TrajOptProblemTpl<Scalar>::TrajOptProblemTpl(
     xyz::polymorphic<CostAbstract> term_cost)
     : TrajOptProblemTpl(
           StateErrorResidual(stages[0]->xspace_, stages[0]->nu(), x0), stages,
-          std::move(term_cost)) {
-  init_state_error_ = static_cast<StateErrorResidual *>(&*init_constraint_);
-}
+          std::move(term_cost)) {}
 
 template <typename Scalar>
 TrajOptProblemTpl<Scalar>::TrajOptProblemTpl(
@@ -131,6 +129,9 @@ template <typename Scalar>
 bool TrajOptProblemTpl<Scalar>::checkIntegrity() const {
   bool ok = true;
 
+  if (init_cond_is_state_error_ != checkInitCondIsStateError())
+    return false;
+
   if (numSteps() == 0)
     return true;
 
@@ -173,4 +174,11 @@ void TrajOptProblemTpl<Scalar>::replaceStageCircular(
   stages_.pop_back();
 }
 
+template <typename Scalar>
+bool TrajOptProblemTpl<Scalar>::checkInitCondIsStateError() const {
+  if (init_constraint_.valueless_after_move())
+    return false;
+  return dynamic_cast<StateErrorResidual const *>(&*init_constraint_);
+}
+
 } // namespace aligator

tests/problem.cpp

@@ -151,4 +151,24 @@ BOOST_AUTO_TEST_CASE(test_workspace) {
   ResultsTpl<double> results(f.problem);
 }
 
+BOOST_AUTO_TEST_CASE(test_copy) {
+  MyFixture f;
+
+  auto copy = f.problem;
+  BOOST_CHECK_EQUAL(copy.getInitState(), f.problem.getInitState());
+
+  Eigen::VectorXd state = f.problem.getInitState();
+
+  state[0] = 0.;
+  f.problem.setInitState(state);
+  BOOST_CHECK_EQUAL(f.problem.getInitState()[0], 0.);
+
+  state[0] = 1.;
+  BOOST_CHECK_EQUAL(f.problem.getInitState()[0], 0.);
+
+  copy.setInitState(state);
+  BOOST_CHECK_EQUAL(copy.getInitState()[0], 1.);
+  BOOST_CHECK_EQUAL(f.problem.getInitState()[0], 0.);
+}
+
 BOOST_AUTO_TEST_SUITE_END()