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