GCC Code Coverage Report

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