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 |