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 "crocoddyl/core/solvers/ddp.hpp" |
11 |
|
|
|
12 |
|
|
#include "python/crocoddyl/core/core.hpp" |
13 |
|
|
#include "python/crocoddyl/utils/copyable.hpp" |
14 |
|
|
#include "python/crocoddyl/utils/deprecate.hpp" |
15 |
|
|
|
16 |
|
|
namespace crocoddyl { |
17 |
|
|
namespace python { |
18 |
|
|
|
19 |
|
32 |
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverDDP_solves, SolverDDP::solve, 0, 5) |
20 |
|
18 |
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverDDP_computeDirections, |
21 |
|
|
SolverDDP::computeDirection, 0, 1) |
22 |
|
36 |
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverDDP_trySteps, SolverDDP::tryStep, |
23 |
|
|
0, 1) |
24 |
|
|
|
25 |
|
10 |
void exposeSolverDDP() { |
26 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<SolverDDP> >(); |
27 |
|
|
|
28 |
✓✗ |
10 |
bp::class_<SolverDDP, bp::bases<SolverAbstract> >( |
29 |
|
|
"SolverDDP", |
30 |
|
|
"DDP solver.\n\n" |
31 |
|
|
"The DDP solver computes an optimal trajectory and control commands by " |
32 |
|
|
"iterates\n" |
33 |
|
|
"running backward and forward passes. The backward-pass updates locally " |
34 |
|
|
"the\n" |
35 |
|
|
"quadratic approximation of the problem and computes descent direction,\n" |
36 |
|
|
"and the forward-pass rollouts this new policy by integrating the system " |
37 |
|
|
"dynamics\n" |
38 |
|
|
"along a tuple of optimized control commands U*.\n" |
39 |
|
|
":param shootingProblem: shooting problem (list of action models along " |
40 |
|
|
"trajectory.)", |
41 |
✓✗ |
10 |
bp::init<boost::shared_ptr<ShootingProblem> >( |
42 |
|
20 |
bp::args("self", "problem"), |
43 |
|
|
"Initialize the vector dimension.\n\n" |
44 |
|
|
":param problem: shooting problem.")) |
45 |
|
|
.def("solve", &SolverDDP::solve, |
46 |
✓✗ |
10 |
SolverDDP_solves( |
47 |
✓✗ |
20 |
bp::args("self", "init_xs", "init_us", "maxiter", "is_feasible", |
48 |
|
|
"init_reg"), |
49 |
|
|
"Compute the optimal trajectory xopt, uopt as lists of T+1 and " |
50 |
|
|
"T terms.\n\n" |
51 |
|
|
"From an initial guess init_xs,init_us (feasible or not), " |
52 |
|
|
"iterate\n" |
53 |
|
|
"over computeDirection and tryStep until stoppingCriteria is " |
54 |
|
|
"below\n" |
55 |
|
|
"threshold. It also describes the globalization strategy used\n" |
56 |
|
|
"during the numerical optimization.\n" |
57 |
|
|
":param init_xs: initial guess for state trajectory with T+1 " |
58 |
|
|
"elements (default []).\n" |
59 |
|
|
":param init_us: initial guess for control trajectory with T " |
60 |
|
|
"elements (default []) (default []).\n" |
61 |
|
|
":param maxiter: maximum allowed number of iterations (default " |
62 |
|
|
"100).\n" |
63 |
|
|
":param is_feasible: true if the init_xs are obtained from " |
64 |
|
|
"integrating the init_us (rollout)\n" |
65 |
|
|
"(default False).\n" |
66 |
|
|
":param init_reg: initial guess for the regularization value. " |
67 |
|
|
"Very low values are typical\n" |
68 |
|
|
" used with very good guess points (default " |
69 |
|
|
"1e-9).\n" |
70 |
|
|
":returns the optimal trajectory xopt, uopt and a boolean that " |
71 |
✓✗ |
10 |
"describes if convergence was reached.")) |
72 |
|
|
.def("computeDirection", &SolverDDP::computeDirection, |
73 |
✓✗ |
10 |
SolverDDP_computeDirections( |
74 |
✓✗ |
20 |
bp::args("self", "recalc"), |
75 |
|
|
"Compute the search direction (dx, du) for the current guess " |
76 |
|
|
"(xs, us).\n\n" |
77 |
|
|
"You must call setCandidate first in order to define the " |
78 |
|
|
"current\n" |
79 |
|
|
"guess. A current guess defines a state and control trajectory\n" |
80 |
|
|
"(xs, us) of T+1 and T elements, respectively.\n" |
81 |
|
|
":params recalc: true for recalculating the derivatives at " |
82 |
|
|
"current state and control.\n" |
83 |
|
|
":returns the search direction dx, du and the dual lambdas as " |
84 |
✓✗ |
10 |
"lists of T+1, T and T+1 lengths.")) |
85 |
|
|
.def("tryStep", &SolverDDP::tryStep, |
86 |
✓✗ |
10 |
SolverDDP_trySteps( |
87 |
✓✗ |
20 |
bp::args("self", "stepLength"), |
88 |
|
|
"Rollout the system with a predefined step length.\n\n" |
89 |
|
|
":param stepLength: step length (default 1)\n" |
90 |
✓✗ |
10 |
":returns the cost improvement.")) |
91 |
✓✗ |
20 |
.def("stoppingCriteria", &SolverDDP::stoppingCriteria, bp::args("self"), |
92 |
|
|
"Return a sum of positive parameters whose sum quantifies the DDP " |
93 |
✓✗ |
10 |
"termination.") |
94 |
|
|
.def("expectedImprovement", &SolverDDP::expectedImprovement, |
95 |
|
|
bp::return_value_policy<bp::copy_const_reference>(), |
96 |
✓✗ |
20 |
bp::args("self"), |
97 |
|
|
"Return two scalars denoting the quadratic improvement model\n\n" |
98 |
|
|
"For computing the expected improvement, you need to compute first\n" |
99 |
|
|
"the search direction by running computeDirection. The quadratic\n" |
100 |
|
|
"improvement model is described as dV = f_0 - f_+ = d1*a + " |
101 |
✓✗ |
10 |
"d2*a**2/2.") |
102 |
✓✗ |
20 |
.def("calcDiff", &SolverDDP::calcDiff, bp::args("self"), |
103 |
|
|
"Update the Jacobian and Hessian of the optimal control problem\n\n" |
104 |
|
|
"These derivatives are computed around the guess state and control\n" |
105 |
|
|
"trajectory. These trajectory can be set by using setCandidate.\n" |
106 |
✓✗ |
10 |
":return the total cost around the guess trajectory.") |
107 |
✓✗ |
20 |
.def("backwardPass", &SolverDDP::backwardPass, bp::args("self"), |
108 |
|
|
"Run the backward pass (Riccati sweep)\n\n" |
109 |
|
|
"It assumes that the Jacobian and Hessians of the optimal control " |
110 |
|
|
"problem have been\n" |
111 |
✓✗ |
10 |
"compute. These terms are computed by running calc.") |
112 |
|
|
.def("forwardPass", &SolverDDP::forwardPass, |
113 |
✓✗ |
20 |
bp::args("self", "stepLength"), |
114 |
|
|
"Run the forward pass or rollout\n\n" |
115 |
|
|
"It rollouts the action model given the computed policy " |
116 |
|
|
"(feedforward terns and feedback\n" |
117 |
|
|
"gains) by the backwardPass. We can define different step lengths\n" |
118 |
✓✗ |
10 |
":param stepLength: applied step length (<= 1. and >= 0.)") |
119 |
|
|
.def("computeActionValueFunction", &SolverDDP::computeActionValueFunction, |
120 |
✓✗ |
20 |
bp::args("self", "t", "model", "data"), |
121 |
|
|
"Compute the linear-quadratic model of the control Hamiltonian\n\n" |
122 |
|
|
":param t: time instance\n" |
123 |
|
|
":param model: action model in the given time instance\n" |
124 |
✓✗ |
10 |
":param data: action data in the given time instance") |
125 |
|
|
.def("computeValueFunction", &SolverDDP::computeValueFunction, |
126 |
✓✗ |
20 |
bp::args("self", "t", "model"), |
127 |
|
|
"Compute the quadratic model of the value function.\n\n" |
128 |
|
|
"This function is called in the backward pass after updating the " |
129 |
|
|
"local Hamiltonian.\n" |
130 |
|
|
":param t: time instance\n" |
131 |
✓✗ |
10 |
":param model: action model in the given time instance") |
132 |
|
|
.add_property( |
133 |
|
|
"Vxx", |
134 |
✓✗ |
10 |
make_function(&SolverDDP::get_Vxx, |
135 |
|
10 |
bp::return_value_policy<bp::copy_const_reference>()), |
136 |
✓✗ |
10 |
"Vxx") |
137 |
|
|
.add_property( |
138 |
|
|
"Vx", |
139 |
✓✗ |
10 |
make_function(&SolverDDP::get_Vx, |
140 |
|
10 |
bp::return_value_policy<bp::copy_const_reference>()), |
141 |
✓✗ |
10 |
"Vx") |
142 |
|
|
.add_property( |
143 |
|
|
"Qxx", |
144 |
✓✗ |
10 |
make_function(&SolverDDP::get_Qxx, |
145 |
|
10 |
bp::return_value_policy<bp::copy_const_reference>()), |
146 |
✓✗ |
10 |
"Qxx") |
147 |
|
|
.add_property( |
148 |
|
|
"Qxu", |
149 |
✓✗ |
10 |
make_function(&SolverDDP::get_Qxu, |
150 |
|
10 |
bp::return_value_policy<bp::copy_const_reference>()), |
151 |
✓✗ |
10 |
"Qxu") |
152 |
|
|
.add_property( |
153 |
|
|
"Quu", |
154 |
✓✗ |
10 |
make_function(&SolverDDP::get_Quu, |
155 |
|
10 |
bp::return_value_policy<bp::copy_const_reference>()), |
156 |
✓✗ |
10 |
"Quu") |
157 |
|
|
.add_property( |
158 |
|
|
"Qx", |
159 |
✓✗ |
10 |
make_function(&SolverDDP::get_Qx, |
160 |
|
10 |
bp::return_value_policy<bp::copy_const_reference>()), |
161 |
✓✗ |
10 |
"Qx") |
162 |
|
|
.add_property( |
163 |
|
|
"Qu", |
164 |
✓✗ |
10 |
make_function(&SolverDDP::get_Qu, |
165 |
|
10 |
bp::return_value_policy<bp::copy_const_reference>()), |
166 |
✓✗ |
10 |
"Qu") |
167 |
|
|
.add_property( |
168 |
|
|
"K", |
169 |
✓✗ |
10 |
make_function(&SolverDDP::get_K, |
170 |
|
10 |
bp::return_value_policy<bp::copy_const_reference>()), |
171 |
✓✗ |
10 |
"K") |
172 |
|
|
.add_property( |
173 |
|
|
"k", |
174 |
✓✗ |
10 |
make_function(&SolverDDP::get_k, |
175 |
|
10 |
bp::return_value_policy<bp::copy_const_reference>()), |
176 |
✓✗ |
10 |
"k") |
177 |
|
|
.add_property( |
178 |
✓✗ |
10 |
"reg_incFactor", bp::make_function(&SolverDDP::get_reg_incfactor), |
179 |
✓✗ |
20 |
bp::make_function(&SolverDDP::set_reg_incfactor), |
180 |
✓✗ |
10 |
"regularization factor used for increasing the damping value.") |
181 |
|
|
.add_property( |
182 |
✓✗ |
10 |
"reg_decFactor", bp::make_function(&SolverDDP::get_reg_decfactor), |
183 |
✓✗ |
20 |
bp::make_function(&SolverDDP::set_reg_decfactor), |
184 |
✓✗ |
10 |
"regularization factor used for decreasing the damping value.") |
185 |
|
|
.add_property( |
186 |
|
|
"regFactor", |
187 |
✓✗ |
10 |
bp::make_function( |
188 |
|
|
&SolverDDP::get_reg_incfactor, |
189 |
✓✗✓✗
|
20 |
deprecated<>("Deprecated. Use reg_incfactor or reg_decfactor")), |
190 |
✓✗ |
20 |
bp::make_function( |
191 |
|
|
&SolverDDP::set_reg_incfactor, |
192 |
✓✗✓✗
|
20 |
deprecated<>("Deprecated. Use reg_incfactor or reg_decfactor")), |
193 |
|
|
"regularization factor used for increasing or decreasing the damping " |
194 |
✓✗ |
10 |
"value.") |
195 |
✓✗ |
10 |
.add_property("reg_min", bp::make_function(&SolverDDP::get_reg_min), |
196 |
✓✗ |
20 |
bp::make_function(&SolverDDP::set_reg_min), |
197 |
✓✗ |
10 |
"minimum regularization value.") |
198 |
✓✗ |
10 |
.add_property("reg_max", bp::make_function(&SolverDDP::get_reg_max), |
199 |
✓✗ |
20 |
bp::make_function(&SolverDDP::set_reg_max), |
200 |
✓✗ |
10 |
"maximum regularization value.") |
201 |
|
|
.add_property("regMin", |
202 |
✓✗ |
10 |
bp::make_function(&SolverDDP::get_reg_min, |
203 |
✓✗✓✗
|
20 |
deprecated<>("Deprecated. Use reg_min")), |
204 |
✓✗ |
20 |
bp::make_function(&SolverDDP::set_reg_min, |
205 |
✓✗✓✗
|
20 |
deprecated<>("Deprecated. Use reg_min")), |
206 |
✓✗ |
10 |
"minimum regularization value.") |
207 |
|
|
.add_property("regMax", |
208 |
✓✗ |
10 |
bp::make_function(&SolverDDP::get_reg_max, |
209 |
✓✗✓✗
|
20 |
deprecated<>("Deprecated. Use reg_max")), |
210 |
✓✗ |
20 |
bp::make_function(&SolverDDP::set_reg_max, |
211 |
✓✗✓✗
|
20 |
deprecated<>("Deprecated. Use reg_max")), |
212 |
✓✗ |
10 |
"maximum regularization value.") |
213 |
✓✗ |
10 |
.add_property("th_stepDec", bp::make_function(&SolverDDP::get_th_stepdec), |
214 |
✓✗ |
20 |
bp::make_function(&SolverDDP::set_th_stepdec), |
215 |
|
|
"threshold for decreasing the regularization after " |
216 |
|
|
"approving a step (higher values decreases the " |
217 |
✓✗ |
10 |
"regularization)") |
218 |
✓✗ |
10 |
.add_property("th_stepInc", bp::make_function(&SolverDDP::get_th_stepinc), |
219 |
✓✗ |
20 |
bp::make_function(&SolverDDP::set_th_stepinc), |
220 |
|
|
"threshold for increasing the regularization after " |
221 |
|
|
"approving a step (higher values decreases the " |
222 |
✓✗ |
10 |
"regularization)") |
223 |
✓✗ |
10 |
.add_property("th_grad", bp::make_function(&SolverDDP::get_th_grad), |
224 |
✓✗ |
20 |
bp::make_function(&SolverDDP::set_th_grad), |
225 |
|
|
"threshold for accepting step which gradients is lower " |
226 |
✓✗ |
10 |
"than this value") |
227 |
|
|
.add_property( |
228 |
|
|
"th_gaptol", |
229 |
✓✗ |
10 |
bp::make_function(&SolverDDP::get_th_gaptol, |
230 |
✓✗✓✗
|
20 |
deprecated<>("Deprecated. Use th_gapTol")), |
231 |
✓✗ |
20 |
bp::make_function(&SolverDDP::set_th_gaptol, |
232 |
✓✗✓✗
|
20 |
deprecated<>("Deprecated. Use th_gapTol")), |
233 |
✓✗ |
10 |
"threshold for accepting a gap as non-zero") |
234 |
|
|
.add_property("alphas", |
235 |
✓✗ |
10 |
bp::make_function( |
236 |
|
|
&SolverDDP::get_alphas, |
237 |
|
|
bp::return_value_policy<bp::copy_const_reference>()), |
238 |
✓✗ |
20 |
bp::make_function(&SolverDDP::set_alphas), |
239 |
✓✗ |
10 |
"list of step length (alpha) values") |
240 |
✓✗ |
10 |
.def(CopyableVisitor<SolverDDP>()); |
241 |
|
10 |
} |
242 |
|
|
|
243 |
|
|
} // namespace python |
244 |
|
|
} // namespace crocoddyl |