1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh, |
5 |
|
|
// University of Oxford, Heriot-Watt University |
6 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
7 |
|
|
// All rights reserved. |
8 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
9 |
|
|
|
10 |
|
|
#include "crocoddyl/core/optctrl/shooting.hpp" |
11 |
|
|
|
12 |
|
|
#include <memory> |
13 |
|
|
|
14 |
|
|
#include "python/crocoddyl/core/core.hpp" |
15 |
|
|
#include "python/crocoddyl/utils/copyable.hpp" |
16 |
|
|
#include "python/crocoddyl/utils/deprecate.hpp" |
17 |
|
|
#include "python/crocoddyl/utils/printable.hpp" |
18 |
|
|
|
19 |
|
|
namespace crocoddyl { |
20 |
|
|
namespace python { |
21 |
|
|
|
22 |
|
10 |
void exposeShootingProblem() { |
23 |
|
|
// TODO: Remove once the deprecated update call has been removed in a future |
24 |
|
|
// release |
25 |
|
|
#pragma GCC diagnostic push |
26 |
|
|
#pragma GCC diagnostic ignored "-Wdeprecated-declarations" |
27 |
|
|
|
28 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<ShootingProblem> >(); |
29 |
|
|
|
30 |
✓✗ |
10 |
bp::class_<ShootingProblem>( |
31 |
|
|
"ShootingProblem", |
32 |
|
|
"Declare a shooting problem.\n\n" |
33 |
|
|
"A shooting problem declares the initial state, a set of running action " |
34 |
|
|
"models and a\n" |
35 |
|
|
"terminal action model. It has three main methods - calc, calcDiff and " |
36 |
|
|
"rollout. The\n" |
37 |
|
|
"first computes the set of next states and cost values per each action " |
38 |
|
|
"model. calcDiff\n" |
39 |
|
|
"updates the derivatives of all action models. The last rollouts the " |
40 |
|
|
"stacks of actions\n" |
41 |
|
|
"models.", |
42 |
✓✗ |
10 |
bp::init<Eigen::VectorXd, |
43 |
|
|
std::vector<boost::shared_ptr<ActionModelAbstract> >, |
44 |
|
|
boost::shared_ptr<ActionModelAbstract> >( |
45 |
|
20 |
bp::args("self", "x0", "runningModels", "terminalModel"), |
46 |
|
|
"Initialize the shooting problem and allocate its data.\n\n" |
47 |
|
|
":param x0: initial state\n" |
48 |
|
|
":param runningModels: running action models (size T)\n" |
49 |
|
|
":param terminalModel: terminal action model")) |
50 |
✓✗ |
10 |
.def(bp::init<Eigen::VectorXd, |
51 |
|
|
std::vector<boost::shared_ptr<ActionModelAbstract> >, |
52 |
|
|
boost::shared_ptr<ActionModelAbstract>, |
53 |
|
|
std::vector<boost::shared_ptr<ActionDataAbstract> >, |
54 |
|
|
boost::shared_ptr<ActionDataAbstract> >( |
55 |
✓✗ |
20 |
bp::args("self", "x0", "runningModels", "terminalModel", |
56 |
|
|
"runningDatas", "terminalData"), |
57 |
|
|
"Initialize the shooting problem (models and datas).\n\n" |
58 |
|
|
":param x0: initial state\n" |
59 |
|
|
":param runningModels: running action models (size T)\n" |
60 |
|
|
":param terminalModel: terminal action model\n" |
61 |
|
|
":param runningDatas: running action datas (size T)\n" |
62 |
✓✗ |
10 |
":param terminalData: terminal action data")) |
63 |
✓✗ |
20 |
.def("calc", &ShootingProblem::calc, bp::args("self", "xs", "us"), |
64 |
|
|
"Compute the cost and the next states.\n\n" |
65 |
|
|
"For each node k, and along the state xs and control us " |
66 |
|
|
"trajectories, it computes the next state x_{k+1}\n" |
67 |
|
|
" and cost l_k.\n" |
68 |
|
|
":param xs: time-discrete state trajectory (size T+1)\n" |
69 |
|
|
":param us: time-discrete control sequence (size T)\n" |
70 |
✓✗ |
10 |
":returns the total cost value") |
71 |
✓✗ |
20 |
.def("calcDiff", &ShootingProblem::calcDiff, bp::args("self", "xs", "us"), |
72 |
|
|
"Compute the derivatives of the cost and dynamics.\n\n" |
73 |
|
|
"For each node k, and along the state x_s and control u_s " |
74 |
|
|
"trajectories, it computes the derivatives of\n" |
75 |
|
|
"the cost (lx, lu, lxx, lxu, luu) and dynamics (fx, fu).\n" |
76 |
|
|
":param xs: time-discrete state trajectory (size T+1)\n" |
77 |
|
|
":param us: time-discrete control sequence (size T)\n" |
78 |
✓✗ |
10 |
":returns the total cost value") |
79 |
✓✗ |
20 |
.def("rollout", &ShootingProblem::rollout_us, bp::args("self", "us"), |
80 |
|
|
"Integrate the dynamics given a control sequence.\n\n" |
81 |
|
|
"Rollout the dynamics give a sequence of control commands\n" |
82 |
✓✗ |
10 |
":param us: time-discrete control sequence (size T)") |
83 |
|
|
.def("quasiStatic", &ShootingProblem::quasiStatic_xs, |
84 |
✓✗ |
20 |
bp::args("self", "xs"), |
85 |
|
|
"Compute the quasi static commands given a state trajectory.\n\n" |
86 |
|
|
"Generally speaking, it uses Newton-Raphson method for computing " |
87 |
|
|
"the quasi static commands.\n" |
88 |
✓✗ |
10 |
":param xs: time-discrete state trajectory (size T)") |
89 |
|
|
.def<void (ShootingProblem::*)(boost::shared_ptr<ActionModelAbstract>, |
90 |
|
|
boost::shared_ptr<ActionDataAbstract>)>( |
91 |
|
|
"circularAppend", &ShootingProblem::circularAppend, |
92 |
✓✗ |
20 |
bp::args("self", "model", "data"), |
93 |
|
|
"Circular append the model and data onto the end running node.\n\n" |
94 |
|
|
"Once we update the end running node, the first running mode is " |
95 |
|
|
"removed as in a circular buffer.\n" |
96 |
|
|
":param model: new model\n" |
97 |
|
20 |
":param data: new data") |
98 |
|
|
.def<void (ShootingProblem::*)(boost::shared_ptr<ActionModelAbstract>)>( |
99 |
|
|
"circularAppend", &ShootingProblem::circularAppend, |
100 |
✓✗ |
20 |
bp::args("self", "model"), |
101 |
|
|
"Circular append the model and data onto the end running node.\n\n" |
102 |
|
|
"Once we update the end running node, the first running mode is " |
103 |
|
|
"removed as in a circular buffer.\n" |
104 |
|
|
"Note that this method allocates new data for the end running node.\n" |
105 |
✓✗ |
10 |
":param model: new model") |
106 |
|
|
.def("updateNode", &ShootingProblem::updateNode, |
107 |
✓✗ |
20 |
bp::args("self", "i", "model", "data"), |
108 |
|
|
"Update the model and data for a specific node.\n\n" |
109 |
|
|
":param i: index of the node (0 <= i <= T + 1)\n" |
110 |
|
|
":param model: new model\n" |
111 |
✓✗✓✗
|
20 |
":param data: new data") |
112 |
|
|
.def("updateModel", &ShootingProblem::updateModel, |
113 |
✓✗ |
20 |
bp::args("self", "i", "model"), |
114 |
|
|
"Update a model and allocated new data for a specific node.\n\n" |
115 |
|
|
":param i: index of the node (0 <= i <= T + 1)\n" |
116 |
✓✗ |
10 |
":param model: new model") |
117 |
✓✗ |
20 |
.add_property("T", bp::make_function(&ShootingProblem::get_T), |
118 |
✓✗ |
10 |
"number of running nodes") |
119 |
|
|
.add_property("x0", |
120 |
|
|
bp::make_function(&ShootingProblem::get_x0, |
121 |
|
10 |
bp::return_internal_reference<>()), |
122 |
✓✗✓✗
|
10 |
&ShootingProblem::set_x0, "initial state") |
123 |
|
|
.add_property( |
124 |
|
|
"runningModels", |
125 |
|
|
bp::make_function(&ShootingProblem::get_runningModels, |
126 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
127 |
✓✗✓✗
|
10 |
&ShootingProblem::set_runningModels, "running models") |
128 |
|
|
.add_property( |
129 |
|
|
"terminalModel", |
130 |
|
|
bp::make_function(&ShootingProblem::get_terminalModel, |
131 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
132 |
✓✗✓✗
|
10 |
&ShootingProblem::set_terminalModel, "terminal model") |
133 |
|
|
.add_property( |
134 |
|
|
"runningDatas", |
135 |
✓✗ |
10 |
bp::make_function(&ShootingProblem::get_runningDatas, |
136 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
137 |
✓✗ |
10 |
"running datas") |
138 |
|
|
.add_property( |
139 |
|
|
"terminalData", |
140 |
✓✗ |
10 |
bp::make_function(&ShootingProblem::get_terminalData, |
141 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
142 |
✓✗ |
10 |
"terminal data") |
143 |
|
|
.add_property("nthreads", |
144 |
✓✗ |
10 |
bp::make_function(&ShootingProblem::get_nthreads), |
145 |
✓✗ |
20 |
bp::make_function(&ShootingProblem::set_nthreads), |
146 |
|
|
"number of threads launch by the multi-threading support " |
147 |
|
|
"(if you set nthreads <= 1, then " |
148 |
✓✗ |
10 |
"nthreads=CROCODDYL_WITH_NTHREADS)") |
149 |
✓✗ |
20 |
.add_property("nx", bp::make_function(&ShootingProblem::get_nx), |
150 |
✓✗ |
10 |
"dimension of state tuple") |
151 |
✓✗ |
20 |
.add_property("ndx", bp::make_function(&ShootingProblem::get_ndx), |
152 |
✓✗ |
10 |
"dimension of the tangent space of the state manifold") |
153 |
|
|
.add_property( |
154 |
|
|
"nu_max", |
155 |
✓✗ |
10 |
bp::make_function(&ShootingProblem::get_nu_max, |
156 |
✓✗✓✗
|
20 |
deprecated<>("Compute yourself the maximum " |
157 |
|
|
"dimension of the control vector")), |
158 |
✓✗ |
10 |
"dimension of the maximum control vector") |
159 |
|
|
.add_property("is_updated", |
160 |
✓✗ |
20 |
bp::make_function(&ShootingProblem::is_updated), |
161 |
|
|
"Returns True if the shooting problem has been updated, " |
162 |
✓✗ |
10 |
"otherwise False") |
163 |
✓✗ |
10 |
.def(CopyableVisitor<ShootingProblem>()) |
164 |
✓✗ |
10 |
.def(PrintableVisitor<ShootingProblem>()); |
165 |
|
|
|
166 |
|
|
#pragma GCC diagnostic pop |
167 |
|
10 |
} |
168 |
|
|
|
169 |
|
|
} // namespace python |
170 |
|
|
} // namespace crocoddyl |