| 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 |
|
|
|