| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2020-2025, LAAS-CNRS, Heriot-Watt University | ||
| 5 | // Copyright note valid unless otherwise stated in individual files. | ||
| 6 | // All rights reserved. | ||
| 7 | /////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | #include "crocoddyl/core/solvers/kkt.hpp" | ||
| 10 | |||
| 11 | #include "python/crocoddyl/core/core.hpp" | ||
| 12 | #include "python/crocoddyl/utils/copyable.hpp" | ||
| 13 | |||
| 14 | #define SCALAR_float32 | ||
| 15 | |||
| 16 | namespace crocoddyl { | ||
| 17 | namespace python { | ||
| 18 | |||
| 19 | BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverKKT_solves, SolverKKT::solve, 0, 5) | ||
| 20 | BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverKKT_computeDirections, | ||
| 21 | SolverKKT::computeDirection, 0, 1) | ||
| 22 | BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverKKT_trySteps, SolverKKT::tryStep, | ||
| 23 | 0, 1) | ||
| 24 | |||
| 25 | ✗ | void exposeSolverKKT() { | |
| 26 | #ifdef SCALAR_float64 | ||
| 27 | bp::register_ptr_to_python<std::shared_ptr<SolverKKT> >(); | ||
| 28 | |||
| 29 | bp::class_<SolverKKT, bp::bases<SolverAbstract> >( | ||
| 30 | "SolverKKT", | ||
| 31 | "KKT solver.\n\n" | ||
| 32 | "The KKT solver computes a primal and dual optimal by inverting\n" | ||
| 33 | "the kkt matrix \n" | ||
| 34 | ":param shootingProblem: shooting problem (list of action models along " | ||
| 35 | "trajectory.)", | ||
| 36 | bp::init<std::shared_ptr<ShootingProblem> >( | ||
| 37 | bp::args("self", "problem"), | ||
| 38 | "Initialize the vector dimension.\n\n" | ||
| 39 | ":param problem: shooting problem.")) | ||
| 40 | .def("solve", &SolverKKT::solve, | ||
| 41 | SolverKKT_solves( | ||
| 42 | bp::args("self", "init_xs", "init_us", "maxiter", "isFeasible", | ||
| 43 | "regInit"), | ||
| 44 | "Compute the optimal primal(xopt, uopt) and dual(Vx) terms.\n\n" | ||
| 45 | ":param init_xs: initial guess for state trajectory with T+1 " | ||
| 46 | "elements (default []).\n" | ||
| 47 | ":param init_us: initial guess for control trajectory with T " | ||
| 48 | "elements (default []) (default []).\n" | ||
| 49 | ":param maxiter: maximun allowed number of iterations (default " | ||
| 50 | "100).\n" | ||
| 51 | ":param isFeasible: true if the init_xs are obtained from " | ||
| 52 | "integrating the init_us (rollout) (default " | ||
| 53 | "False).\n" | ||
| 54 | ":param regInit: initial guess for the regularization value. " | ||
| 55 | "Very low values are typical\n" | ||
| 56 | " used with very good guess points (init_xs, " | ||
| 57 | "init_us) (default None).\n" | ||
| 58 | ":returns the optimal trajectory xopt, uopt and a boolean that " | ||
| 59 | "describes if convergence was reached.")) | ||
| 60 | .def("computeDirection", &SolverKKT::computeDirection, | ||
| 61 | SolverKKT_computeDirections( | ||
| 62 | bp::args("self", "recalc"), | ||
| 63 | "Compute the search direction (dx, du), lambdas for the current " | ||
| 64 | "guess (xs, us).\n\n" | ||
| 65 | "You must call setCandidate first in order to define the " | ||
| 66 | "current\n" | ||
| 67 | "guess. A current guess defines a state and control trajectory\n" | ||
| 68 | "(xs, us) of T+1 and T elements, respectively.\n" | ||
| 69 | ":params recalc: true for recalculating the derivatives at " | ||
| 70 | "current state and control.\n" | ||
| 71 | ":returns the search direction dx, du and the dual lambdas as " | ||
| 72 | "lists of T+1, T and T+1 lengths.")) | ||
| 73 | .def("tryStep", &SolverKKT::tryStep, | ||
| 74 | SolverKKT_trySteps( | ||
| 75 | bp::args("self", " stepLength=1"), | ||
| 76 | "Rollout the system with a predefined step length.\n\n" | ||
| 77 | ":param stepLength: step length\n" | ||
| 78 | ":returns the cost improvement.")) | ||
| 79 | .def("stoppingCriteria", &SolverKKT::stoppingCriteria, bp::args("self"), | ||
| 80 | "Return a sum of positive parameters whose sum quantifies the DDP " | ||
| 81 | "termination.") | ||
| 82 | .def("expectedImprovement", &SolverKKT::expectedImprovement, | ||
| 83 | bp::return_value_policy<bp::reference_existing_object>(), | ||
| 84 | bp::args("self"), | ||
| 85 | "Return two scalars denoting the quadratic improvement model\n\n" | ||
| 86 | "For computing the expected improvement, you need to compute first\n" | ||
| 87 | "the search direction by running computeDirection. The quadratic\n" | ||
| 88 | "improvement model is described as dV = f_0 - f_+ = d1*a + " | ||
| 89 | "d2*a**2/2.") | ||
| 90 | .add_property( | ||
| 91 | "kkt", | ||
| 92 | make_function( | ||
| 93 | &SolverKKT::get_kkt, | ||
| 94 | bp::return_value_policy<bp::reference_existing_object>()), | ||
| 95 | "kkt") | ||
| 96 | .add_property( | ||
| 97 | "kktref", | ||
| 98 | make_function( | ||
| 99 | &SolverKKT::get_kktref, | ||
| 100 | bp::return_value_policy<bp::reference_existing_object>()), | ||
| 101 | "kktref") | ||
| 102 | .add_property( | ||
| 103 | "primaldual", | ||
| 104 | make_function( | ||
| 105 | &SolverKKT::get_primaldual, | ||
| 106 | bp::return_value_policy<bp::reference_existing_object>()), | ||
| 107 | "primaldual") | ||
| 108 | .add_property( | ||
| 109 | "lambdas", | ||
| 110 | make_function( | ||
| 111 | &SolverKKT::get_lambdas, | ||
| 112 | bp::return_value_policy<bp::reference_existing_object>()), | ||
| 113 | "lambdas") | ||
| 114 | .add_property( | ||
| 115 | "dxs", | ||
| 116 | make_function( | ||
| 117 | &SolverKKT::get_dxs, | ||
| 118 | bp::return_value_policy<bp::reference_existing_object>()), | ||
| 119 | "dxs") | ||
| 120 | .add_property( | ||
| 121 | "dus", | ||
| 122 | make_function( | ||
| 123 | &SolverKKT::get_dus, | ||
| 124 | bp::return_value_policy<bp::reference_existing_object>()), | ||
| 125 | "dus") | ||
| 126 | .def(CopyableVisitor<SolverKKT>()); | ||
| 127 | #endif | ||
| 128 | ✗ | } | |
| 129 | |||
| 130 | } // namespace python | ||
| 131 | } // namespace crocoddyl | ||
| 132 |