1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2019-2023, 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/costs/cost-sum.hpp" |
11 |
|
|
|
12 |
|
|
#include <functional> |
13 |
|
|
#include <map> |
14 |
|
|
#include <memory> |
15 |
|
|
|
16 |
|
|
#include "python/crocoddyl/core/action-base.hpp" |
17 |
|
|
#include "python/crocoddyl/core/core.hpp" |
18 |
|
|
#include "python/crocoddyl/core/diff-action-base.hpp" |
19 |
|
|
#include "python/crocoddyl/utils/copyable.hpp" |
20 |
|
|
#include "python/crocoddyl/utils/deprecate.hpp" |
21 |
|
|
#include "python/crocoddyl/utils/map-converter.hpp" |
22 |
|
|
#include "python/crocoddyl/utils/printable.hpp" |
23 |
|
|
|
24 |
|
|
namespace crocoddyl { |
25 |
|
|
namespace python { |
26 |
|
|
|
27 |
✓✗ |
130 |
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(CostModelSum_addCost_wrap, |
28 |
|
|
CostModelSum::addCost, 3, 4) |
29 |
|
|
|
30 |
|
10 |
void exposeCostSum() { |
31 |
|
|
// Register custom converters between std::map and Python dict |
32 |
|
|
typedef boost::shared_ptr<CostItem> CostItemPtr; |
33 |
|
|
typedef boost::shared_ptr<CostDataAbstract> CostDataPtr; |
34 |
|
|
StdMapPythonVisitor<std::string, CostItemPtr, std::less<std::string>, |
35 |
|
|
std::allocator<std::pair<const std::string, CostItemPtr>>, |
36 |
✓✗✓✗
✓✗ |
10 |
true>::expose("StdMap_CostItem"); |
37 |
|
|
StdMapPythonVisitor<std::string, CostDataPtr, std::less<std::string>, |
38 |
|
|
std::allocator<std::pair<const std::string, CostDataPtr>>, |
39 |
✓✗✓✗
✓✗ |
10 |
true>::expose("StdMap_CostData"); |
40 |
|
|
|
41 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<CostItem>>(); |
42 |
|
|
|
43 |
✓✗ |
10 |
bp::class_<CostItem>( |
44 |
|
|
"CostItem", "Describe a cost item.\n\n", |
45 |
✓✗ |
10 |
bp::init<std::string, boost::shared_ptr<CostModelAbstract>, double, |
46 |
|
|
bp::optional<bool>>( |
47 |
|
20 |
bp::args("self", "name", "cost", "weight", "active"), |
48 |
|
|
"Initialize the cost item.\n\n" |
49 |
|
|
":param name: cost name\n" |
50 |
|
|
":param cost: cost model\n" |
51 |
|
|
":param weight: cost weight\n" |
52 |
|
|
":param active: True if the cost is activated (default true)")) |
53 |
✓✗ |
10 |
.def_readwrite("name", &CostItem::name, "cost name") |
54 |
|
|
.add_property( |
55 |
|
|
"cost", |
56 |
✓✗ |
10 |
bp::make_getter(&CostItem::cost, |
57 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
58 |
✓✗ |
10 |
"cost model") |
59 |
✓✗ |
10 |
.def_readwrite("weight", &CostItem::weight, "cost weight") |
60 |
✓✗ |
10 |
.def_readwrite("active", &CostItem::active, "cost status") |
61 |
✓✗ |
10 |
.def(CopyableVisitor<CostItem>()) |
62 |
✓✗ |
10 |
.def(PrintableVisitor<CostItem>()); |
63 |
|
|
|
64 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<CostModelSum>>(); |
65 |
|
|
|
66 |
✓✗ |
10 |
bp::class_<CostModelSum>( |
67 |
✓✗ |
10 |
"CostModelSum", bp::init<boost::shared_ptr<StateAbstract>, std::size_t>( |
68 |
|
20 |
bp::args("self", "state", "nu"), |
69 |
|
|
"Initialize the total cost model.\n\n" |
70 |
|
|
":param state: state description\n" |
71 |
|
|
":param nu: dimension of control vector")) |
72 |
✓✗ |
10 |
.def(bp::init<boost::shared_ptr<StateAbstract>, std::size_t>( |
73 |
✓✗ |
20 |
bp::args("self", "state", "nu"), |
74 |
|
|
"Initialize the total cost model.\n\n" |
75 |
|
|
"For this case the default nu is equals to model.nv.\n" |
76 |
|
|
":param state: state description\n" |
77 |
✓✗ |
10 |
":param nu: dimension of control vector")) |
78 |
✓✗ |
10 |
.def(bp::init<boost::shared_ptr<StateAbstract>>( |
79 |
✓✗ |
20 |
bp::args("self", "state"), |
80 |
|
|
"Initialize the total cost model.\n\n" |
81 |
|
|
"For this case the default nu is equals to model.nv.\n" |
82 |
✓✗ |
10 |
":param state: state description")) |
83 |
|
|
.def("addCost", &CostModelSum::addCost, |
84 |
✓✗ |
10 |
CostModelSum_addCost_wrap( |
85 |
✓✗ |
20 |
bp::args("self", "name", "cost", "weight", "active"), |
86 |
|
|
"Add a cost item.\n\n" |
87 |
|
|
":param name: cost name\n" |
88 |
|
|
":param cost: cost model\n" |
89 |
|
|
":param weight: cost weight\n" |
90 |
✓✗ |
10 |
":param active: True if the cost is activated (default true)")) |
91 |
✓✗ |
20 |
.def("removeCost", &CostModelSum::removeCost, bp::args("self", "name"), |
92 |
|
|
"Remove a cost item.\n\n" |
93 |
✓✗ |
10 |
":param name: cost name") |
94 |
|
|
.def( |
95 |
|
|
"changeCostStatus", &CostModelSum::changeCostStatus, |
96 |
✓✗ |
20 |
bp::args("self", "name", "active"), |
97 |
|
|
"Change the cost status.\n\n" |
98 |
|
|
":param name: cost name\n" |
99 |
✓✗ |
10 |
":param active: cost status (true for active and false for inactive)") |
100 |
|
|
.def<void (CostModelSum::*)(const boost::shared_ptr<CostDataSum>&, |
101 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
102 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
103 |
✓✗ |
20 |
"calc", &CostModelSum::calc, bp::args("self", "data", "x", "u"), |
104 |
|
|
"Compute the total cost.\n\n" |
105 |
|
|
":param data: cost-sum data\n" |
106 |
|
|
":param x: state point (dim. state.nx)\n" |
107 |
|
20 |
":param u: control input (dim. nu)") |
108 |
|
|
.def<void (CostModelSum::*)(const boost::shared_ptr<CostDataSum>&, |
109 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
110 |
✓✗ |
20 |
"calc", &CostModelSum::calc, bp::args("self", "data", "x"), |
111 |
|
|
"Compute the total cost value for nodes that depends only on the " |
112 |
|
|
"state.\n\n" |
113 |
|
|
"It updates the total cost based on the state only.\n" |
114 |
|
|
"This function is used in the terminal nodes of an optimal control " |
115 |
|
|
"problem.\n" |
116 |
|
|
":param data: cost-sum data\n" |
117 |
✓✗ |
10 |
":param x: state point (dim. state.nx)") |
118 |
|
|
.def<void (CostModelSum::*)(const boost::shared_ptr<CostDataSum>&, |
119 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
120 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
121 |
|
|
"calcDiff", &CostModelSum::calcDiff, |
122 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
123 |
|
|
"Compute the derivatives of the total cost.\n\n" |
124 |
|
|
"It assumes that calc has been run first.\n" |
125 |
|
|
":param data: action data\n" |
126 |
|
|
":param x: state point (dim. state.nx)\n" |
127 |
✓✗ |
10 |
":param u: control input (dim. nu)") |
128 |
|
|
.def<void (CostModelSum::*)(const boost::shared_ptr<CostDataSum>&, |
129 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
130 |
✓✗ |
20 |
"calcDiff", &CostModelSum::calcDiff, bp::args("self", "data", "x"), |
131 |
|
|
"Compute the Jacobian and Hessian of the total cost for nodes that " |
132 |
|
|
"depends on the state only.\n\n" |
133 |
|
|
"It updates the Jacobian and Hessian of the total cost based on the " |
134 |
|
|
"state only.\n" |
135 |
|
|
"This function is used in the terminal nodes of an optimal control " |
136 |
|
|
"problem.\n" |
137 |
|
|
":param data: cost-sum data\n" |
138 |
✓✗ |
10 |
":param x: state point (dim. state.nx)") |
139 |
|
|
.def("createData", &CostModelSum::createData, |
140 |
|
|
bp::with_custodian_and_ward_postcall<0, 2>(), |
141 |
✓✗ |
20 |
bp::args("self", "data"), |
142 |
|
|
"Create the total cost data.\n\n" |
143 |
|
|
":param data: shared data\n" |
144 |
✓✗✓✗
|
20 |
":return total cost data.") |
145 |
|
|
.add_property( |
146 |
|
|
"state", |
147 |
✓✗ |
10 |
bp::make_function(&CostModelSum::get_state, |
148 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
149 |
✓✗ |
10 |
"state description") |
150 |
|
|
.add_property( |
151 |
|
|
"costs", |
152 |
✓✗ |
10 |
bp::make_function(&CostModelSum::get_costs, |
153 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
154 |
✓✗ |
10 |
"stack of costs") |
155 |
✓✗ |
20 |
.add_property("nu", bp::make_function(&CostModelSum::get_nu), |
156 |
✓✗ |
10 |
"dimension of control vector") |
157 |
✓✗ |
20 |
.add_property("nr", bp::make_function(&CostModelSum::get_nr), |
158 |
✓✗ |
10 |
"dimension of the residual vector of active cost") |
159 |
✓✗ |
20 |
.add_property("nr_total", bp::make_function(&CostModelSum::get_nr_total), |
160 |
✓✗ |
10 |
"dimension of the total residual vector") |
161 |
|
|
.add_property( |
162 |
|
|
"active", |
163 |
✓✗ |
10 |
bp::make_function( |
164 |
|
|
&CostModelSum::get_active, |
165 |
✓✗✓✗
|
20 |
deprecated<bp::return_value_policy<bp::return_by_value>>( |
166 |
|
|
"Deprecated. Use property active_set")), |
167 |
✓✗ |
10 |
"list of names of active contact items") |
168 |
|
|
.add_property( |
169 |
|
|
"inactive", |
170 |
✓✗ |
10 |
bp::make_function( |
171 |
|
|
&CostModelSum::get_inactive, |
172 |
✓✗✓✗
|
20 |
deprecated<bp::return_value_policy<bp::return_by_value>>( |
173 |
|
|
"Deprecated. Use property inactive_set")), |
174 |
✓✗ |
10 |
"list of names of inactive contact items") |
175 |
|
|
.add_property( |
176 |
|
|
"active_set", |
177 |
✓✗ |
10 |
bp::make_function(&CostModelSum::get_active_set, |
178 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
179 |
✓✗ |
10 |
"name of the active set of cost items") |
180 |
|
|
.add_property( |
181 |
|
|
"inactive_set", |
182 |
✓✗ |
10 |
bp::make_function(&CostModelSum::get_inactive_set, |
183 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
184 |
✓✗ |
10 |
"name of the inactive set of cost items") |
185 |
|
|
.def("getCostStatus", &CostModelSum::getCostStatus, |
186 |
✓✗ |
20 |
bp::args("self", "name"), |
187 |
|
|
"Return the cost status of a given cost name.\n\n" |
188 |
✓✗ |
10 |
":param name: cost name") |
189 |
✓✗ |
10 |
.def(CopyableVisitor<CostModelSum>()) |
190 |
✓✗ |
10 |
.def(PrintableVisitor<CostModelSum>()); |
191 |
|
|
|
192 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<CostDataSum>>(); |
193 |
|
|
|
194 |
✓✗ |
10 |
bp::class_<CostDataSum>( |
195 |
|
|
"CostDataSum", "Class for total cost data.\n\n", |
196 |
✓✗ |
10 |
bp::init<CostModelSum*, DataCollectorAbstract*>( |
197 |
✓✗ |
10 |
bp::args("self", "model", "data"), |
198 |
|
|
"Create total cost data.\n\n" |
199 |
|
|
":param model: total cost model\n" |
200 |
✓✗ |
20 |
":param data: shared data")[bp::with_custodian_and_ward<1, 3>()]) |
201 |
|
|
.def( |
202 |
|
|
"shareMemory", |
203 |
|
|
&CostDataSum::shareMemory<DifferentialActionDataAbstract>, |
204 |
✓✗ |
20 |
bp::args("self", "model"), |
205 |
|
|
"Share memory with a given differential action data\n\n" |
206 |
|
20 |
":param model: differential action data that we want to share memory") |
207 |
|
|
.def("shareMemory", &CostDataSum::shareMemory<ActionDataAbstract>, |
208 |
✓✗ |
20 |
bp::args("self", "model"), |
209 |
|
|
"Share memory with a given action data\n\n" |
210 |
✓✗ |
10 |
":param model: action data that we want to share memory") |
211 |
|
|
.add_property( |
212 |
|
|
"costs", |
213 |
✓✗ |
10 |
bp::make_getter(&CostDataSum::costs, |
214 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
215 |
✓✗✓✗
|
20 |
"stack of costs data") |
216 |
|
|
.add_property("shared", |
217 |
✓✗ |
10 |
bp::make_getter(&CostDataSum::shared, |
218 |
|
10 |
bp::return_internal_reference<>()), |
219 |
✓✗ |
10 |
"shared data") |
220 |
|
|
.add_property( |
221 |
|
|
"cost", |
222 |
✓✗ |
10 |
bp::make_getter(&CostDataSum::cost, |
223 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
224 |
✓✗✓✗
|
30 |
bp::make_setter(&CostDataSum::cost), "cost value") |
225 |
|
|
.add_property( |
226 |
|
|
"Lx", |
227 |
✓✗ |
10 |
bp::make_function(&CostDataSum::get_Lx, |
228 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
229 |
✓✗✓✗
|
30 |
bp::make_function(&CostDataSum::set_Lx), "Jacobian of the cost") |
230 |
|
|
.add_property( |
231 |
|
|
"Lu", |
232 |
✓✗ |
10 |
bp::make_function(&CostDataSum::get_Lu, |
233 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
234 |
✓✗✓✗
|
30 |
bp::make_function(&CostDataSum::set_Lu), "Jacobian of the cost") |
235 |
|
|
.add_property( |
236 |
|
|
"Lxx", |
237 |
✓✗ |
10 |
bp::make_function(&CostDataSum::get_Lxx, |
238 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
239 |
✓✗✓✗
|
30 |
bp::make_function(&CostDataSum::set_Lxx), "Hessian of the cost") |
240 |
|
|
.add_property( |
241 |
|
|
"Lxu", |
242 |
✓✗ |
10 |
bp::make_function(&CostDataSum::get_Lxu, |
243 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
244 |
✓✗✓✗
|
30 |
bp::make_function(&CostDataSum::set_Lxu), "Hessian of the cost") |
245 |
|
|
.add_property( |
246 |
|
|
"Luu", |
247 |
✓✗ |
10 |
bp::make_function(&CostDataSum::get_Luu, |
248 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
249 |
✓✗✓✗
|
30 |
bp::make_function(&CostDataSum::set_Luu), "Hessian of the cost") |
250 |
✓✗ |
10 |
.def(CopyableVisitor<CostDataSum>()); |
251 |
|
10 |
} |
252 |
|
|
|
253 |
|
|
} // namespace python |
254 |
|
|
} // namespace crocoddyl |