From e080d615d3519c6442bad3dcae5350d58618353b Mon Sep 17 00:00:00 2001
From: Ewen Dantec <ewen.dantec@inria.fr>
Date: Mon, 28 Oct 2024 10:59:29 +0100
Subject: [PATCH] Revert solver cycling and integrate friction cone review

---
 .../multibody/multibody-friction-cone.hpp     |  4 +---
 .../multibody/multibody-friction-cone.hxx     |  2 +-
 .../solvers/proxddp/solver-proxddp.hxx        | 19 -------------------
 3 files changed, 2 insertions(+), 23 deletions(-)

diff --git a/include/aligator/modelling/multibody/multibody-friction-cone.hpp b/include/aligator/modelling/multibody/multibody-friction-cone.hpp
index c24dac983..e87243d7a 100644
--- a/include/aligator/modelling/multibody/multibody-friction-cone.hpp
+++ b/include/aligator/modelling/multibody/multibody-friction-cone.hpp
@@ -83,9 +83,7 @@ struct MultibodyFrictionConeDataTpl : StageFunctionDataTpl<Scalar> {
   using Base = StageFunctionDataTpl<Scalar>;
   using PinData = pinocchio::DataTpl<Scalar>;
   using VectorXs = typename math_types<Scalar>::VectorXs;
-  using Vector3s = typename math_types<Scalar>::Vector3s;
   using MatrixXs = typename math_types<Scalar>::MatrixXs;
-  using Matrix6Xs = typename math_types<Scalar>::Matrix6Xs;
   using RigidConstraintDataVector =
       PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData);
 
@@ -93,7 +91,7 @@ struct MultibodyFrictionConeDataTpl : StageFunctionDataTpl<Scalar> {
   PinData pin_data_;
   VectorXs tau_;
   MatrixXs temp_;
-  MatrixXs dcone_df_;
+  Eigen::Matrix<Scalar, 1, 3> dcone_df_;
 
   RigidConstraintDataVector constraint_datas_;
   pinocchio::ProximalSettingsTpl<Scalar> settings;
diff --git a/include/aligator/modelling/multibody/multibody-friction-cone.hxx b/include/aligator/modelling/multibody/multibody-friction-cone.hxx
index 360921947..588848667 100644
--- a/include/aligator/modelling/multibody/multibody-friction-cone.hxx
+++ b/include/aligator/modelling/multibody/multibody-friction-cone.hxx
@@ -69,7 +69,7 @@ template <typename Scalar>
 MultibodyFrictionConeDataTpl<Scalar>::MultibodyFrictionConeDataTpl(
     const MultibodyFrictionConeResidualTpl<Scalar> *model)
     : Base(model->ndx1, model->nu, 2), pin_data_(model->pin_model_),
-      tau_(model->pin_model_.nv), temp_(3, model->nu), dcone_df_(1, 3) {
+      tau_(model->pin_model_.nv), temp_(3, model->nu) {
   tau_.setZero();
   temp_.setZero();
   dcone_df_.setZero();
diff --git a/include/aligator/solvers/proxddp/solver-proxddp.hxx b/include/aligator/solvers/proxddp/solver-proxddp.hxx
index 7627e6c70..512df57cd 100644
--- a/include/aligator/solvers/proxddp/solver-proxddp.hxx
+++ b/include/aligator/solvers/proxddp/solver-proxddp.hxx
@@ -187,25 +187,6 @@ void SolverProxDDPTpl<Scalar>::cycleProblem(
   results_.cycleAppend(problem, problem.getInitState());
   workspace_.cycleAppend(problem, data);
   linearSolver_->cycleAppend(workspace_.knots[workspace_.nsteps - 1]);
-
-  switch (linear_solver_choice) {
-  case LQSolverChoice::SERIAL: {
-    // linearSolver_->cycleAppend(workspace_.knots[workspace_.nsteps - 1]);
-    linearSolver_ = std::make_unique<gar::ProximalRiccatiSolver<Scalar>>(
-        workspace_.lqr_problem);
-    break;
-  }
-  case LQSolverChoice::PARALLEL: {
-    linearSolver_ = std::make_unique<gar::ParallelRiccatiSolver<Scalar>>(
-        workspace_.lqr_problem, num_threads_);
-    break;
-  }
-  case LQSolverChoice::STAGEDENSE: {
-    linearSolver_ = std::make_unique<gar::RiccatiSolverDense<Scalar>>(
-        workspace_.lqr_problem);
-    break;
-  }
-  }
 }
 
 template <typename... VArgs> bool is_nan_any(const VArgs &...args) {