1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2020-2023, University of Edinburgh, Heriot-Watt University |
5 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
6 |
|
|
// All rights reserved. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
#include "crocoddyl/core/constraints/constraint-manager.hpp" |
10 |
|
|
|
11 |
|
|
#include <functional> |
12 |
|
|
#include <map> |
13 |
|
|
#include <memory> |
14 |
|
|
#include <string> |
15 |
|
|
#include <utility> |
16 |
|
|
|
17 |
|
|
#include "python/crocoddyl/core/action-base.hpp" |
18 |
|
|
#include "python/crocoddyl/core/core.hpp" |
19 |
|
|
#include "python/crocoddyl/core/diff-action-base.hpp" |
20 |
|
|
#include "python/crocoddyl/utils/copyable.hpp" |
21 |
|
|
#include "python/crocoddyl/utils/map-converter.hpp" |
22 |
|
|
#include "python/crocoddyl/utils/printable.hpp" |
23 |
|
|
#include "python/crocoddyl/utils/set-converter.hpp" |
24 |
|
|
|
25 |
|
|
namespace crocoddyl { |
26 |
|
|
namespace python { |
27 |
|
|
|
28 |
✗✗ |
20 |
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS( |
29 |
|
|
ConstraintModelManager_addConstraint_wrap, |
30 |
|
|
ConstraintModelManager::addConstraint, 2, 3) |
31 |
|
|
|
32 |
|
10 |
void exposeConstraintManager() { |
33 |
|
|
// Register custom converters between std::map and Python dict |
34 |
|
|
typedef boost::shared_ptr<ConstraintItem> ConstraintItemPtr; |
35 |
|
|
typedef boost::shared_ptr<ConstraintDataAbstract> ConstraintDataPtr; |
36 |
|
|
StdMapPythonVisitor< |
37 |
|
|
std::string, ConstraintItemPtr, std::less<std::string>, |
38 |
|
|
std::allocator<std::pair<const std::string, ConstraintItemPtr> >, |
39 |
✓✗✓✗
✓✗ |
10 |
true>::expose("StdMap_ConstraintItem"); |
40 |
|
|
StdMapPythonVisitor< |
41 |
|
|
std::string, ConstraintDataPtr, std::less<std::string>, |
42 |
|
|
std::allocator<std::pair<const std::string, ConstraintDataPtr> >, |
43 |
✓✗✓✗
✓✗ |
10 |
true>::expose("StdMap_ConstraintData"); |
44 |
|
|
|
45 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<ConstraintItem> >(); |
46 |
|
|
|
47 |
✓✗ |
10 |
bp::class_<ConstraintItem>( |
48 |
|
|
"ConstraintItem", "Describe a constraint item.\n\n", |
49 |
✓✗ |
10 |
bp::init<std::string, boost::shared_ptr<ConstraintModelAbstract>, |
50 |
|
|
bp::optional<bool> >( |
51 |
|
20 |
bp::args("self", "name", "constraint", "active"), |
52 |
|
|
"Initialize the constraint item.\n\n" |
53 |
|
|
":param name: constraint name\n" |
54 |
|
|
":param constraint: constraint model\n" |
55 |
|
|
":param active: True if the constraint is activated (default true)")) |
56 |
✓✗ |
10 |
.def_readwrite("name", &ConstraintItem::name, "constraint name") |
57 |
|
|
.add_property( |
58 |
|
|
"constraint", |
59 |
✓✗ |
10 |
bp::make_getter(&ConstraintItem::constraint, |
60 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
61 |
✓✗ |
10 |
"constraint model") |
62 |
✓✗ |
10 |
.def_readwrite("active", &ConstraintItem::active, "constraint status") |
63 |
✓✗ |
10 |
.def(CopyableVisitor<ConstraintItem>()) |
64 |
✓✗ |
10 |
.def(PrintableVisitor<ConstraintItem>()); |
65 |
|
|
|
66 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<ConstraintModelManager> >(); |
67 |
|
|
|
68 |
✓✗ |
10 |
bp::class_<ConstraintModelManager>( |
69 |
|
|
"ConstraintModelManager", |
70 |
✓✗ |
10 |
bp::init<boost::shared_ptr<StateAbstract>, std::size_t>( |
71 |
|
20 |
bp::args("self", "state", "nu"), |
72 |
|
|
"Initialize the total constraint model.\n\n" |
73 |
|
|
":param state: state description\n" |
74 |
|
|
":param nu: dimension of control vector")) |
75 |
✓✗ |
10 |
.def(bp::init<boost::shared_ptr<StateAbstract>, std::size_t>( |
76 |
✓✗ |
20 |
bp::args("self", "state", "nu"), |
77 |
|
|
"Initialize the total constraint model.\n\n" |
78 |
|
|
"For this case the default nu is equals to model.nv.\n" |
79 |
|
|
":param state: state description\n" |
80 |
✓✗ |
10 |
":param nu: dimension of control vector")) |
81 |
✓✗ |
10 |
.def(bp::init<boost::shared_ptr<StateAbstract> >( |
82 |
✓✗ |
20 |
bp::args("self", "state"), |
83 |
|
|
"Initialize the total constraint model.\n\n" |
84 |
|
|
"For this case the default nu is equals to model.nv.\n" |
85 |
✓✗ |
10 |
":param state: state description")) |
86 |
|
|
.def("addConstraint", &ConstraintModelManager::addConstraint, |
87 |
✓✗ |
10 |
ConstraintModelManager_addConstraint_wrap( |
88 |
✓✗ |
20 |
bp::args("self", "name", "constraint", "active"), |
89 |
|
|
"Add a constraint item.\n\n" |
90 |
|
|
":param name: constraint name\n" |
91 |
|
|
":param constraint: constraint model\n" |
92 |
|
|
":param active: True if the constraint is activated (default " |
93 |
✓✗ |
10 |
"true)")) |
94 |
|
|
.def("removeConstraint", &ConstraintModelManager::removeConstraint, |
95 |
✓✗ |
20 |
bp::args("self", "name"), |
96 |
|
|
"Remove a constraint item.\n\n" |
97 |
✓✗ |
10 |
":param name: constraint name") |
98 |
|
|
.def("changeConstraintStatus", |
99 |
|
|
&ConstraintModelManager::changeConstraintStatus, |
100 |
✓✗ |
20 |
bp::args("self", "name", "active"), |
101 |
|
|
"Change the constraint status.\n\n" |
102 |
|
|
":param name: constraint name\n" |
103 |
|
|
":param active: constraint status (true for active and false for " |
104 |
✓✗ |
10 |
"inactive)") |
105 |
|
|
.def<void (ConstraintModelManager::*)( |
106 |
|
|
const boost::shared_ptr<ConstraintDataManager>&, |
107 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
108 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
109 |
|
|
"calc", &ConstraintModelManager::calc, |
110 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
111 |
|
|
"Compute the total constraint.\n\n" |
112 |
|
|
":param data: constraint-manager data\n" |
113 |
|
|
":param x: state point (dim. state.nx)\n" |
114 |
|
20 |
":param u: control input (dim. nu)") |
115 |
|
|
.def<void (ConstraintModelManager::*)( |
116 |
|
|
const boost::shared_ptr<ConstraintDataManager>&, |
117 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
118 |
✓✗ |
20 |
"calc", &ConstraintModelManager::calc, bp::args("self", "data", "x"), |
119 |
|
|
"Compute the total constraint value for nodes that depends only on " |
120 |
|
|
"the state.\n\n" |
121 |
|
|
"It updates the total constraint based on the state only.\n" |
122 |
|
|
"This function is used in the terminal nodes of an optimal control " |
123 |
|
|
"problem.\n" |
124 |
|
|
":param data: constraint-manager data\n" |
125 |
✓✗ |
10 |
":param x: state point (dim. state.nx)") |
126 |
|
|
.def<void (ConstraintModelManager::*)( |
127 |
|
|
const boost::shared_ptr<ConstraintDataManager>&, |
128 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
129 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
130 |
|
|
"calcDiff", &ConstraintModelManager::calcDiff, |
131 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
132 |
|
|
"Compute the derivatives of the total constraint.\n\n" |
133 |
|
|
"It assumes that calc has been run first.\n" |
134 |
|
|
":param data: constraint-manager data\n" |
135 |
|
|
":param x: state point (dim. state.nx)\n" |
136 |
✓✗ |
10 |
":param u: control input (dim. nu)\n") |
137 |
|
|
.def<void (ConstraintModelManager::*)( |
138 |
|
|
const boost::shared_ptr<ConstraintDataManager>&, |
139 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
140 |
|
|
"calcDiff", &ConstraintModelManager::calcDiff, |
141 |
✓✗ |
20 |
bp::args("self", "data", "x"), |
142 |
|
|
"Compute the Jacobian of the total constraint for nodes that depends " |
143 |
|
|
"on the state only.\n\n" |
144 |
|
|
"It updates the Jacobian of the total constraint based on the state " |
145 |
|
|
"only.\n" |
146 |
|
|
"This function is used in the terminal nodes of an optimal control " |
147 |
|
|
"problem.\n" |
148 |
|
|
":param data: constraint-manager data\n" |
149 |
✓✗ |
10 |
":param x: state point (dim. state.nx)") |
150 |
|
|
.def("createData", &ConstraintModelManager::createData, |
151 |
|
|
bp::with_custodian_and_ward_postcall<0, 2>(), |
152 |
✓✗ |
20 |
bp::args("self", "data"), |
153 |
|
|
"Create the total constraint data.\n\n" |
154 |
|
|
":param data: shared data\n" |
155 |
✓✗✓✗
|
20 |
":return total constraint data.") |
156 |
|
|
.add_property( |
157 |
|
|
"state", |
158 |
✓✗ |
10 |
bp::make_function(&ConstraintModelManager::get_state, |
159 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
160 |
✓✗ |
10 |
"state description") |
161 |
|
|
.add_property( |
162 |
|
|
"constraints", |
163 |
✓✗ |
10 |
bp::make_function(&ConstraintModelManager::get_constraints, |
164 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
165 |
✓✗ |
10 |
"stack of constraints") |
166 |
✓✗ |
20 |
.add_property("nu", bp::make_function(&ConstraintModelManager::get_nu), |
167 |
✓✗ |
10 |
"dimension of control vector") |
168 |
✓✗ |
20 |
.add_property("ng", bp::make_function(&ConstraintModelManager::get_ng), |
169 |
✓✗ |
10 |
"number of active inequality constraints") |
170 |
✓✗ |
20 |
.add_property("nh", bp::make_function(&ConstraintModelManager::get_nh), |
171 |
✓✗ |
10 |
"number of active equality constraints") |
172 |
|
|
.add_property( |
173 |
|
|
"active_set", |
174 |
✓✗ |
10 |
bp::make_function(&ConstraintModelManager::get_active_set, |
175 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
176 |
✓✗ |
10 |
"name of the active set of constraint items") |
177 |
|
|
.add_property( |
178 |
|
|
"inactive_set", |
179 |
✓✗ |
10 |
bp::make_function(&ConstraintModelManager::get_inactive_set, |
180 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
181 |
✓✗ |
10 |
"name of the inactive set of constraint items") |
182 |
|
|
.add_property("g_lb", |
183 |
✓✗ |
10 |
bp::make_function(&ConstraintModelManager::get_lb, |
184 |
|
10 |
bp::return_internal_reference<>()), |
185 |
✓✗ |
10 |
"lower bound of the inequality constraints") |
186 |
|
|
.add_property("g_ub", |
187 |
✓✗ |
10 |
bp::make_function(&ConstraintModelManager::get_ub, |
188 |
|
10 |
bp::return_internal_reference<>()), |
189 |
✓✗ |
10 |
"upper bound of the inequality constraints") |
190 |
|
|
.def("getConstraintStatus", &ConstraintModelManager::getConstraintStatus, |
191 |
✓✗ |
20 |
bp::args("self", "name"), |
192 |
|
|
"Return the constraint status of a given constraint name.\n\n" |
193 |
✓✗ |
10 |
":param name: constraint name") |
194 |
✓✗ |
10 |
.def(CopyableVisitor<ConstraintModelManager>()) |
195 |
✓✗ |
10 |
.def(PrintableVisitor<ConstraintModelManager>()); |
196 |
|
|
|
197 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<ConstraintDataManager> >(); |
198 |
|
|
|
199 |
✓✗ |
10 |
bp::class_<ConstraintDataManager>( |
200 |
|
|
"ConstraintDataManager", "Class for total constraint data.\n\n", |
201 |
✓✗ |
10 |
bp::init<ConstraintModelManager*, DataCollectorAbstract*>( |
202 |
✓✗ |
10 |
bp::args("self", "model", "data"), |
203 |
|
|
"Create total constraint data.\n\n" |
204 |
|
|
":param model: total constraint model\n" |
205 |
✓✗ |
20 |
":param data: shared data")[bp::with_custodian_and_ward<1, 3>()]) |
206 |
|
|
.def( |
207 |
|
|
"shareMemory", |
208 |
|
|
&ConstraintDataManager::shareMemory<DifferentialActionDataAbstract>, |
209 |
✓✗ |
20 |
bp::args("self", "data"), |
210 |
|
|
"Share memory with a given differential action data\n\n" |
211 |
|
20 |
":param model: differential action data that we want to share memory") |
212 |
|
|
.def("shareMemory", |
213 |
|
|
&ConstraintDataManager::shareMemory<ActionDataAbstract>, |
214 |
✓✗ |
20 |
bp::args("self", "data"), |
215 |
|
|
"Share memory with a given action data\n\n" |
216 |
✓✗ |
10 |
":param model: action data that we want to share memory") |
217 |
|
|
.def("resize", |
218 |
|
|
&ConstraintDataManager::resize<DifferentialActionModelAbstract, |
219 |
|
|
DifferentialActionDataAbstract>, |
220 |
|
|
bp::with_custodian_and_ward_postcall<0, 2>(), |
221 |
✓✗ |
20 |
bp::args("self", "model", "data"), |
222 |
|
|
"Resize the data given differential action data\n\n" |
223 |
|
|
":param model: differential action model that defines how to resize " |
224 |
|
|
"the data\n" |
225 |
✓✗ |
10 |
":param data: differential action data that we want to resize") |
226 |
|
|
.def("resize", |
227 |
|
|
&ConstraintDataManager::resize<ActionModelAbstract, |
228 |
|
|
ActionDataAbstract>, |
229 |
|
|
bp::with_custodian_and_ward_postcall<0, 2>(), |
230 |
✓✗ |
20 |
bp::args("self", "model", "data"), |
231 |
|
|
"Resize the data given action data\n\n" |
232 |
|
|
":param model: action model that defines how to resize the data\n" |
233 |
✓✗ |
10 |
":param data: action data that we want to resize") |
234 |
|
|
.add_property( |
235 |
|
|
"constraints", |
236 |
✓✗ |
10 |
bp::make_getter(&ConstraintDataManager::constraints, |
237 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
238 |
✓✗✓✗
|
20 |
"stack of constraints data") |
239 |
|
|
.add_property("shared", |
240 |
✓✗ |
10 |
bp::make_getter(&ConstraintDataManager::shared, |
241 |
|
10 |
bp::return_internal_reference<>()), |
242 |
✓✗ |
10 |
"shared data") |
243 |
|
|
.add_property( |
244 |
|
|
"g", |
245 |
✓✗ |
10 |
bp::make_function(&ConstraintDataManager::get_g, |
246 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
247 |
✓✗ |
20 |
bp::make_function(&ConstraintDataManager::set_g), |
248 |
✓✗ |
10 |
"Inequality constraint residual") |
249 |
|
|
.add_property( |
250 |
|
|
"Gx", |
251 |
✓✗ |
10 |
bp::make_function(&ConstraintDataManager::get_Gx, |
252 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
253 |
✓✗ |
20 |
bp::make_function(&ConstraintDataManager::set_Gx), |
254 |
✓✗ |
10 |
"Jacobian of the inequality constraint") |
255 |
|
|
.add_property( |
256 |
|
|
"Gu", |
257 |
✓✗ |
10 |
bp::make_function(&ConstraintDataManager::get_Gu, |
258 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
259 |
✓✗ |
20 |
bp::make_function(&ConstraintDataManager::set_Gu), |
260 |
✓✗ |
10 |
"Jacobian of the inequality constraint") |
261 |
|
|
.add_property( |
262 |
|
|
"h", |
263 |
✓✗ |
10 |
bp::make_function(&ConstraintDataManager::get_h, |
264 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
265 |
✓✗ |
20 |
bp::make_function(&ConstraintDataManager::set_h), |
266 |
✓✗ |
10 |
"Equality constraint residual") |
267 |
|
|
.add_property( |
268 |
|
|
"Hx", |
269 |
✓✗ |
10 |
bp::make_function(&ConstraintDataManager::get_Hx, |
270 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
271 |
✓✗ |
20 |
bp::make_function(&ConstraintDataManager::set_Hx), |
272 |
✓✗ |
10 |
"Jacobian of the equality constraint") |
273 |
|
|
.add_property( |
274 |
|
|
"Hu", |
275 |
✓✗ |
10 |
bp::make_function(&ConstraintDataManager::get_Hu, |
276 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
277 |
✓✗ |
20 |
bp::make_function(&ConstraintDataManager::set_Hu), |
278 |
✓✗ |
10 |
"Jacobian of the equality constraint") |
279 |
✓✗ |
10 |
.def(CopyableVisitor<ConstraintDataManager>()); |
280 |
|
10 |
} |
281 |
|
|
|
282 |
|
|
} // namespace python |
283 |
|
|
} // namespace crocoddyl |