-
Notifications
You must be signed in to change notification settings - Fork 19
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #236 from Simple-Robotics/topic/gar/devel
topic/gar/devel
- Loading branch information
Showing
30 changed files
with
777 additions
and
519 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,44 @@ | ||
| #include "aligator/python/fwd.hpp" | ||
| #include "aligator/gar/utils.hpp" | ||
|
|
||
| #include <eigenpy/std-array.hpp> | ||
|
|
||
| namespace aligator::python { | ||
| using namespace gar; | ||
|
|
||
| using context::Scalar; | ||
| using lqr_t = LQRProblemTpl<context::Scalar>; | ||
|
|
||
| bp::dict lqr_sol_initialize_wrap(const lqr_t &problem) { | ||
| bp::dict out; | ||
| auto ss = lqrInitializeSolution(problem); | ||
| auto &[xs, us, vs, lbdas] = ss; | ||
| out["xs"] = xs; | ||
| out["us"] = us; | ||
| out["vs"] = vs; | ||
| out["lbdas"] = lbdas; | ||
| return out; | ||
| } | ||
|
|
||
| void exposeGarUtils() { | ||
|
|
||
| bp::def( | ||
| "lqrDenseMatrix", | ||
| +[](const lqr_t &problem, Scalar mudyn, Scalar mueq) { | ||
| auto mat_rhs = lqrDenseMatrix(problem, mudyn, mueq); | ||
| return bp::make_tuple(std::get<0>(mat_rhs), std::get<1>(mat_rhs)); | ||
| }, | ||
| ("problem"_a, "mudyn", "mueq")); | ||
|
|
||
| bp::def("lqrCreateSparseMatrix", lqrCreateSparseMatrix<Scalar>, | ||
| ("problem"_a, "mudyn", "mueq", "mat", "rhs", "update"), | ||
| "Create or update a sparse matrix from an LQRProblem."); | ||
|
|
||
| bp::def("lqrInitializeSolution", lqr_sol_initialize_wrap, ("problem"_a)); | ||
|
|
||
| bp::def("lqrComputeKktError", lqrComputeKktError<Scalar>, | ||
| ("problem"_a, "xs", "us", "vs", "lbdas", "mudyn", "mueq", "theta", | ||
| "verbose"_a = false), | ||
| "Compute the KKT residual of the LQR problem."); | ||
| } | ||
| } // namespace aligator::python |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,46 @@ | ||
| /// @copyright Copyright (C) 2024 LAAS-CNRS, INRIA | ||
| /// @author Wilson Jallet | ||
| #pragma once | ||
|
|
||
| #ifdef ALIGATOR_WITH_CHOLMOD | ||
| #include "cholmod-solver.hpp" | ||
| #include "utils.hpp" | ||
|
|
||
| namespace aligator::gar { | ||
|
|
||
| template <typename Scalar> | ||
| CholmodLqSolver<Scalar>::CholmodLqSolver(const Problem &problem, | ||
| uint numRefinementSteps) | ||
| : kktMatrix(), kktRhs(), cholmod(), numRefinementSteps(numRefinementSteps), | ||
| problem_(&problem) { | ||
| lqrCreateSparseMatrix(problem, 1., 1., kktMatrix, kktRhs, false); | ||
| assert(kktMatrix.cols() == kktRhs.rows()); | ||
| kktSol.resize(kktRhs.rows()); | ||
| kktResidual.resize(kktRhs.rows()); | ||
| cholmod.analyzePattern(kktMatrix); | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| bool CholmodLqSolver<Scalar>::backward(const Scalar mudyn, const Scalar mueq) { | ||
| // update the sparse linear problem | ||
| lqrCreateSparseMatrix(*problem_, mudyn, mueq, kktMatrix, kktRhs, true); | ||
| cholmod.factorize(kktMatrix); | ||
| return cholmod.info() == Eigen::Success; | ||
| } | ||
|
|
||
| template <typename Scalar> | ||
| bool CholmodLqSolver<Scalar>::forward(std::vector<VectorXs> &xs, | ||
| std::vector<VectorXs> &us, | ||
| std::vector<VectorXs> &vs, | ||
| std::vector<VectorXs> &lbdas) const { | ||
| kktSol = cholmod.solve(-kktRhs); | ||
| kktResidual = kktRhs; | ||
| for (uint i = 0; i < numRefinementSteps; i++) { | ||
| kktResidual.noalias() += kktMatrix * kktSol; | ||
| kktSol += cholmod.solve(-kktResidual); | ||
| } | ||
| lqrDenseSolutionToTraj(*problem_, kktSol, xs, us, vs, lbdas); | ||
| return true; | ||
| }; | ||
| } // namespace aligator::gar | ||
| #endif |
Oops, something went wrong.