Line |
Branch |
Exec |
Source |
1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2021-2025, University of Edinburgh, Heriot-Watt University |
5 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
6 |
|
|
// All rights reserved. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
// Auto-generated file for float |
10 |
|
|
#include "crocoddyl/multibody/actions/contact-invdyn.hpp" |
11 |
|
|
|
12 |
|
|
#include "python/crocoddyl/core/diff-action-base.hpp" |
13 |
|
|
#include "python/crocoddyl/multibody/multibody.hpp" |
14 |
|
|
|
15 |
|
|
namespace crocoddyl { |
16 |
|
|
namespace python { |
17 |
|
|
|
18 |
|
|
template <typename Model> |
19 |
|
|
struct DifferentialActionModelContactInvDynamicsVisitor |
20 |
|
|
: public bp::def_visitor< |
21 |
|
|
DifferentialActionModelContactInvDynamicsVisitor<Model>> { |
22 |
|
|
typedef typename Model::DifferentialActionDataAbstract Data; |
23 |
|
|
typedef typename Model::VectorXs VectorXs; |
24 |
|
|
template <class PyClass> |
25 |
|
✗ |
void visit(PyClass& cl) const { |
26 |
|
✗ |
cl.def("calc", |
27 |
|
|
static_cast<void (Model::*)( |
28 |
|
|
const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&, |
29 |
|
|
const Eigen::Ref<const VectorXs>&)>(&Model::calc), |
30 |
|
|
bp::args("self", "data", "x", "u"), |
31 |
|
|
"Compute the next state, cost value and constraints.\n\n" |
32 |
|
|
":param data: contact inverse-dynamics data\n" |
33 |
|
|
":param x: state point (dim. state.nx)\n" |
34 |
|
|
":param u: control input (dim. nu)") |
35 |
|
✗ |
.def("calc", |
36 |
|
|
static_cast<void (Model::*)(const std::shared_ptr<Data>&, |
37 |
|
|
const Eigen::Ref<const VectorXs>&)>( |
38 |
|
|
&Model::calc), |
39 |
|
|
bp::args("self", "data", "x")) |
40 |
|
✗ |
.def( |
41 |
|
|
"calcDiff", |
42 |
|
|
static_cast<void (Model::*)( |
43 |
|
|
const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&, |
44 |
|
|
const Eigen::Ref<const VectorXs>&)>(&Model::calcDiff), |
45 |
|
|
bp::args("self", "data", "x", "u"), |
46 |
|
|
"Compute the derivatives of the differential multibody system, and " |
47 |
|
|
"its cost and constraint functions.\n\n" |
48 |
|
|
"It computes the partial derivatives of the differential multibody " |
49 |
|
|
"system, the cost and constraint functions. It assumes that calc " |
50 |
|
|
"has been run first. This function builds a quadratic " |
51 |
|
|
"approximation of the action model (i.e., dynamical system, cost " |
52 |
|
|
"and constraint functions).\n" |
53 |
|
|
":param data: contact inverse-dynamics data\n" |
54 |
|
|
":param x: state point (dim. state.nx)\n" |
55 |
|
|
":param u: control input (dim. nu)") |
56 |
|
✗ |
.def("calcDiff", |
57 |
|
|
static_cast<void (Model::*)(const std::shared_ptr<Data>&, |
58 |
|
|
const Eigen::Ref<const VectorXs>&)>( |
59 |
|
|
&Model::calcDiff), |
60 |
|
|
bp::args("self", "data", "x")) |
61 |
|
✗ |
.def("createData", &Model::createData, bp::args("self"), |
62 |
|
|
"Create the contact inverse-dynamics differential action data.") |
63 |
|
✗ |
.add_property( |
64 |
|
|
"actuation", |
65 |
|
|
bp::make_function(&Model::get_actuation, |
66 |
|
✗ |
bp::return_value_policy<bp::return_by_value>()), |
67 |
|
|
"actuation model") |
68 |
|
✗ |
.add_property( |
69 |
|
|
"contacts", |
70 |
|
|
bp::make_function(&Model::get_contacts, |
71 |
|
✗ |
bp::return_value_policy<bp::return_by_value>()), |
72 |
|
|
"multiple contact model") |
73 |
|
✗ |
.add_property( |
74 |
|
|
"costs", |
75 |
|
|
bp::make_function(&Model::get_costs, |
76 |
|
✗ |
bp::return_value_policy<bp::return_by_value>()), |
77 |
|
|
"total cost model") |
78 |
|
✗ |
.add_property( |
79 |
|
|
"constraints", |
80 |
|
|
bp::make_function(&Model::get_constraints, |
81 |
|
✗ |
bp::return_value_policy<bp::return_by_value>()), |
82 |
|
|
"constraint model manager"); |
83 |
|
|
} |
84 |
|
|
}; |
85 |
|
|
|
86 |
|
|
template <typename Model> |
87 |
|
|
struct DifferentialActionModelContactInvDynamicsResidualModelActuationVisitor |
88 |
|
|
: public bp::def_visitor< |
89 |
|
|
DifferentialActionModelContactInvDynamicsResidualModelActuationVisitor< |
90 |
|
|
Model>> { |
91 |
|
|
typedef typename Model::ResidualDataAbstract Data; |
92 |
|
|
typedef typename Model::VectorXs VectorXs; |
93 |
|
|
template <class PyClass> |
94 |
|
✗ |
void visit(PyClass& cl) const { |
95 |
|
✗ |
cl.def("calc", |
96 |
|
|
static_cast<void (Model::*)( |
97 |
|
|
const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&, |
98 |
|
|
const Eigen::Ref<const VectorXs>&)>(&Model::calc), |
99 |
|
|
bp::args("self", "data", "x", "u"), |
100 |
|
|
"Compute the actuation residual.\n\n" |
101 |
|
|
":param data: residual data\n" |
102 |
|
|
":param x: state point (dim. state.nx)\n" |
103 |
|
|
":param u: control input (dim. nu)") |
104 |
|
✗ |
.def("calc", |
105 |
|
|
static_cast<void (Model::*)(const std::shared_ptr<Data>&, |
106 |
|
|
const Eigen::Ref<const VectorXs>&)>( |
107 |
|
|
&Model::calc), |
108 |
|
|
bp::args("self", "data", "x")) |
109 |
|
✗ |
.def( |
110 |
|
|
"calcDiff", |
111 |
|
|
static_cast<void (Model::*)( |
112 |
|
|
const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&, |
113 |
|
|
const Eigen::Ref<const VectorXs>&)>(&Model::calcDiff), |
114 |
|
|
bp::args("self", "data", "x", "u"), |
115 |
|
|
"Compute the Jacobians of the actuation residual.\n\n" |
116 |
|
|
"It assumes that calc has been run first.\n" |
117 |
|
|
":param data: action data\n" |
118 |
|
|
":param x: state point (dim. state.nx)\n" |
119 |
|
|
":param u: control input (dim. nu)\n") |
120 |
|
✗ |
.def("calcDiff", |
121 |
|
|
static_cast<void (Model::*)(const std::shared_ptr<Data>&, |
122 |
|
|
const Eigen::Ref<const VectorXs>&)>( |
123 |
|
|
&Model::calcDiff), |
124 |
|
|
bp::args("self", "data", "x")) |
125 |
|
✗ |
.def("createData", &Model::createData, |
126 |
|
✗ |
bp::with_custodian_and_ward_postcall<0, 2>(), |
127 |
|
|
bp::args("self", "data"), |
128 |
|
|
"Create the actuation residual data.\n\n" |
129 |
|
|
"Each residual model has its own data that needs to be allocated. " |
130 |
|
|
"This function returns the allocated data for the actuation " |
131 |
|
|
"residual.\n" |
132 |
|
|
":param data: shared data\n" |
133 |
|
|
":return residual data."); |
134 |
|
|
} |
135 |
|
|
}; |
136 |
|
|
|
137 |
|
|
template <typename Model> |
138 |
|
|
struct DifferentialActionModelContactInvDynamicsResidualModelContactVisitor |
139 |
|
|
: public bp::def_visitor< |
140 |
|
|
DifferentialActionModelContactInvDynamicsResidualModelContactVisitor< |
141 |
|
|
Model>> { |
142 |
|
|
typedef typename Model::ResidualDataAbstract Data; |
143 |
|
|
typedef typename Model::VectorXs VectorXs; |
144 |
|
|
template <class PyClass> |
145 |
|
✗ |
void visit(PyClass& cl) const { |
146 |
|
✗ |
cl.def("calc", |
147 |
|
|
static_cast<void (Model::*)( |
148 |
|
|
const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&, |
149 |
|
|
const Eigen::Ref<const VectorXs>&)>(&Model::calc), |
150 |
|
|
bp::args("self", "data", "x", "u"), |
151 |
|
|
"Compute the contact-acceleration residual.\n\n" |
152 |
|
|
":param data: residual data\n" |
153 |
|
|
":param x: state vector\n" |
154 |
|
|
":param u: control input") |
155 |
|
✗ |
.def("calc", |
156 |
|
|
static_cast<void (Model::*)(const std::shared_ptr<Data>&, |
157 |
|
|
const Eigen::Ref<const VectorXs>&)>( |
158 |
|
|
&Model::calc), |
159 |
|
|
bp::args("self", "data", "x")) |
160 |
|
✗ |
.def( |
161 |
|
|
"calcDiff", |
162 |
|
|
static_cast<void (Model::*)( |
163 |
|
|
const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&, |
164 |
|
|
const Eigen::Ref<const VectorXs>&)>(&Model::calcDiff), |
165 |
|
|
bp::args("self", "data", "x", "u"), |
166 |
|
|
"Compute the Jacobians of the contact-acceleration residual.\n\n" |
167 |
|
|
"It assumes that calc has been run first.\n" |
168 |
|
|
":param data: action data\n" |
169 |
|
|
":param x: state vector\n" |
170 |
|
|
":param u: control input\n") |
171 |
|
✗ |
.def("calcDiff", |
172 |
|
|
static_cast<void (Model::*)(const std::shared_ptr<Data>&, |
173 |
|
|
const Eigen::Ref<const VectorXs>&)>( |
174 |
|
|
&Model::calcDiff), |
175 |
|
|
bp::args("self", "data", "x")) |
176 |
|
✗ |
.def("createData", &Model::createData, |
177 |
|
✗ |
bp::with_custodian_and_ward_postcall<0, 2>(), |
178 |
|
|
bp::args("self", "data"), |
179 |
|
|
"Create the contact-acceleration residual data.\n\n" |
180 |
|
|
"Each residual model has its own data that needs to be allocated. " |
181 |
|
|
"This function returns the allocated data for the " |
182 |
|
|
"contact-acceleration residual.\n" |
183 |
|
|
":param data: shared data\n" |
184 |
|
|
":return residual data."); |
185 |
|
|
} |
186 |
|
|
}; |
187 |
|
|
|
188 |
|
|
template <typename Data> |
189 |
|
|
struct DifferentialActionDataContactInvDynamicsVisitor |
190 |
|
|
: public bp::def_visitor< |
191 |
|
|
DifferentialActionDataContactInvDynamicsVisitor<Data>> { |
192 |
|
|
template <class PyClass> |
193 |
|
✗ |
void visit(PyClass& cl) const { |
194 |
|
✗ |
cl.add_property( |
195 |
|
|
"pinocchio", |
196 |
|
✗ |
bp::make_getter(&Data::pinocchio, bp::return_internal_reference<>()), |
197 |
|
|
"pinocchio data") |
198 |
|
✗ |
.add_property("multibody", |
199 |
|
|
bp::make_getter(&Data::multibody, |
200 |
|
✗ |
bp::return_internal_reference<>()), |
201 |
|
|
"multibody data") |
202 |
|
✗ |
.add_property( |
203 |
|
|
"costs", |
204 |
|
|
bp::make_getter(&Data::costs, |
205 |
|
✗ |
bp::return_value_policy<bp::return_by_value>()), |
206 |
|
|
"total cost data") |
207 |
|
✗ |
.add_property( |
208 |
|
|
"constraints", |
209 |
|
|
bp::make_getter(&Data::constraints, |
210 |
|
✗ |
bp::return_value_policy<bp::return_by_value>()), |
211 |
|
|
"constraint data"); |
212 |
|
|
} |
213 |
|
|
}; |
214 |
|
|
|
215 |
|
|
#define CROCODDYL_ACTION_MODEL_CONTACT_INVDYN_PYTHON_BINDINGS(Scalar) \ |
216 |
|
|
typedef DifferentialActionModelContactInvDynamicsTpl<Scalar> Model; \ |
217 |
|
|
typedef DifferentialActionModelAbstractTpl<Scalar> ModelBase; \ |
218 |
|
|
typedef typename Model::StateMultibody State; \ |
219 |
|
|
typedef typename Model::ActuationModelAbstract Actuation; \ |
220 |
|
|
typedef typename Model::ContactModelMultiple Contacts; \ |
221 |
|
|
typedef typename Model::CostModelSum Costs; \ |
222 |
|
|
typedef typename Model::ConstraintModelManager Constraints; \ |
223 |
|
|
bp::register_ptr_to_python<std::shared_ptr<Model>>(); \ |
224 |
|
|
bp::scope model_outer = \ |
225 |
|
|
bp::class_<Model, bp::bases<ModelBase>>( \ |
226 |
|
|
"DifferentialActionModelContactInvDynamics", \ |
227 |
|
|
"Differential action model for inverse dynamics in multibody " \ |
228 |
|
|
"systems with contacts.\n\n" \ |
229 |
|
|
"This class implements forward kinematic with holonomic contact " \ |
230 |
|
|
"constraints (defined at the acceleration level) and " \ |
231 |
|
|
"inverse-dynamics computation using the Recursive Newton Euler " \ |
232 |
|
|
"Algorithm (RNEA). On the other hand, the stack of cost and " \ |
233 |
|
|
"constraint functions are implemented in ConstraintModelManager() " \ |
234 |
|
|
"and CostModelSum(), respectively.", \ |
235 |
|
|
bp::init<std::shared_ptr<State>, std::shared_ptr<Actuation>, \ |
236 |
|
|
std::shared_ptr<Contacts>, std::shared_ptr<Costs>, \ |
237 |
|
|
bp::optional<std::shared_ptr<Constraints>>>( \ |
238 |
|
|
bp::args("self", "state", "actuation", "contacts", "costs", \ |
239 |
|
|
"constraints"), \ |
240 |
|
|
"Initialize the inverse-dynamics action model for system with " \ |
241 |
|
|
"contact.\n\n" \ |
242 |
|
|
"It describes the kinematic evolution of the multibody system " \ |
243 |
|
|
"with contacts, and computes the needed torques using " \ |
244 |
|
|
"inverse-dynamics.\n." \ |
245 |
|
|
":param state: multibody state\n" \ |
246 |
|
|
":param actuation: abstract actuation model\n" \ |
247 |
|
|
":param contacts: stack of contact model\n" \ |
248 |
|
|
":param costs: stack of cost functions\n" \ |
249 |
|
|
":param constraints: stack of constraint functions")) \ |
250 |
|
|
.def(DifferentialActionModelContactInvDynamicsVisitor<Model>()) \ |
251 |
|
|
.def(CastVisitor<Model>()) \ |
252 |
|
|
.def(PrintableVisitor<Model>()) \ |
253 |
|
|
.def(CopyableVisitor<Model>()); \ |
254 |
|
|
typedef typename Model::ResidualModelActuation ResidualModelActuation; \ |
255 |
|
|
typedef typename ResidualModelActuation::Base ResidualModelBase; \ |
256 |
|
|
bp::register_ptr_to_python<std::shared_ptr<ResidualModelActuation>>(); \ |
257 |
|
|
bp::class_<ResidualModelActuation, bp::bases<ResidualModelBase>>( \ |
258 |
|
|
"ResidualModelActuation", \ |
259 |
|
|
"This residual function enforces the torques of under-actuated joints " \ |
260 |
|
|
"(e.g., floating-base joints) to be zero. We compute these torques and " \ |
261 |
|
|
"their derivatives using RNEA inside " \ |
262 |
|
|
"DifferentialActionModelContactInvDynamics.", \ |
263 |
|
|
bp::init<std::shared_ptr<State>, std::size_t, std::size_t>( \ |
264 |
|
|
bp::args("self", "state", "nu", "nc"), \ |
265 |
|
|
"Initialize the actuation residual model.\n\n" \ |
266 |
|
|
":param nu: dimension of control vector\n" \ |
267 |
|
|
":param nc: number of the contacts")) \ |
268 |
|
|
.def( \ |
269 |
|
|
DifferentialActionModelContactInvDynamicsResidualModelActuationVisitor< \ |
270 |
|
|
ResidualModelActuation>()) \ |
271 |
|
|
.def(CopyableVisitor<ResidualModelActuation>()); \ |
272 |
|
|
typedef typename Model::ResidualModelContact ResidualModelContact; \ |
273 |
|
|
bp::register_ptr_to_python<std::shared_ptr<ResidualModelContact>>(); \ |
274 |
|
|
bp::class_<ResidualModelContact, bp::bases<ResidualModelBase>>( \ |
275 |
|
|
"ResidualModelContact", \ |
276 |
|
|
"This residual function for the contact acceleration, i.e., r = a0, " \ |
277 |
|
|
"where a0 is the desired contact acceleration which also considers the " \ |
278 |
|
|
"Baumgarte stabilization.", \ |
279 |
|
|
bp::init<std::shared_ptr<State>, pinocchio::FrameIndex, std::size_t, \ |
280 |
|
|
std::size_t>( \ |
281 |
|
|
bp::args("self", "state", "id", "nr", "nc"), \ |
282 |
|
|
"Initialize the contact-acceleration residual model.\n\n" \ |
283 |
|
|
":param id: contact id\n" \ |
284 |
|
|
":param nr: dimension of contact residual\n" \ |
285 |
|
|
":param nc: dimension of contact vector")) \ |
286 |
|
|
.def( \ |
287 |
|
|
DifferentialActionModelContactInvDynamicsResidualModelContactVisitor< \ |
288 |
|
|
ResidualModelContact>()) \ |
289 |
|
|
.def(CopyableVisitor<ResidualModelContact>()); |
290 |
|
|
|
291 |
|
|
#define CROCODDYL_ACTION_DATA_CONTACT_INVDYN_PYTHON_BINDINGS(Scalar) \ |
292 |
|
|
typedef DifferentialActionDataContactInvDynamicsTpl<Scalar> Data; \ |
293 |
|
|
typedef DifferentialActionDataAbstractTpl<Scalar> DataBase; \ |
294 |
|
|
typedef DifferentialActionModelContactInvDynamicsTpl<Scalar> Model; \ |
295 |
|
|
bp::register_ptr_to_python<std::shared_ptr<Data>>(); \ |
296 |
|
|
bp::scope data_outer = \ |
297 |
|
|
bp::class_<Data, bp::bases<DataBase>>( \ |
298 |
|
|
"DifferentialActionDataContactInvDynamics", \ |
299 |
|
|
"Differential action data for the inverse-dynamics for system with " \ |
300 |
|
|
"contact.", \ |
301 |
|
|
bp::init<Model*>( \ |
302 |
|
|
bp::args("self", "model"), \ |
303 |
|
|
"Create inverse-dynamics action data for system with " \ |
304 |
|
|
"contacts.\n\n" \ |
305 |
|
|
":param model: contact inverse-dynamics action model")) \ |
306 |
|
|
.def(DifferentialActionDataContactInvDynamicsVisitor<Data>()) \ |
307 |
|
|
.def(CopyableVisitor<Data>()); \ |
308 |
|
|
typedef typename Model::ResidualModelActuation ResidualModelActuation; \ |
309 |
|
|
typedef typename Data::ResidualDataActuation ResidualDataActuation; \ |
310 |
|
|
typedef typename ResidualDataActuation::Base ResidualDataBase; \ |
311 |
|
|
typedef \ |
312 |
|
|
typename ResidualModelActuation::DataCollectorAbstract DataCollector; \ |
313 |
|
|
bp::register_ptr_to_python<std::shared_ptr<ResidualDataActuation>>(); \ |
314 |
|
|
bp::class_<ResidualDataActuation, bp::bases<ResidualDataBase>>( \ |
315 |
|
|
"ResidualDataActuation", "Data for actuation residual.\n\n", \ |
316 |
|
|
bp::init<ResidualModelActuation*, DataCollector*>( \ |
317 |
|
|
bp::args("self", "model", "data"), \ |
318 |
|
|
"Create actuation residual data.\n\n" \ |
319 |
|
|
":param model: actuation residual model\n" \ |
320 |
|
|
":param data: shared data")[bp::with_custodian_and_ward< \ |
321 |
|
|
1, 2, bp::with_custodian_and_ward<1, 3>>()]) \ |
322 |
|
|
.def(CopyableVisitor<ResidualDataActuation>()); \ |
323 |
|
|
typedef typename Model::ResidualModelContact ResidualModelContact; \ |
324 |
|
|
typedef typename Data::ResidualDataContact ResidualDataContact; \ |
325 |
|
|
bp::register_ptr_to_python<std::shared_ptr<ResidualDataContact>>(); \ |
326 |
|
|
bp::class_<ResidualDataContact, bp::bases<ResidualDataBase>>( \ |
327 |
|
|
"ResidualDataContact", "Data for contact acceleration residual.\n\n", \ |
328 |
|
|
bp::init<ResidualModelContact*, DataCollector*, std::size_t>( \ |
329 |
|
|
bp::args("self", "model", "data", "id"), \ |
330 |
|
|
"Create contact-acceleration residual data.\n\n" \ |
331 |
|
|
":param model: contact-acceleration residual model\n" \ |
332 |
|
|
":param data: shared data\n" \ |
333 |
|
|
":param id: contact id")[bp::with_custodian_and_ward< \ |
334 |
|
|
1, 2, bp::with_custodian_and_ward<1, 3>>()]) \ |
335 |
|
|
.add_property( \ |
336 |
|
|
"contact", \ |
337 |
|
|
bp::make_getter(&ResidualDataContact::contact, \ |
338 |
|
|
bp::return_value_policy<bp::return_by_value>()), \ |
339 |
|
|
"contact data associated with the current residual") \ |
340 |
|
|
.def(CopyableVisitor<ResidualDataContact>()); |
341 |
|
|
|
342 |
|
✗ |
void exposeDifferentialActionContactInvDynamics() { |
343 |
|
✗ |
CROCODDYL_ACTION_MODEL_CONTACT_INVDYN_PYTHON_BINDINGS(float) |
344 |
|
✗ |
CROCODDYL_ACTION_DATA_CONTACT_INVDYN_PYTHON_BINDINGS(float) |
345 |
|
|
} |
346 |
|
|
|
347 |
|
|
} // namespace python |
348 |
|
|
} // namespace crocoddyl |
349 |
|
|
|