Revert solver cycling and integrate friction cone review

Simple-Robotics · Nov 4, 2024 · e080d61 · e080d61
1 parent 4ecb9d9
commit e080d61
Show file tree

Hide file tree

Showing 3 changed files with 2 additions and 23 deletions.
diff --git a/include/aligator/modelling/multibody/multibody-friction-cone.hpp b/include/aligator/modelling/multibody/multibody-friction-cone.hpp
@@ -83,17 +83,15 @@ 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);
 
   /// Pinocchio data object.
   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
@@ -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
@@ -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) {