GCC Code Coverage Report

Directory: ./
File: include/pinocchio/bindings/python/algorithm/contact-cholesky.hpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 70 74 94.6%
Branches: 89 180 49.4%
Line Branch Exec Source
1 //
2 // Copyright (c) 2020-2024 INRIA
3 //
4
5 #ifndef __pinocchio_python_algorithm_contact_cholesky_hpp__
6 #define __pinocchio_python_algorithm_contact_cholesky_hpp__
7
8 #include <eigenpy/memory.hpp>
9 #include "pinocchio/algorithm/contact-cholesky.hpp"
10
11 #include "pinocchio/algorithm/delassus-operator-dense.hpp"
12 #include "pinocchio/algorithm/delassus-operator-sparse.hpp"
13
14 #include "pinocchio/bindings/python/utils/macros.hpp"
15 #include "pinocchio/bindings/python/utils/std-vector.hpp"
16 #include "pinocchio/bindings/python/utils/comparable.hpp"
17 #include "pinocchio/bindings/python/utils/copyable.hpp"
18 #include "pinocchio/bindings/python/utils/model-checker.hpp"
19
20 #include "pinocchio/bindings/python/algorithm/delassus-operator.hpp"
21
22 namespace pinocchio
23 {
24 namespace python
25 {
26 namespace bp = boost::python;
27
28 template<typename ContactCholeskyDecomposition>
29 struct ContactCholeskyDecompositionPythonVisitor
30 : public boost::python::def_visitor<
31 ContactCholeskyDecompositionPythonVisitor<ContactCholeskyDecomposition>>
32 {
33 typedef ContactCholeskyDecomposition Self;
34 typedef typename ContactCholeskyDecomposition::Scalar Scalar;
35 typedef typename ContactCholeskyDecomposition::RigidConstraintModel RigidConstraintModel;
36 typedef typename ContactCholeskyDecomposition::RigidConstraintData RigidConstraintData;
37 typedef typename ContactCholeskyDecomposition::Matrix Matrix;
38 typedef typename ContactCholeskyDecomposition::Vector Vector;
39 typedef typename PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)
40 RigidConstraintModelVector;
41 typedef typename PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData)
42 RigidConstraintDataVector;
43
44 typedef pinocchio::python::context::Model Model;
45 typedef pinocchio::python::context::Data Data;
46
47 template<class PyClass>
48 72 void visit(PyClass & cl) const
49 {
50
2/4
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 72 times.
✗ Branch 6 not taken.
 72 cl.def(bp::init<>(bp::arg("self"), "Default constructor."))
51
4/8
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
 216 .def(bp::init<const Model &>(
52 bp::args("self", "model"),
53 72 "Constructor from a model.")[mimic_not_supported_function<>(1)])
54
4/8
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
 216 .def(bp::init<const Model &, const RigidConstraintModelVector &>(
55
3/6
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
 216 (bp::arg("self"), bp::arg("model"), bp::arg("contact_models")),
56 "Constructor from a model and a collection of RigidConstraintModels.")
57
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
 72 [mimic_not_supported_function<>(1)])
58
59
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
 144 .PINOCCHIO_ADD_PROPERTY_READONLY_BYVALUE(Self, U, "")
60
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
 144 .PINOCCHIO_ADD_PROPERTY_READONLY_BYVALUE(Self, D, "")
61
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
 144 .PINOCCHIO_ADD_PROPERTY_READONLY_BYVALUE(Self, Dinv, "")
62
63
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 144 .def("size", &Self::size, bp::arg("self"), "Size of the decomposition.")
64
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 .def(
65
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 144 "constraintDim", &Self::constraintDim, bp::arg("self"),
66 "Returns the total dimension of the constraints contained in the Cholesky "
67 "factorization.")
68
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 .def(
69
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 144 "numContacts", &Self::numContacts, bp::arg("self"),
70 "Returns the number of contacts associated to this decomposition.")
71
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
 144 .def(
72
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 144 "matrix", (Matrix(Self::*)(void) const)&Self::matrix, bp::arg("self"),
73 "Returns the matrix resulting from the decomposition.")
74
75
5/10
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 72 times.
✗ Branch 14 not taken.
 360 .def(
76 "compute",
77 (void (*)(
78 Self & self, const Model &, Data &, const RigidConstraintModelVector &,
79 RigidConstraintDataVector &, const Scalar))&compute,
80
4/8
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
 288 (bp::arg("self"), bp::arg("model"), bp::arg("data"), bp::arg("contact_models"),
81
4/8
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
 216 bp::arg("contact_datas"), bp::arg("mu") = 0),
82 "Computes the Cholesky decompostion of the augmented matrix containing the KKT matrix\n"
83 "related to the system mass matrix and the Jacobians of the contact patches contained "
84 "in\n"
85 "the vector of RigidConstraintModel named contact_models. The decomposition is "
86 "regularized with a factor mu.",
87 144 mimic_not_supported_function<>(1))
88
89
6/12
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 72 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 72 times.
✗ Branch 17 not taken.
 360 .def(
90 "compute",
91 (void (*)(
92 Self & self, const Model &, Data &, const RigidConstraintModelVector &,
93 RigidConstraintDataVector &, const Vector &))&compute,
94
4/8
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
 288 (bp::arg("self"), bp::arg("model"), bp::arg("data"), bp::arg("contact_models"),
95
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
 216 bp::arg("contact_datas"), bp::arg("mus")),
96 "Computes the Cholesky decompostion of the augmented matrix containing the KKT matrix\n"
97 "related to the system mass matrix and the Jacobians of the contact patches contained "
98 "in\n"
99 "the vector of RigidConstraintModel named contact_models. The decomposition is "
100 "regularized with a factor mu.",
101 144 mimic_not_supported_function<>(1))
102
103
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
 144 .def(
104 "updateDamping", (void(Self::*)(const Scalar &)) & Self::updateDamping,
105 bp::args("self", "mu"),
106 "Update the damping term on the upper left block part of the KKT matrix. The "
107 "damping term should be positive.")
108
109
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 144 .def(
110 "updateDamping", &Self::template updateDamping<Vector>, bp::args("self", "mus"),
111 "Update the damping terms on the upper left block part of the KKT matrix. The "
112 "damping terms should be all positives.")
113
114
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
 144 .def(
115 "getInverseOperationalSpaceInertiaMatrix",
116 (Matrix(Self::*)(void) const)&Self::getInverseOperationalSpaceInertiaMatrix,
117
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 bp::arg("self"),
118 "Returns the Inverse of the Operational Space Inertia Matrix resulting from the "
119 "decomposition.",
120 72 bp::return_value_policy<bp::return_by_value>())
121
122
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 .def(
123 "getOperationalSpaceInertiaMatrix",
124
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 (Matrix(Self::*)(void) const)&Self::getOperationalSpaceInertiaMatrix, bp::arg("self"),
125 "Returns the Operational Space Inertia Matrix resulting from the decomposition.",
126 72 bp::return_value_policy<bp::return_by_value>())
127
128
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 .def(
129 "getInverseMassMatrix", (Matrix(Self::*)(void) const)&Self::getInverseMassMatrix,
130
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 bp::arg("self"),
131 "Returns the inverse of the Joint Space Inertia Matrix or \"mass matrix\".",
132 72 bp::return_value_policy<bp::return_by_value>())
133
134
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
 72 .def(
135 "solve", &solve<Matrix>, bp::args("self", "matrix"),
136 "Computes the solution of \f$ A x = b \f$ where self corresponds to the Cholesky "
137 "decomposition of A.",
138 72 bp::return_value_policy<bp::return_by_value>())
139
140
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 .def(
141
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 144 "inverse", (Matrix(Self::*)(void) const)&Self::inverse, bp::arg("self"),
142 "Returns the inverse matrix resulting from the decomposition.")
143
144 144 .def(
145 "getMassMatrixChoeslkyDecomposition",
146 &Self::template getMassMatrixChoeslkyDecomposition<
147 Scalar, 0, JointCollectionDefaultTpl>,
148
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 144 bp::arg("self"),
149 "Retrieves the Cholesky decomposition of the Mass Matrix contained in the current "
150 "decomposition.")
151
152
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 .def(
153
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 "getDelassusCholeskyExpression", &Self::getDelassusCholeskyExpression, bp::arg("self"),
154 "Returns the Cholesky decomposition expression associated to the underlying "
155 "Delassus matrix.",
156 72 bp::with_custodian_and_ward_postcall<0, 1>())
157
158
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
 72 .def(ComparableVisitor<Self, pinocchio::is_floating_point<Scalar>::value>());
159 72 }
160
161 72 static void expose()
162 {
163 72 bp::class_<ContactCholeskyDecomposition>(
164 "ContactCholeskyDecomposition",
165 "Contact information container for contact dynamic algorithms.", bp::no_init)
166
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 .def(ContactCholeskyDecompositionPythonVisitor<ContactCholeskyDecomposition>())
167
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 .def(CopyableVisitor<ContactCholeskyDecomposition>());
168
169 {
170 typedef typename ContactCholeskyDecomposition::DelassusCholeskyExpression
171 DelassusCholeskyExpression;
172
173 72 bp::class_<DelassusCholeskyExpression>(
174 "DelassusCholeskyExpression",
175 "Delassus Cholesky expression associated to a "
176 "given ContactCholeskyDecomposition object.",
177 bp::no_init)
178
3/6
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
 216 .def(bp::init<const ContactCholeskyDecomposition &>(
179 bp::args("self", "cholesky_decomposition"),
180 "Build from a given ContactCholeskyDecomposition object.")
181
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 [bp::with_custodian_and_ward<1, 2>()])
182
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
 72 .def(
183 "cholesky",
184 ✗ +[](const DelassusCholeskyExpression & self) -> ContactCholeskyDecomposition & {
185 ✗ return const_cast<ContactCholeskyDecomposition &>(self.cholesky());
186 },
187
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 bp::arg("self"),
188 "Returns the Constraint Cholesky decomposition associated to this "
189 "DelassusCholeskyExpression.",
190 72 bp::return_internal_reference<>())
191
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 .def(DelassusOperatorBasePythonVisitor<DelassusCholeskyExpression>());
192 }
193
194 {
195 #ifndef PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
196 typedef DelassusOperatorDenseTpl<context::Scalar, context::Options> DelassusOperatorDense;
197 68 bp::class_<DelassusOperatorDense>(
198 "DelassusOperatorDense", "Delassus Cholesky dense operator from a dense matrix.",
199 bp::no_init)
200
3/6
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 68 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 68 times.
✗ Branch 8 not taken.
 136 .def(bp::init<const Eigen::Ref<const context::MatrixXs>>(
201 bp::args("self", "matrix"), "Build from a given dense matrix"))
202
203
1/2
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
 68 .def(DelassusOperatorBasePythonVisitor<DelassusOperatorDense>());
204 #endif // PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
205 }
206
207 {
208 #ifndef PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
209 typedef DelassusOperatorSparseTpl<context::Scalar, context::Options>
210 DelassusOperatorSparse;
211 68 bp::class_<DelassusOperatorSparse, boost::noncopyable>(
212 "DelassusOperatorSparse", "Delassus Cholesky sparse operator from a sparse matrix.",
213 bp::no_init)
214
3/6
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 68 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 68 times.
✗ Branch 8 not taken.
 136 .def(bp::init<const context::SparseMatrix &>(
215 bp::args("self", "matrix"), "Build from a given sparse matrix"))
216
217
1/2
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
 68 .def(DelassusOperatorBasePythonVisitor<DelassusOperatorSparse>());
218 #endif // PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
219 }
220 #ifdef PINOCCHIO_WITH_ACCELERATE_SUPPORT
221 {
222 typedef Eigen::AccelerateLLT<context::SparseMatrix> AccelerateCholeskyDecomposition;
223 typedef DelassusOperatorSparseTpl<
224 context::Scalar, context::Options, AccelerateCholeskyDecomposition>
225 DelassusOperatorSparseAccelerate;
226 bp::class_<DelassusOperatorSparseAccelerate, boost::noncopyable>(
227 "DelassusOperatorSparseAccelerate",
228 "Delassus Cholesky sparse operator leveraging the Accelerate framework on APPLE "
229 "systems.",
230 bp::no_init)
231 .def(bp::init<const context::SparseMatrix &>(
232 bp::args("self", "matrix"), "Build from a given sparse matrix"))
233
234 .def(DelassusOperatorBasePythonVisitor<DelassusOperatorSparseAccelerate>());
235 }
236 #endif
237 72 }
238
239 template<typename MatrixType>
240 7 static Matrix solve(const Self & self, const MatrixType & mat)
241 {
242 7 return self.solve(mat);
243 }
244
245 8 static void compute(
246 Self & self,
247 const Model & model,
248 Data & data,
249 const RigidConstraintModelVector & contact_models,
250 RigidConstraintDataVector & contact_datas,
251 const Scalar mu = static_cast<Scalar>(0))
252 {
253
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 8 self.compute(model, data, contact_models, contact_datas, mu);
254 8 }
255
256 ✗ static void compute(
257 Self & self,
258 const Model & model,
259 Data & data,
260 const RigidConstraintModelVector & contact_models,
261 RigidConstraintDataVector & contact_datas,
262 const Vector & mus)
263 {
264 ✗ self.compute(model, data, contact_models, contact_datas, mus);
265 }
266 };
267
268 } // namespace python
269 } // namespace pinocchio
270
271 #endif // ifndef __pinocchio_python_algorithm_contact_cholesky_hpp__
272