GCC Code Coverage Report

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