GCC Code Coverage Report

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