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