GCC Code Coverage Report

Directory: ./
File: bindings/python/crocoddyl/multibody/contacts/multiple-contacts.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 53 55 96.4%
Branches: 125 250 50.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5 // University of Oxford, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
11
12 #include <functional>
13 #include <map>
14 #include <memory>
15
16 #include "python/crocoddyl/multibody/multibody.hpp"
17 #include "python/crocoddyl/utils/map-converter.hpp"
18 #include "python/crocoddyl/utils/set-converter.hpp"
19
20 namespace crocoddyl {
21 namespace python {
22
23 template <typename Model>
24 struct ContactItemVisitor : public bp::def_visitor<ContactItemVisitor<Model>> {
25 template <class PyClass>
26 40 void visit(PyClass& cl) const {
27
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 cl.def_readwrite("name", &Model::name, "contact name")
28
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
29 "contact",
30 bp::make_getter(&Model::contact,
31 ✗ bp::return_value_policy<bp::return_by_value>()),
32 "contact model")
33
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 .def_readwrite("active", &Model::active, "contact status");
34 40 }
35 };
36
37 template <typename Model>
38 struct ContactModelMultipleVisitor
39 : public bp::def_visitor<ContactModelMultipleVisitor<Model>> {
40
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 66 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ContactModelMultiple_addContact_wrap,
41 Model::addContact, 2, 3)
42 template <class PyClass>
43 40 void visit(PyClass& cl) const {
44 80 cl.def(
45 "addContact", &Model::addContact,
46
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 40 ContactModelMultiple_addContact_wrap(
47 bp::args("self", "name", "contact", "active"),
48 "Add a contact item.\n\n"
49 ":param name: contact name\n"
50 ":param contact: contact model\n"
51 ":param active: True if the contact is activated (default true)"))
52
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("removeContact", &Model::removeContact, bp::args("self", "name"),
53 "Remove a contact item.\n\n"
54 ":param name: contact name")
55
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("changeContactStatus", &Model::changeContactStatus,
56 bp::args("self", "name", "active"),
57 "Change the contact status.\n\n"
58 ":param name: contact name\n"
59 ":param active: contact status (true for active and false for "
60 "inactive)")
61
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calc", &Model::calc, bp::args("self", "data", "x"),
62 "Compute the contact Jacobian and contact acceleration.\n\n"
63 "The rigid contact model throught acceleration-base holonomic "
64 "constraint\n"
65 "of the contact frame placement.\n"
66 ":param data: contact data\n"
67 ":param x: state point (dim. state.nx)")
68
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calcDiff", &Model::calcDiff, bp::args("self", "data", "x"),
69 "Compute the derivatives of the contact holonomic constraint.\n\n"
70 "The rigid contact model throught acceleration-base holonomic "
71 "constraint\n"
72 "of the contact frame placement.\n"
73 "It assumes that calc has been run first.\n"
74 ":param data: contact data\n"
75 ":param x: state point (dim. state.nx)")
76
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("updateAcceleration", &Model::updateAcceleration,
77 bp::args("self", "data", "dv"),
78 "Update the constrained system acceleration.\n\n"
79 ":param data: contact data\n"
80 ":param dv: constrained acceleration (dimension nv)")
81
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("updateForce", &Model::updateForce,
82 bp::args("self", "data", "force"),
83 "Update the spatial force in frame coordinate.\n\n"
84 ":param data: contact data\n"
85 ":param force: force vector (dimension nc)")
86
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def(
87 "updateAccelerationDiff", &Model::updateAccelerationDiff,
88 bp::args("self", "data", "ddv_dx"),
89 "Update the Jacobian of the constrained system acceleration.\n\n"
90 ":param data: contact data\n"
91 ":param ddv_dx: Jacobian of the system acceleration in generalized "
92 "coordinates (dimension nv*ndx)")
93
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("updateForceDiff", &Model::updateForceDiff,
94 bp::args("self", "data", "df_dx", "df_du"),
95 "Update the Jacobians of the spatial force defined in frame "
96 "coordinates.\n\n"
97 ":param data: contact data\n"
98 ":param df_dx: Jacobian of the force with respect to the state "
99 "(dimension nc*ndx)\n"
100 ":param df_du: Jacobian of the force with respect to the control "
101 "(dimension nc*nu)")
102
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("updateRneaDiff", &Model::updateRneaDiff,
103 bp::args("self", "data", "pinocchio"),
104 "Update the RNEA derivative dtau_dq by by adding the skew term "
105 "(necessary for contacts expressed in\n"
106 "LOCAL_WORLD_ALIGNED / WORLD).\n\n"
107 ":param data: contact data\n"
108 ":param pinocchio: Pinocchio data")
109
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("createData", &Model::createData,
110 ✗ bp::with_custodian_and_ward_postcall<0, 2>(),
111 bp::args("self", "data"),
112 "Create the total contact data.\n\n"
113 ":param data: Pinocchio data\n"
114 ":return total contact data.")
115
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 40 .add_property(
116 "contacts",
117 bp::make_function(&Model::get_contacts,
118 40 bp::return_value_policy<bp::return_by_value>()),
119 "stack of contacts")
120
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
121 "state",
122 bp::make_function(&Model::get_state,
123 40 bp::return_value_policy<bp::return_by_value>()),
124 "state of the multibody system")
125
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .add_property("nc", bp::make_function(&Model::get_nc),
126 "dimension of the active contact vector")
127
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .add_property("nc_total", bp::make_function(&Model::get_nc_total),
128 "dimension of the total contact vector")
129
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .add_property("nu", bp::make_function(&Model::get_nu),
130 "dimension of control vector")
131
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
132 "active_set",
133 bp::make_function(&Model::get_active_set,
134 40 bp::return_value_policy<bp::return_by_value>()),
135 "names of the active set of contact items")
136
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
137 "inactive_set",
138 bp::make_function(&Model::get_inactive_set,
139 40 bp::return_value_policy<bp::return_by_value>()),
140 "names of the inactive set of contact items")
141
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 80 .def("getContactStatus", &Model::getContactStatus,
142 bp::args("self", "name"),
143 "Return the contact status of a given contact name.\n\n"
144 ":param name: contact name")
145
4/8
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
 40 .add_property(
146 "computeAllContacts",
147 bp::make_function(&Model::getComputeAllContacts),
148 bp::make_function(&Model::setComputeAllContacts),
149 "type of contact computation (True for all contacts and False for "
150 "active contacts)");
151 40 }
152 };
153
154 template <typename Data>
155 struct ContactDataMultipleVisitor
156 : public bp::def_visitor<ContactDataMultipleVisitor<Data>> {
157 template <class PyClass>
158 40 void visit(PyClass& cl) const {
159
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 40 cl.add_property(
160 40 "Jc", bp::make_getter(&Data::Jc, bp::return_internal_reference<>()),
161 bp::make_setter(&Data::Jc),
162 "contact Jacobian in frame coordinate (memory defined for "
163 "active and inactive contacts)")
164
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 80 .add_property(
165 40 "a0", bp::make_getter(&Data::a0, bp::return_internal_reference<>()),
166 bp::make_setter(&Data::a0),
167 "desired spatial contact acceleration in frame coordinate "
168 "(memory defined for active and inactive contacts)")
169
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 80 .add_property(
170 "da0_dx",
171 40 bp::make_getter(&Data::da0_dx, bp::return_internal_reference<>()),
172 bp::make_setter(&Data::da0_dx),
173 "Jacobian of the desired spatial contact acceleration in "
174 "frame coordinate (memory defined for active and "
175 "inactive contacts)")
176
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 80 .add_property(
177 40 "dv", bp::make_getter(&Data::dv, bp::return_internal_reference<>()),
178 bp::make_setter(&Data::dv),
179 "constrained system acceleration in generalized coordinates")
180
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 80 .add_property(
181 "ddv_dx",
182 40 bp::make_getter(&Data::ddv_dx, bp::return_internal_reference<>()),
183 bp::make_setter(&Data::ddv_dx),
184 "Jacobian of the constrained system acceleration in "
185 "generalized coordinates")
186
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
187 "contacts",
188 bp::make_getter(&Data::contacts,
189 40 bp::return_value_policy<bp::return_by_value>()),
190 "stack of contacts data")
191
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 .def_readwrite("fext", &Data::fext,
192 "external spatial forces in join coordinates");
193 40 }
194 };
195
196 #define CROCODDYL_CONTACT_ITEM_PYTHON_BINDINGS(Scalar) \
197 typedef ContactItemTpl<Scalar> Model; \
198 typedef Model::ContactModelAbstract ContactModel; \
199 typedef std::shared_ptr<Model> ContactItemPtr; \
200 StdMapPythonVisitor< \
201 std::string, ContactItemPtr, std::less<std::string>, \
202 std::allocator<std::pair<const std::string, ContactItemPtr>>, \
203 true>::expose("StdMap_ContactItem"); \
204 typedef ContactDataAbstractTpl<Scalar> ContactData; \
205 typedef std::shared_ptr<ContactData> ContactDataPtr; \
206 StdMapPythonVisitor< \
207 std::string, ContactDataPtr, std::less<std::string>, \
208 std::allocator<std::pair<const std::string, ContactDataPtr>>, \
209 true>::expose("StdMap_ContactData"); \
210 bp::register_ptr_to_python<std::shared_ptr<Model>>(); \
211 bp::class_<Model>( \
212 "ContactItem", "Describe a contact item.\n\n", \
213 bp::init<std::string, std::shared_ptr<ContactModel>, \
214 bp::optional<bool>>( \
215 bp::args("self", "name", "contact", "active"), \
216 "Initialize the contact item.\n\n" \
217 ":param name: contact name\n" \
218 ":param contact: contact model\n" \
219 ":param active: True if the contact is activated (default true)")) \
220 .def(ContactItemVisitor<Model>()) \
221 .def(CastVisitor<Model>()) \
222 .def(PrintableVisitor<Model>()) \
223 .def(CopyableVisitor<Model>());
224
225 #define CROCODDYL_CONTACT_MODEL_MULTIPLE_PYTHON_BINDINGS(Scalar) \
226 typedef ContactModelMultipleTpl<Scalar> Model; \
227 typedef Model::StateMultibody State; \
228 bp::register_ptr_to_python<std::shared_ptr<Model>>(); \
229 bp::class_<Model>( \
230 "ContactModelMultiple", \
231 bp::init<std::shared_ptr<State>, bp::optional<std::size_t>>( \
232 bp::args("self", "state", "nu"), \
233 "Initialize the multiple contact model.\n\n" \
234 ":param state: state of the multibody system\n" \
235 ":param nu: dimension of control vector")) \
236 .def(ContactModelMultipleVisitor<Model>()) \
237 .def(CastVisitor<Model>()) \
238 .def(PrintableVisitor<Model>()) \
239 .def(CopyableVisitor<Model>());
240
241 #define CROCODDYL_CONTACT_DATA_MULTIPLE_PYTHON_BINDINGS(Scalar) \
242 typedef ContactDataMultipleTpl<Scalar> Data; \
243 typedef ContactModelMultipleTpl<Scalar> Model; \
244 typedef pinocchio::DataTpl<Scalar> PinocchioData; \
245 bp::register_ptr_to_python<std::shared_ptr<Data>>(); \
246 bp::class_<Data>( \
247 "ContactDataMultiple", "Data class for multiple contacts.\n\n", \
248 bp::init<Model*, PinocchioData*>( \
249 bp::args("self", "model", "data"), \
250 "Create multicontact data.\n\n" \
251 ":param model: multicontact model\n" \
252 ":param data: Pinocchio data")[bp::with_custodian_and_ward< \
253 1, 2, bp::with_custodian_and_ward<1, 3>>()]) \
254 .def(ContactDataMultipleVisitor<Data>()) \
255 .def(CopyableVisitor<Data>());
256
257 10 void exposeContactMultiple() {
258
29/58
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✓ Branch 17 taken 10 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
✓ Branch 33 taken 10 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 10 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 10 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 10 times.
✗ Branch 43 not taken.
✓ Branch 45 taken 10 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 10 times.
✗ Branch 49 not taken.
✓ Branch 54 taken 10 times.
✗ Branch 55 not taken.
✓ Branch 57 taken 10 times.
✗ Branch 58 not taken.
✓ Branch 60 taken 10 times.
✗ Branch 61 not taken.
✓ Branch 66 taken 10 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 10 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 10 times.
✗ Branch 74 not taken.
✓ Branch 81 taken 10 times.
✗ Branch 82 not taken.
✓ Branch 85 taken 10 times.
✗ Branch 86 not taken.
✓ Branch 88 taken 10 times.
✗ Branch 89 not taken.
✓ Branch 95 taken 10 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 10 times.
✗ Branch 99 not taken.
✓ Branch 101 taken 10 times.
✗ Branch 102 not taken.
✓ Branch 104 taken 10 times.
✗ Branch 105 not taken.
✓ Branch 107 taken 10 times.
✗ Branch 108 not taken.
✓ Branch 110 taken 10 times.
✗ Branch 111 not taken.
✓ Branch 113 taken 10 times.
✗ Branch 114 not taken.
✓ Branch 116 taken 10 times.
✗ Branch 117 not taken.
 20 CROCODDYL_PYTHON_SCALARS(CROCODDYL_CONTACT_ITEM_PYTHON_BINDINGS)
259
17/34
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 10 times.
✗ Branch 31 not taken.
✓ Branch 35 taken 10 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 10 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 10 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 10 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 10 times.
✗ Branch 51 not taken.
✓ Branch 53 taken 10 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 10 times.
✗ Branch 57 not taken.
 20 CROCODDYL_PYTHON_SCALARS(CROCODDYL_CONTACT_MODEL_MULTIPLE_PYTHON_BINDINGS)
260
15/30
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10 times.
✗ Branch 28 not taken.
✓ Branch 32 taken 10 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 10 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 10 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 10 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 10 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 10 times.
✗ Branch 51 not taken.
 20 CROCODDYL_PYTHON_SCALARS(CROCODDYL_CONTACT_DATA_MULTIPLE_PYTHON_BINDINGS)
261 10 }
262
263 } // namespace python
264 } // namespace crocoddyl
265