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 "python/crocoddyl/core/diff-action-base.hpp" |
11 |
|
|
|
12 |
|
|
#include "python/crocoddyl/core/core.hpp" |
13 |
|
|
#include "python/crocoddyl/utils/copyable.hpp" |
14 |
|
|
#include "python/crocoddyl/utils/printable.hpp" |
15 |
|
|
#include "python/crocoddyl/utils/vector-converter.hpp" |
16 |
|
|
|
17 |
|
|
namespace crocoddyl { |
18 |
|
|
namespace python { |
19 |
|
|
|
20 |
|
10 |
void exposeDifferentialActionAbstract() { |
21 |
|
|
// Register custom converters between std::vector and Python list |
22 |
|
|
typedef boost::shared_ptr<DifferentialActionModelAbstract> |
23 |
|
|
DifferentialActionModelPtr; |
24 |
|
|
typedef boost::shared_ptr<DifferentialActionDataAbstract> |
25 |
|
|
DifferentialActionDataPtr; |
26 |
✓✗✓✗
✓✗ |
10 |
StdVectorPythonVisitor<std::vector<DifferentialActionModelPtr>, true>::expose( |
27 |
|
|
"StdVec_DiffActionModel"); |
28 |
✓✗✓✗
✓✗ |
10 |
StdVectorPythonVisitor<std::vector<DifferentialActionDataPtr>, true>::expose( |
29 |
|
|
"StdVec_DiffActionData"); |
30 |
|
|
|
31 |
|
|
bp::register_ptr_to_python< |
32 |
|
10 |
boost::shared_ptr<DifferentialActionModelAbstract> >(); |
33 |
|
|
|
34 |
✓✗ |
10 |
bp::class_<DifferentialActionModelAbstract_wrap, boost::noncopyable>( |
35 |
|
|
"DifferentialActionModelAbstract", |
36 |
|
|
"Abstract class for the differential action model.\n\n" |
37 |
|
|
"A differential action model is the time-continuous version of an action " |
38 |
|
|
"model. Each\n" |
39 |
|
|
"node, in our optimal control problem, is described through an action " |
40 |
|
|
"model. Every\n" |
41 |
|
|
"time that we want describe a problem, we need to provide ways of " |
42 |
|
|
"computing the\n" |
43 |
|
|
"dynamics, cost functions, constraints and their derivatives. These " |
44 |
|
|
"computations are\n" |
45 |
|
|
"mainly carried out inside calc() and calcDiff(), respectively.", |
46 |
✓✗ |
10 |
bp::init<boost::shared_ptr<StateAbstract>, std::size_t, |
47 |
|
|
bp::optional<std::size_t, std::size_t, std::size_t> >( |
48 |
|
20 |
bp::args("self", "state", "nu", "nr", "ng", "nh"), |
49 |
|
|
"Initialize the differential action model.\n\n" |
50 |
|
|
"We can also describe autonomous systems by setting nu = 0.\n" |
51 |
|
|
":param state: state\n" |
52 |
|
|
":param nu: dimension of control vector\n" |
53 |
|
|
":param nr: dimension of cost-residual vector (default 1)\n" |
54 |
|
|
":param ng: number of inequality constraints (default 0)\n" |
55 |
|
|
":param nh: number of equality constraints (default 0)")) |
56 |
|
|
.def("calc", pure_virtual(&DifferentialActionModelAbstract_wrap::calc), |
57 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
58 |
|
|
"Compute the system acceleration and cost value.\n\n" |
59 |
|
|
":param data: differential action data\n" |
60 |
|
|
":param x: state point (dim. state.nx)\n" |
61 |
✓✗✓✗
|
10 |
":param u: control input (dim. nu)") |
62 |
|
|
.def<void (DifferentialActionModelAbstract::*)( |
63 |
|
|
const boost::shared_ptr<DifferentialActionDataAbstract>&, |
64 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
65 |
|
|
"calc", &DifferentialActionModelAbstract::calc, |
66 |
✓✗ |
20 |
bp::args("self", "data", "x"), |
67 |
|
|
"Compute the total cost value for nodes that depends only on the " |
68 |
|
|
"state.\n\n" |
69 |
|
|
"It updates the total cost and the system acceleration is not " |
70 |
|
|
"updated as the control\n" |
71 |
|
|
"input is undefined. This function is used in the terminal nodes of " |
72 |
|
|
"an optimal\n" |
73 |
|
|
"control problem.\n" |
74 |
|
|
":param data: differential action data\n" |
75 |
|
20 |
":param x: state point (dim. state.nx)") |
76 |
|
|
.def("calcDiff", |
77 |
|
|
pure_virtual(&DifferentialActionModelAbstract_wrap::calcDiff), |
78 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
79 |
|
|
"Compute the derivatives of the dynamics and cost functions.\n\n" |
80 |
|
|
"It computes the partial derivatives of the dynamical system and " |
81 |
|
|
"the cost\n" |
82 |
|
|
"function. It assumes that calc has been run first.\n" |
83 |
|
|
"This function builds a quadratic approximation of the\n" |
84 |
|
|
"time-continuous action model (i.e. dynamical system and cost " |
85 |
|
|
"function).\n" |
86 |
|
|
":param data: differential action data\n" |
87 |
|
|
":param x: state point (dim. state.nx)\n" |
88 |
✓✗✓✗
✓✗ |
20 |
":param u: control input (dim. nu)") |
89 |
|
|
.def<void (DifferentialActionModelAbstract::*)( |
90 |
|
|
const boost::shared_ptr<DifferentialActionDataAbstract>&, |
91 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
92 |
|
|
"calcDiff", &DifferentialActionModelAbstract::calcDiff, |
93 |
✓✗ |
20 |
bp::args("self", "data", "x"), |
94 |
|
|
"Compute the derivatives of the cost functions with respect to the " |
95 |
|
|
"state only.\n\n" |
96 |
|
|
"It updates the derivatives of the cost function with respect to the " |
97 |
|
|
"state only.\n" |
98 |
|
|
"This function is used in the terminal nodes of an optimal control " |
99 |
|
|
"problem.\n" |
100 |
|
|
":param data: action data\n" |
101 |
|
20 |
":param x: state point (dim. state.nx)") |
102 |
|
|
.def("createData", &DifferentialActionModelAbstract_wrap::createData, |
103 |
|
|
&DifferentialActionModelAbstract_wrap::default_createData, |
104 |
✓✗ |
20 |
bp::args("self"), |
105 |
|
|
"Create the differential action data.\n\n" |
106 |
|
|
"Each differential action model has its own data that needs to be\n" |
107 |
|
|
"allocated. This function returns the allocated data for a " |
108 |
|
|
"predefined\n" |
109 |
|
|
"DAM.\n" |
110 |
✓✗✓✗
|
20 |
":return DAM data.") |
111 |
|
|
.def("quasiStatic", &DifferentialActionModelAbstract_wrap::quasiStatic_x, |
112 |
✓✗ |
10 |
DifferentialActionModel_quasiStatic_wraps( |
113 |
✓✗ |
20 |
bp::args("self", "data", "x", "maxiter", "tol"), |
114 |
|
|
"Compute the quasic-static control given a state.\n\n" |
115 |
|
|
"It runs an iterative Newton step in order to compute the " |
116 |
|
|
"quasic-static regime\n" |
117 |
|
|
"given a state configuration.\n" |
118 |
|
|
":param data: action data\n" |
119 |
|
|
":param x: discrete-time state vector\n" |
120 |
|
|
":param maxiter: maximum allowed number of iterations\n" |
121 |
|
|
":param tol: stopping tolerance criteria (default 1e-9)\n" |
122 |
✓✗ |
10 |
":return u: quasic-static control")) |
123 |
|
|
.def("quasiStatic", &DifferentialActionModelAbstract_wrap::quasiStatic, |
124 |
|
|
&DifferentialActionModelAbstract_wrap::default_quasiStatic, |
125 |
✓✗✓✗
|
20 |
bp::args("self", "data", "u", "x", "maxiter", "tol")) |
126 |
|
|
.add_property( |
127 |
|
|
"nu", |
128 |
✓✗ |
20 |
bp::make_function(&DifferentialActionModelAbstract_wrap::get_nu), |
129 |
✓✗ |
10 |
"dimension of control vector") |
130 |
|
|
.add_property( |
131 |
|
|
"nr", |
132 |
✓✗ |
20 |
bp::make_function(&DifferentialActionModelAbstract_wrap::get_nr), |
133 |
✓✗ |
10 |
"dimension of cost-residual vector") |
134 |
|
|
.add_property( |
135 |
|
|
"ng", |
136 |
✓✗ |
20 |
bp::make_function(&DifferentialActionModelAbstract_wrap::get_ng), |
137 |
✓✗ |
10 |
"number of inequality constraints") |
138 |
|
|
.add_property( |
139 |
|
|
"nh", |
140 |
✓✗ |
20 |
bp::make_function(&DifferentialActionModelAbstract_wrap::get_nh), |
141 |
✓✗ |
10 |
"number of equality constraints") |
142 |
|
|
.add_property( |
143 |
|
|
"state", |
144 |
✓✗ |
10 |
bp::make_function(&DifferentialActionModelAbstract_wrap::get_state, |
145 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
146 |
✓✗ |
10 |
"state") |
147 |
|
|
.add_property( |
148 |
|
|
"g_lb", |
149 |
|
|
bp::make_function(&DifferentialActionModelAbstract_wrap::get_g_lb, |
150 |
|
10 |
bp::return_internal_reference<>()), |
151 |
|
|
&DifferentialActionModelAbstract_wrap::set_g_lb, |
152 |
✓✗✓✗
|
10 |
"lower bound of the inequality constraints") |
153 |
|
|
.add_property( |
154 |
|
|
"g_ub", |
155 |
|
|
bp::make_function(&DifferentialActionModelAbstract_wrap::get_g_ub, |
156 |
|
10 |
bp::return_internal_reference<>()), |
157 |
|
|
&DifferentialActionModelAbstract_wrap::set_g_ub, |
158 |
✓✗✓✗
|
10 |
"upper bound of the inequality constraints") |
159 |
|
|
.add_property( |
160 |
|
|
"u_lb", |
161 |
|
|
bp::make_function(&DifferentialActionModelAbstract_wrap::get_u_lb, |
162 |
|
10 |
bp::return_internal_reference<>()), |
163 |
|
|
&DifferentialActionModelAbstract_wrap::set_u_lb, |
164 |
✓✗✓✗
|
10 |
"lower control limits") |
165 |
|
|
.add_property( |
166 |
|
|
"u_ub", |
167 |
|
|
bp::make_function(&DifferentialActionModelAbstract_wrap::get_u_ub, |
168 |
|
10 |
bp::return_internal_reference<>()), |
169 |
|
|
&DifferentialActionModelAbstract_wrap::set_u_ub, |
170 |
✓✗✓✗
|
10 |
"upper control limits") |
171 |
|
|
.add_property( |
172 |
|
|
"has_control_limits", |
173 |
✓✗ |
20 |
bp::make_function( |
174 |
|
|
&DifferentialActionModelAbstract_wrap::get_has_control_limits), |
175 |
✓✗ |
10 |
"indicates whether problem has finite control limits") |
176 |
✓✗ |
10 |
.def(PrintableVisitor<DifferentialActionModelAbstract>()); |
177 |
|
|
|
178 |
|
|
bp::register_ptr_to_python< |
179 |
|
10 |
boost::shared_ptr<DifferentialActionDataAbstract> >(); |
180 |
|
|
|
181 |
✓✗ |
10 |
bp::class_<DifferentialActionDataAbstract>( |
182 |
|
|
"DifferentialActionDataAbstract", |
183 |
|
|
"Abstract class for differential action data.\n\n" |
184 |
|
|
"In crocoddyl, an action data contains all the required information for " |
185 |
|
|
"processing an\n" |
186 |
|
|
"user-defined action model. The action data typically is allocated onces " |
187 |
|
|
"by running\n" |
188 |
|
|
"model.createData() and contains the first- and second- order " |
189 |
|
|
"derivatives of the dynamics\n" |
190 |
|
|
"and cost function, respectively.", |
191 |
✓✗ |
10 |
bp::init<DifferentialActionModelAbstract*>( |
192 |
|
20 |
bp::args("self", "model"), |
193 |
|
|
"Create common data shared between DAMs.\n\n" |
194 |
|
|
"The differential action data uses the model in order to first " |
195 |
|
|
"process it.\n" |
196 |
|
|
":param model: differential action model")) |
197 |
|
|
.add_property( |
198 |
|
|
"cost", |
199 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::cost, |
200 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
201 |
✓✗✓✗
|
30 |
bp::make_setter(&DifferentialActionDataAbstract::cost), "cost value") |
202 |
|
|
.add_property("xout", |
203 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::xout, |
204 |
|
|
bp::return_internal_reference<>()), |
205 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::xout), |
206 |
✓✗ |
10 |
"evolution state") |
207 |
|
|
.add_property("r", |
208 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::r, |
209 |
|
|
bp::return_internal_reference<>()), |
210 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::r), |
211 |
✓✗ |
10 |
"cost residual") |
212 |
|
|
.add_property("Fx", |
213 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::Fx, |
214 |
|
|
bp::return_internal_reference<>()), |
215 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::Fx), |
216 |
✓✗ |
10 |
"Jacobian of the dynamics w.r.t. the state") |
217 |
|
|
.add_property("Fu", |
218 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::Fu, |
219 |
|
|
bp::return_internal_reference<>()), |
220 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::Fu), |
221 |
✓✗ |
10 |
"Jacobian of the dynamics w.r.t. the control") |
222 |
|
|
.add_property("Lx", |
223 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::Lx, |
224 |
|
|
bp::return_internal_reference<>()), |
225 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::Lx), |
226 |
✓✗ |
10 |
"Jacobian of the cost w.r.t. the state") |
227 |
|
|
.add_property("Lu", |
228 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::Lu, |
229 |
|
|
bp::return_internal_reference<>()), |
230 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::Lu), |
231 |
✓✗ |
10 |
"Jacobian of the cost w.r.t. the control") |
232 |
|
|
.add_property("Lxx", |
233 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::Lxx, |
234 |
|
|
bp::return_internal_reference<>()), |
235 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::Lxx), |
236 |
✓✗ |
10 |
"Hessian of the cost w.r.t. the state") |
237 |
|
|
.add_property("Lxu", |
238 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::Lxu, |
239 |
|
|
bp::return_internal_reference<>()), |
240 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::Lxu), |
241 |
✓✗ |
10 |
"Hessian of the cost w.r.t. the state and control") |
242 |
|
|
.add_property("Luu", |
243 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::Luu, |
244 |
|
|
bp::return_internal_reference<>()), |
245 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::Luu), |
246 |
✓✗ |
10 |
"Hessian of the cost w.r.t. the control") |
247 |
|
|
.add_property("g", |
248 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::g, |
249 |
|
|
bp::return_internal_reference<>()), |
250 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::g), |
251 |
✓✗ |
10 |
"Inequality constraint values") |
252 |
|
|
.add_property("Gx", |
253 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::Gx, |
254 |
|
|
bp::return_internal_reference<>()), |
255 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::Gx), |
256 |
✓✗ |
10 |
"Jacobian of the inequality constraint w.r.t. the state") |
257 |
|
|
.add_property("Gu", |
258 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::Gu, |
259 |
|
|
bp::return_internal_reference<>()), |
260 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::Gu), |
261 |
✓✗ |
10 |
"Jacobian of the inequality constraint w.r.t. the control") |
262 |
|
|
.add_property("h", |
263 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::h, |
264 |
|
|
bp::return_internal_reference<>()), |
265 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::h), |
266 |
✓✗ |
10 |
"Equality constraint values") |
267 |
|
|
.add_property("Hx", |
268 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::Hx, |
269 |
|
|
bp::return_internal_reference<>()), |
270 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::Hx), |
271 |
✓✗ |
10 |
"Jacobian of the equality constraint w.r.t. the state") |
272 |
|
|
.add_property("Hu", |
273 |
✓✗ |
10 |
bp::make_getter(&DifferentialActionDataAbstract::Hu, |
274 |
|
|
bp::return_internal_reference<>()), |
275 |
✓✗ |
20 |
bp::make_setter(&DifferentialActionDataAbstract::Hu), |
276 |
✓✗ |
10 |
"Jacobian of the equality constraint w.r.t. the control") |
277 |
✓✗ |
10 |
.def(CopyableVisitor<DifferentialActionDataAbstract>()); |
278 |
|
10 |
} |
279 |
|
|
|
280 |
|
|
} // namespace python |
281 |
|
|
} // namespace crocoddyl |