GCC Code Coverage Report

Directory: ./
File: bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 47 48 97.9%
Branches: 83 166 50.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2025, 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/multibody/contacts/contact-1d.hpp"
11
12 #include "python/crocoddyl/multibody/multibody.hpp"
13
14 namespace crocoddyl {
15 namespace python {
16
17 template <typename Model>
18 struct ContactModel1DVisitor
19 : public bp::def_visitor<ContactModel1DVisitor<Model>> {
20 typedef typename Model::StateMultibody State;
21 typedef typename Model::Vector2s Vector2s;
22 template <class PyClass>
23 40 void visit(PyClass& cl) const {
24
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 40 cl.def(bp::init<std::shared_ptr<State>, pinocchio::FrameIndex, double,
25 pinocchio::ReferenceFrame, bp::optional<Vector2s>>(
26 bp::args("self", "state", "id", "xref", "type", "gains"),
27 "Initialize the contact model.\n\n"
28 ":param state: state of the multibody system\n"
29 ":param id: reference frame id of the contact\n"
30 ":param xref: contact position used for the Baumgarte "
31 "stabilization\n"
32 ":param type: type of contact\n"
33 ":param gains: gains of the contact model (default "
34 "np.matrix([0.,0.]))"))
35
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 80 .def("calc", &Model::calc, bp::args("self", "data", "x"),
36 "Compute the 1D contact Jacobian and drift.\n\n"
37 "The rigid contact model throught acceleration-base holonomic "
38 "constraint of the contact frame placement.\n"
39 ":param data: contact data\n"
40 ":param x: state point (dim. state.nx)")
41
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"),
42 "Compute the derivatives of the 1D contact holonomic "
43 "constraint.\n\n"
44 "The rigid contact model throught acceleration-base holonomic "
45 "constraint of the contact frame placement. It assumes that calc "
46 "has been run first.\n"
47 ":param data: cost data\n"
48 ":param x: state point (dim. state.nx)")
49
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,
50 bp::args("self", "data", "force"),
51 "Convert the force into a stack of spatial forces.\n\n"
52 ":param data: cost data\n"
53 ":param force: force vector (dimension 1)")
54
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,
55 ✗ bp::with_custodian_and_ward_postcall<0, 2>(),
56 bp::args("self", "data"),
57 "Create the 1D contact data.\n\n"
58 "Each contact model has its own data that needs to be allocated. "
59 "This function returns the allocated data for a predefined cost.\n"
60 ":param data: Pinocchio data\n"
61 ":return contact data.")
62
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .add_property(
63 "reference",
64 bp::make_function(&Model::get_reference,
65 40 bp::return_value_policy<bp::return_by_value>()),
66 &Model::set_reference, "reference contact translation")
67
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .add_property("Raxis",
68 bp::make_function(&Model::get_axis_rotation,
69 40 bp::return_internal_reference<>()),
70 &Model::set_axis_rotation,
71 "rotation of the reference frame's z axis")
72
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(
73 "gains",
74 bp::make_function(&Model::get_gains,
75 40 bp::return_value_policy<bp::return_by_value>()),
76 "contact gains");
77 40 }
78 };
79
80 template <typename Data>
81 struct ContactData1DVisitor
82 : public bp::def_visitor<ContactData1DVisitor<Data>> {
83 template <class PyClass>
84 40 void visit(PyClass& cl) const {
85
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 cl.add_property(
86 "v",
87 bp::make_getter(&Data::v,
88 40 bp::return_value_policy<bp::return_by_value>()),
89 "spatial velocity of the contact body")
90
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(
91 "a0_local",
92 40 bp::make_getter(&Data::a0_local, bp::return_internal_reference<>()),
93 bp::make_setter(&Data::a0_local),
94 "desired local contact acceleration")
95
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(
96 "a0_skew",
97 40 bp::make_getter(&Data::a0_skew, bp::return_internal_reference<>()),
98 bp::make_setter(&Data::a0_skew),
99 "contact acceleration skew (local)")
100
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("a0_world_skew",
101 bp::make_getter(&Data::a0_world_skew,
102 40 bp::return_internal_reference<>()),
103 bp::make_setter(&Data::a0_world_skew),
104 "contact acceleration skew (world)")
105
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(
106 40 "dp", bp::make_getter(&Data::dp, bp::return_internal_reference<>()),
107 bp::make_setter(&Data::dp),
108 "Translation error computed for the Baumgarte regularization term")
109
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(
110 "dp_local",
111 40 bp::make_getter(&Data::dp_local, bp::return_internal_reference<>()),
112 bp::make_setter(&Data::dp_local),
113 "local translation error computed for the Baumgarte "
114 "regularization term")
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.
 80 .add_property(
116 "f_local",
117 40 bp::make_getter(&Data::f_local, bp::return_internal_reference<>()),
118 bp::make_setter(&Data::f_local),
119 "spatial contact force in local coordinates")
120
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("da0_local_dx",
121 bp::make_getter(&Data::da0_local_dx,
122 40 bp::return_internal_reference<>()),
123 bp::make_setter(&Data::da0_local_dx),
124 "Jacobian of the desired local contact acceleration")
125
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
126 "fJf",
127 40 bp::make_getter(&Data::fJf, bp::return_internal_reference<>()),
128 "local Jacobian of the contact frame")
129
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property("v_partial_dq",
130 bp::make_getter(&Data::v_partial_dq,
131 40 bp::return_internal_reference<>()),
132 "Jacobian of the spatial body velocity")
133
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property("a_partial_dq",
134 bp::make_getter(&Data::a_partial_dq,
135 40 bp::return_internal_reference<>()),
136 "Jacobian of the spatial body acceleration")
137
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property("a_partial_dv",
138 bp::make_getter(&Data::a_partial_dv,
139 40 bp::return_internal_reference<>()),
140 "Jacobian of the spatial body acceleration")
141
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property("a_partial_da",
142 bp::make_getter(&Data::a_partial_da,
143 40 bp::return_internal_reference<>()),
144 "Jacobian of the spatial body acceleration")
145
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
146 "oRf",
147 40 bp::make_getter(&Data::oRf, bp::return_internal_reference<>()),
148 "Rotation matrix of the contact body expressed in the world frame");
149 40 }
150 };
151
152 #define CROCODDYL_CONTACT_MODEL_1D_PYTHON_BINDINGS(Scalar) \
153 typedef ContactModel1DTpl<Scalar> Model; \
154 typedef ContactModelAbstractTpl<Scalar> ModelBase; \
155 typedef Model::StateMultibody State; \
156 typedef Model::Vector2s Vector2s; \
157 typedef Model::Matrix3s Matrix3s; \
158 bp::register_ptr_to_python<std::shared_ptr<Model>>(); \
159 bp::class_<Model, bp::bases<ModelBase>>( \
160 "ContactModel1D", \
161 "Rigid 1D contact model.\n\n" \
162 "It defines a rigid 1D contact model (point contact) based on " \
163 "acceleration-based holonomic constraints, in the z direction. The " \
164 "calc and calcDiff functions compute the contact Jacobian and drift " \
165 "(holonomic constraint) or the derivatives of the holonomic " \
166 "constraint, respectively.", \
167 bp::init<std::shared_ptr<State>, pinocchio::FrameIndex, Scalar, \
168 pinocchio::ReferenceFrame, Matrix3s, std::size_t, \
169 bp::optional<Vector2s>>( \
170 bp::args("self", "state", "id", "xref", "type", "rotation", "nu", \
171 "gains"), \
172 "Initialize the contact model.\n\n" \
173 ":param state: state of the multibody system\n" \
174 ":param id: reference frame id of the contact\n" \
175 ":param xref: contact position used for the Baumgarte " \
176 "stabilization\n" \
177 ":param type: type of contact\n" \
178 ":param rotation: rotation of the reference frame's z axis\n" \
179 ":param nu: dimension of control vector\n" \
180 ":param gains: gains of the contact model (default " \
181 "np.matrix([0.,0.]))")) \
182 .def(ContactModel1DVisitor<Model>()) \
183 .def(CastVisitor<Model>()) \
184 .def(PrintableVisitor<Model>()) \
185 .def(CopyableVisitor<Model>());
186
187 #define CROCODDYL_CONTACT_DATA_1D_PYTHON_BINDINGS(Scalar) \
188 typedef ContactData1DTpl<Scalar> Data; \
189 typedef ContactDataAbstractTpl<Scalar> DataBase; \
190 typedef ContactModel1DTpl<Scalar> Model; \
191 typedef pinocchio::DataTpl<Scalar> PinocchioData; \
192 bp::register_ptr_to_python<std::shared_ptr<Data>>(); \
193 bp::class_<Data, bp::bases<DataBase>>( \
194 "ContactData1D", "Data for 1D contact.\n\n", \
195 bp::init<Model*, PinocchioData*>( \
196 bp::args("self", "model", "data"), \
197 "Create 1D contact data.\n\n" \
198 ":param model: 1D contact model\n" \
199 ":param data: Pinocchio data")[bp::with_custodian_and_ward< \
200 1, 2, bp::with_custodian_and_ward<1, 3>>()]) \
201 .def(ContactData1DVisitor<Data>()) \
202 .def(CopyableVisitor<Data>());
203
204 10 void exposeContact1D() {
205
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_1D_PYTHON_BINDINGS)
206
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_1D_PYTHON_BINDINGS)
207 10 }
208
209 } // namespace python
210 } // namespace crocoddyl
211