| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh, | ||
| 5 | // Heriot-Watt University | ||
| 6 | // Copyright note valid unless otherwise stated in individual files. | ||
| 7 | // All rights reserved. | ||
| 8 | /////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | #include "python/crocoddyl/core/solver-base.hpp" | ||
| 11 | |||
| 12 | #include "python/crocoddyl/utils/copyable.hpp" | ||
| 13 | #include "python/crocoddyl/utils/deprecate.hpp" | ||
| 14 | #include "python/crocoddyl/utils/vector-converter.hpp" | ||
| 15 | |||
| 16 | #define SCALAR_float64 | ||
| 17 | |||
| 18 | namespace crocoddyl { | ||
| 19 | namespace python { | ||
| 20 | |||
| 21 | ✗ | void exposeSolverAbstract() { | |
| 22 | #ifdef SCALAR_float64 | ||
| 23 | // Register custom converters between std::vector and Python list | ||
| 24 | typedef std::shared_ptr<CallbackAbstract> CallbackAbstractPtr; | ||
| 25 | ✗ | StdVectorPythonVisitor<std::vector<CallbackAbstractPtr>, true>::expose( | |
| 26 | "StdVec_Callback"); | ||
| 27 | #endif | ||
| 28 | |||
| 29 | #ifdef SCALAR_float64 | ||
| 30 | ✗ | bp::enum_<FeasibilityNorm>("FeasibilityNorm") | |
| 31 | ✗ | .value("LInf", LInf) | |
| 32 | ✗ | .value("L1", L1) | |
| 33 | ✗ | .export_values(); | |
| 34 | #endif | ||
| 35 | |||
| 36 | #ifdef SCALAR_float64 | ||
| 37 | ✗ | bp::class_<SolverAbstract_wrap, boost::noncopyable>( | |
| 38 | "SolverAbstract", | ||
| 39 | "Abstract class for optimal control solvers.\n\n" | ||
| 40 | "A solver resolves an optimal control solver which is formulated in a " | ||
| 41 | "shooting problem\n" | ||
| 42 | "abstraction. The main routines are computeDirection and tryStep. The " | ||
| 43 | "former finds\n" | ||
| 44 | "a search direction and typically computes the derivatives of each " | ||
| 45 | "action model. The latter\n" | ||
| 46 | "rollout the dynamics and cost (i.e. the action) to try the search " | ||
| 47 | "direction found by\n" | ||
| 48 | "computeDirection. Both functions used the current guess defined by " | ||
| 49 | "setCandidate. Finally\n" | ||
| 50 | "solve function is used to define when the search direction and length " | ||
| 51 | "are computed in each\n" | ||
| 52 | "iterate. It also describes the globalization strategy (i.e. " | ||
| 53 | "regularization) of the\n" | ||
| 54 | "numerical optimization.", | ||
| 55 | ✗ | bp::init<std::shared_ptr<ShootingProblem> >( | |
| 56 | ✗ | bp::args("self", "problem"), | |
| 57 | "Initialize the solver model.\n\n" | ||
| 58 | ":param problem: shooting problem")) | ||
| 59 | ✗ | .def("solve", pure_virtual(&SolverAbstract_wrap::solve), | |
| 60 | ✗ | bp::args("self", "init_xs", "init_us", "maxiter", "isFeasible", | |
| 61 | "regInit"), | ||
| 62 | "Compute the optimal trajectory xopt,uopt as lists of T+1 and T " | ||
| 63 | "terms.\n\n" | ||
| 64 | "From an initial guess init_xs,init_us (feasible or not), iterate\n" | ||
| 65 | "over computeDirection and tryStep until stoppingCriteria is below\n" | ||
| 66 | "threshold. It also describes the globalization strategy used\n" | ||
| 67 | "during the numerical optimization.\n" | ||
| 68 | ":param init_xs: initial guess for state trajectory with T+1 " | ||
| 69 | "elements (default [])\n" | ||
| 70 | ":param init_us: initial guess for control trajectory with T " | ||
| 71 | "elements (default []).\n" | ||
| 72 | ":param maxiter: maximum allowed number of iterations (default " | ||
| 73 | "100).\n" | ||
| 74 | ":param isFeasible: true if the init_xs are obtained from " | ||
| 75 | "integrating the init_us (rollout) (default " | ||
| 76 | "False).\n" | ||
| 77 | ":param regInit: initial guess for the regularization value. Very " | ||
| 78 | "low\n" | ||
| 79 | " values are typical used with very good guess " | ||
| 80 | "points (init_xs, init_us).\n" | ||
| 81 | ":returns A boolean that describes if convergence was reached.") | ||
| 82 | ✗ | .def("computeDirection", | |
| 83 | pure_virtual(&SolverAbstract_wrap::computeDirection), | ||
| 84 | ✗ | bp::args("self", "recalc"), | |
| 85 | "Compute the search direction (dx, du) for the current guess (xs, " | ||
| 86 | "us).\n\n" | ||
| 87 | "You must call setCandidate first in order to define the current\n" | ||
| 88 | "guess. A current guess defines a state and control trajectory\n" | ||
| 89 | "(xs, us) of T+1 and T elements, respectively.\n" | ||
| 90 | ":params recalc: true for recalculating the derivatives at current " | ||
| 91 | "state and control.\n" | ||
| 92 | ":returns the search direction dx, du and the dual lambdas as lists " | ||
| 93 | "of T+1, T and T+1 lengths.") | ||
| 94 | ✗ | .def( | |
| 95 | "tryStep", pure_virtual(&SolverAbstract_wrap::tryStep), | ||
| 96 | ✗ | bp::args("self", "stepLength"), | |
| 97 | "Try a predefined step length and compute its cost improvement.\n\n" | ||
| 98 | "It uses the search direction found by computeDirection to try a\n" | ||
| 99 | "determined step length; so you need to first run computeDirection.\n" | ||
| 100 | "Additionally it returns the cost improvement along the predefined\n" | ||
| 101 | "step length.\n" | ||
| 102 | ":param stepLength: step length\n" | ||
| 103 | ":returns the cost improvement.") | ||
| 104 | ✗ | .def( | |
| 105 | "stoppingCriteria", | ||
| 106 | pure_virtual(&SolverAbstract_wrap::stoppingCriteria), | ||
| 107 | ✗ | bp::args("self"), | |
| 108 | "Return a positive value that quantifies the algorithm " | ||
| 109 | "termination.\n\n" | ||
| 110 | "These values typically represents the gradient norm which tell us\n" | ||
| 111 | "that it's been reached the local minima. This function is used to\n" | ||
| 112 | "evaluate the algorithm convergence. The stopping criteria strictly\n" | ||
| 113 | "speaking depends on the search direction (calculated by\n" | ||
| 114 | "computeDirection) but it could also depend on the chosen step\n" | ||
| 115 | "length, tested by tryStep.") | ||
| 116 | ✗ | .def("expectedImprovement", | |
| 117 | pure_virtual(&SolverAbstract_wrap::expectedImprovement_wrap), | ||
| 118 | ✗ | bp::args("self"), | |
| 119 | "Return the expected improvement from a given current search " | ||
| 120 | "direction.\n\n" | ||
| 121 | "For computing the expected improvement, you need to compute first\n" | ||
| 122 | "the search direction by running computeDirection.") | ||
| 123 | ✗ | .def("setCandidate", &SolverAbstract_wrap::setCandidate, | |
| 124 | ✗ | setCandidate_overloads( | |
| 125 | ✗ | bp::args("self", "xs", "us", "isFeasible"), | |
| 126 | "Set the solver candidate warm-point values (xs, us).\n\n" | ||
| 127 | "The solver candidates are defined as a state and control " | ||
| 128 | "trajectory\n" | ||
| 129 | "(xs, us) of T+1 and T elements, respectively. Additionally, we " | ||
| 130 | "need\n" | ||
| 131 | "to define is (xs,us) pair is feasible, this means that the " | ||
| 132 | "dynamics\n" | ||
| 133 | "rollout give us produces xs.\n" | ||
| 134 | ":param xs: state trajectory of T+1 elements (default []).\n" | ||
| 135 | ":param us: control trajectory of T elements (default []).\n" | ||
| 136 | ":param isFeasible: true if the xs are obtained from " | ||
| 137 | "integrating the\n" | ||
| 138 | "us (rollout).")) | ||
| 139 | ✗ | .def("computeDynamicFeasibility", | |
| 140 | ✗ | &SolverAbstract_wrap::computeDynamicFeasibility, bp::args("self"), | |
| 141 | "Compute the dynamic feasibility for the current guess.\n\n" | ||
| 142 | "The feasibility can be computed using the computed using the l-1 " | ||
| 143 | "and l-inf norms.\n" | ||
| 144 | "By default we use the l-inf norm, however, we can use the l-1 norm " | ||
| 145 | "by defining inffeas as False.") | ||
| 146 | ✗ | .def("computeInequalityFeasibility", | |
| 147 | ✗ | &SolverAbstract_wrap::computeInequalityFeasibility, bp::args("self"), | |
| 148 | "Compute the feasibility of the inequality constraint for the " | ||
| 149 | "current guess.\n\n" | ||
| 150 | "The feasibility can be computed using the computed using the l-1 " | ||
| 151 | "and l-inf norms.\n" | ||
| 152 | "By default we use the l-inf norm, however, we can use the l-1 norm " | ||
| 153 | "by defining inffeas as False.") | ||
| 154 | ✗ | .def("computeEqualityFeasibility", | |
| 155 | ✗ | &SolverAbstract_wrap::computeEqualityFeasibility, bp::args("self"), | |
| 156 | "Compute the feasibility of the equality constraint for the current " | ||
| 157 | "guess.\n\n" | ||
| 158 | "The feasibility can be computed using the computed using the l-1 " | ||
| 159 | "and l-inf norms.\n" | ||
| 160 | "By default we use the l-inf norm, however, we can use the l-1 norm " | ||
| 161 | "by defining inffeas as False.") | ||
| 162 | ✗ | .def("setCallbacks", &SolverAbstract_wrap::setCallbacks, | |
| 163 | ✗ | bp::args("self", "callbacks"), | |
| 164 | "Set a list of callback functions using for diagnostic.\n\n" | ||
| 165 | "Each iteration, the solver calls these set of functions in order " | ||
| 166 | "to\n" | ||
| 167 | "allowed user the diagnostic of the its performance.\n" | ||
| 168 | ":param callbacks: set of callback functions.") | ||
| 169 | ✗ | .def("getCallbacks", &SolverAbstract_wrap::getCallbacks, | |
| 170 | ✗ | bp::return_value_policy<bp::copy_const_reference>(), | |
| 171 | ✗ | bp::args("self"), | |
| 172 | "Return the list of callback functions using for diagnostic.\n\n" | ||
| 173 | ":return set of callback functions.") | ||
| 174 | ✗ | .add_property("problem", | |
| 175 | ✗ | bp::make_function( | |
| 176 | &SolverAbstract_wrap::get_problem, | ||
| 177 | ✗ | bp::return_value_policy<bp::copy_const_reference>()), | |
| 178 | "shooting problem") | ||
| 179 | ✗ | .def_readwrite("xs", &SolverAbstract_wrap::xs_, "state trajectory") | |
| 180 | ✗ | .def_readwrite("us", &SolverAbstract_wrap::us_, "control sequence") | |
| 181 | ✗ | .def_readwrite("fs", &SolverAbstract_wrap::fs_, "dynamics gaps") | |
| 182 | ✗ | .def_readwrite("isFeasible", &SolverAbstract_wrap::is_feasible_, | |
| 183 | "feasible (xs,us)") | ||
| 184 | ✗ | .def_readwrite("cost", &SolverAbstract_wrap::cost_, | |
| 185 | "cost for the current guess") | ||
| 186 | ✗ | .def_readwrite("merit", &SolverAbstract_wrap::merit_, | |
| 187 | "merit for the current guess") | ||
| 188 | ✗ | .def_readwrite("stop", &SolverAbstract_wrap::stop_, | |
| 189 | "stopping criteria value") | ||
| 190 | ✗ | .def_readwrite("d", &SolverAbstract_wrap::d_, | |
| 191 | "linear and quadratic terms of the expected improvement") | ||
| 192 | ✗ | .def_readwrite("dV", &SolverAbstract_wrap::dV_, | |
| 193 | "reduction in the cost function computed by `tryStep()`") | ||
| 194 | ✗ | .def_readwrite("dPhi", &SolverAbstract_wrap::dPhi_, | |
| 195 | "reduction in the merit function computed by `tryStep()`") | ||
| 196 | ✗ | .def_readwrite("dVexp", &SolverAbstract_wrap::dVexp_, | |
| 197 | "expected reduction in the cost function") | ||
| 198 | ✗ | .def_readwrite("dPhiexp", &SolverAbstract_wrap::dPhiexp_, | |
| 199 | "expected reduction in the merit function") | ||
| 200 | ✗ | .def_readwrite("dfeas", &SolverAbstract_wrap::dfeas_, | |
| 201 | "reduction in the feasibility") | ||
| 202 | ✗ | .def_readwrite("feas", &SolverAbstract_wrap::feas_, | |
| 203 | "total feasibility for the current guess") | ||
| 204 | ✗ | .def_readwrite( | |
| 205 | ✗ | "ffeas", &SolverAbstract_wrap::ffeas_, | |
| 206 | "feasibility of the dynamic constraint for the current guess") | ||
| 207 | ✗ | .def_readwrite( | |
| 208 | ✗ | "gfeas", &SolverAbstract_wrap::gfeas_, | |
| 209 | "feasibility of the inequality constraint for the current guess") | ||
| 210 | ✗ | .def_readwrite( | |
| 211 | ✗ | "hfeas", &SolverAbstract_wrap::hfeas_, | |
| 212 | "feasibility of the equality constraint for the current guess") | ||
| 213 | ✗ | .def_readwrite( | |
| 214 | ✗ | "ffeas_try", &SolverAbstract_wrap::ffeas_try_, | |
| 215 | "feasibility of the dynamic constraint for the current step length") | ||
| 216 | ✗ | .def_readwrite("gfeas_try", &SolverAbstract_wrap::gfeas_try_, | |
| 217 | "feasibility of the inequality constraint for the current " | ||
| 218 | "step length") | ||
| 219 | ✗ | .def_readwrite( | |
| 220 | ✗ | "hfeas_try", &SolverAbstract_wrap::hfeas_try_, | |
| 221 | "feasibility of the equality constraint for the current step length") | ||
| 222 | ✗ | .add_property("preg", bp::make_function(&SolverAbstract_wrap::get_preg), | |
| 223 | ✗ | bp::make_function(&SolverAbstract_wrap::set_preg), | |
| 224 | "primal-variable regularization") | ||
| 225 | ✗ | .add_property("dreg", bp::make_function(&SolverAbstract_wrap::get_dreg), | |
| 226 | ✗ | bp::make_function(&SolverAbstract_wrap::set_dreg), | |
| 227 | "dual-variable regularization") | ||
| 228 | ✗ | .add_property( | |
| 229 | "x_reg", | ||
| 230 | ✗ | bp::make_function( | |
| 231 | &SolverAbstract_wrap::get_xreg, | ||
| 232 | ✗ | deprecated<bp::return_value_policy<bp::return_by_value> >( | |
| 233 | "Deprecated. Use preg")), | ||
| 234 | ✗ | bp::make_function(&SolverAbstract_wrap::set_xreg, | |
| 235 | ✗ | deprecated<>("Deprecated. Use preg.")), | |
| 236 | "state regularization") | ||
| 237 | ✗ | .add_property( | |
| 238 | "u_reg", | ||
| 239 | ✗ | bp::make_function( | |
| 240 | &SolverAbstract_wrap::get_ureg, | ||
| 241 | ✗ | deprecated<bp::return_value_policy<bp::return_by_value> >( | |
| 242 | "Deprecated. Use preg")), | ||
| 243 | ✗ | bp::make_function(&SolverAbstract_wrap::set_ureg, | |
| 244 | ✗ | deprecated<>("Deprecated. Use preg.")), | |
| 245 | "control regularization") | ||
| 246 | ✗ | .def_readwrite("stepLength", &SolverAbstract_wrap::steplength_, | |
| 247 | "applied step length") | ||
| 248 | ✗ | .add_property("th_acceptStep", | |
| 249 | ✗ | bp::make_function(&SolverAbstract_wrap::get_th_acceptstep), | |
| 250 | ✗ | bp::make_function(&SolverAbstract_wrap::set_th_acceptstep), | |
| 251 | "threshold for step acceptance") | ||
| 252 | ✗ | .add_property("th_stop", | |
| 253 | ✗ | bp::make_function(&SolverAbstract_wrap::get_th_stop), | |
| 254 | ✗ | bp::make_function(&SolverAbstract_wrap::set_th_stop), | |
| 255 | "threshold for stopping criteria") | ||
| 256 | ✗ | .add_property("th_gapTol", | |
| 257 | ✗ | bp::make_function(&SolverAbstract_wrap::get_th_gaptol), | |
| 258 | ✗ | bp::make_function(&SolverAbstract_wrap::set_th_gaptol), | |
| 259 | "threshold for accepting a gap as non-zero") | ||
| 260 | ✗ | .add_property( | |
| 261 | ✗ | "feasNorm", bp::make_function(&SolverAbstract_wrap::get_feasnorm), | |
| 262 | ✗ | bp::make_function(&SolverAbstract_wrap::set_feasnorm), | |
| 263 | "norm used to compute the dynamic and constraints feasibility") | ||
| 264 | ✗ | .def_readwrite("iter", &SolverAbstract_wrap::iter_, | |
| 265 | "number of iterations runned in solve()") | ||
| 266 | ✗ | .def(CopyableVisitor<SolverAbstract_wrap>()); | |
| 267 | |||
| 268 | ✗ | bp::class_<CallbackAbstract_wrap, boost::noncopyable>( | |
| 269 | "CallbackAbstract", | ||
| 270 | "Abstract class for solver callbacks.\n\n" | ||
| 271 | "A callback is used to diagnostic the behaviour of our solver in each " | ||
| 272 | "iteration of it.\n" | ||
| 273 | "For instance, it can be used to print values, record data or display " | ||
| 274 | "motions") | ||
| 275 | ✗ | .def("__call__", pure_virtual(&CallbackAbstract_wrap::operator()), | |
| 276 | ✗ | bp::args("self", "solver"), | |
| 277 | "Run the callback function given a solver.\n\n" | ||
| 278 | ":param solver: solver to be diagnostic") | ||
| 279 | ✗ | .def(CopyableVisitor<CallbackAbstract_wrap>()); | |
| 280 | #endif | ||
| 281 | ✗ | } | |
| 282 | |||
| 283 | } // namespace python | ||
| 284 | } // namespace crocoddyl | ||
| 285 |