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