GCC Code Coverage Report


Directory: ./
File: bindings/python/crocoddyl/multibody/actions/contact-invdyn-float.cpp
Date: 2025-04-18 16:41:15
Exec Total Coverage
Lines: 0 40 0.0%
Branches: 0 170 0.0%

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