GCC Code Coverage Report

Directory: ./
File: bindings/python/algorithm/expose-contact-dynamics.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 36 36 100.0%
Branches: 71 162 43.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2020-2021 INRIA
3 //
4
5 #include "pinocchio/bindings/python/algorithm/algorithms.hpp"
6 #include "pinocchio/algorithm/contact-dynamics.hpp"
7
8 namespace pinocchio
9 {
10 namespace python
11 {
12
13 6 static const context::VectorXs forwardDynamics_proxy(
14 const context::Model & model,
15 context::Data & data,
16 const context::VectorXs & q,
17 const context::VectorXs & v,
18 const context::VectorXs & tau,
19 const context::MatrixXs & J,
20 const context::VectorXs & gamma,
21 const context::Scalar inv_damping = context::Scalar(0.0))
22 {
23
24 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
25 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
26
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 6 return forwardDynamics(model, data, q, v, tau, J, gamma, inv_damping);
27 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
28 }
29
30 4 static const context::VectorXs forwardDynamics_proxy_no_q(
31 const context::Model & model,
32 context::Data & data,
33 const context::VectorXs & tau,
34 const context::MatrixXs & J,
35 const context::VectorXs & gamma,
36 const context::Scalar inv_damping = context::Scalar(0.0))
37 {
38
39 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
40 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
41
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 4 return forwardDynamics(model, data, tau, J, gamma, inv_damping);
42 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
43 }
44
45 6 static const context::VectorXs impulseDynamics_proxy(
46 const context::Model & model,
47 context::Data & data,
48 const context::VectorXs & q,
49 const context::VectorXs & v_before,
50 const context::MatrixXs & J,
51 const context::Scalar r_coeff = context::Scalar(0.),
52 const context::Scalar inv_damping = context::Scalar(0.))
53 {
54
55 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
56 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
57
0/6
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 6 return impulseDynamics(model, data, q, v_before, J, r_coeff, inv_damping);
58 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
59 }
60
61 6 static const context::VectorXs impulseDynamics_proxy_no_q(
62 const context::Model & model,
63 context::Data & data,
64 const context::VectorXs & v_before,
65 const context::MatrixXs & J,
66 const context::Scalar r_coeff = context::Scalar(0.),
67 const context::Scalar inv_damping = context::Scalar(0.))
68 {
69
70 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
71 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
72
0/6
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 6 return impulseDynamics(model, data, v_before, J, r_coeff, inv_damping);
73 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
74 }
75
76 2 static context::MatrixXs computeKKTContactDynamicMatrixInverse_proxy(
77 const context::Model & model,
78 context::Data & data,
79 const context::VectorXs & q,
80 const context::MatrixXs & J,
81 const context::Scalar mu = context::Scalar(0))
82 {
83
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 context::MatrixXs KKTMatrix_inv(model.nv + J.rows(), model.nv + J.rows());
84
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 computeKKTContactDynamicMatrixInverse(model, data, q, J, KKTMatrix_inv, mu);
85 2 return KKTMatrix_inv;
86 }
87
88 2 static const context::MatrixXs getKKTContactDynamicMatrixInverse_proxy(
89 const context::Model & model, context::Data & data, const context::MatrixXs & J)
90 {
91
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 context::MatrixXs MJtJ_inv(model.nv + J.rows(), model.nv + J.rows());
92
93 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
94 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
95
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 getKKTContactDynamicMatrixInverse(model, data, J, MJtJ_inv);
96 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
97
98 2 return MJtJ_inv;
99 }
100
101 69 void exposeContactDynamics()
102 {
103 using namespace Eigen;
104
105
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 bp::def(
106 "forwardDynamics", &forwardDynamics_proxy,
107
9/18
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 69 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 69 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 69 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 69 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 69 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 69 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 69 times.
✗ Branch 27 not taken.
 207 (bp::arg("model"), bp::arg("data"), bp::arg("q"), bp::arg("v"), bp::arg("tau"),
108
6/12
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 69 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 69 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 69 times.
✗ Branch 17 not taken.
 207 bp::arg("constraint_jacobian"), bp::arg("constraint_drift"), bp::arg("damping") = 0),
109 "Solves the constrained dynamics problem with contacts, puts the result in "
110 "context::Data::ddq and return it. The contact forces are stored in data.lambda_c.\n"
111 "Note: internally, pinocchio.computeAllTerms is called.");
112
113
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 bp::def(
114 "forwardDynamics", &forwardDynamics_proxy_no_q,
115
7/14
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 69 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 69 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 69 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 69 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 69 times.
✗ Branch 21 not taken.
 207 (bp::arg("model"), bp::arg("data"), bp::arg("tau"), bp::arg("constraint_jacobian"),
116
4/8
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 69 times.
✗ Branch 11 not taken.
 207 bp::arg("constraint_drift"), bp::arg("damping") = 0),
117 "Solves the forward dynamics problem with contacts, puts the result in "
118 "context::Data::ddq and return it. The contact forces are stored in data.lambda_c.\n"
119 "Note: this function assumes that pinocchio.computeAllTerms has been called first.");
120
121
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 bp::def(
122 "impulseDynamics", &impulseDynamics_proxy,
123
7/14
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 69 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 69 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 69 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 69 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 69 times.
✗ Branch 21 not taken.
 207 (bp::arg("model"), bp::arg("data"), bp::arg("q"), bp::arg("v_before"),
124
4/8
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 69 times.
✗ Branch 11 not taken.
 276 bp::arg("constraint_jacobian"), bp::arg("restitution_coefficient") = 0,
125
3/6
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
 138 bp::arg("damping") = 0),
126 "Solves the impact dynamics problem with contacts, store the result in "
127 "context::Data::dq_after and return it. The contact impulses are stored in "
128 "data.impulse_c.\n"
129 "Note: internally, pinocchio.crba is called.");
130
131
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 bp::def(
132 "impulseDynamics", &impulseDynamics_proxy_no_q,
133
6/12
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 69 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 69 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 69 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 69 times.
✗ Branch 18 not taken.
 138 (bp::arg("model"), bp::arg("data"), bp::arg("v_before"), bp::arg("constraint_jacobian"),
134
6/12
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 69 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 69 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 69 times.
✗ Branch 17 not taken.
 138 bp::arg("restitution_coefficient") = 0, bp::arg("damping") = 0),
135 "Solves the impact dynamics problem with contacts, store the result in "
136 "context::Data::dq_after and return it. The contact impulses are stored in "
137 "data.impulse_c.\n"
138 "Note: this function assumes that pinocchio.crba has been called first.");
139
140
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 bp::def(
141 "computeKKTContactDynamicMatrixInverse", computeKKTContactDynamicMatrixInverse_proxy,
142
6/12
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 69 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 69 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 69 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 69 times.
✗ Branch 18 not taken.
 207 (bp::arg("model"), bp::arg("data"), bp::arg("q"), bp::arg("constraint_jacobian"),
143
3/6
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
 138 bp::arg("damping") = 0),
144 "Computes the inverse of the constraint matrix [[M J^T], [J 0]].");
145
146
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 bp::def(
147 "getKKTContactDynamicMatrixInverse", getKKTContactDynamicMatrixInverse_proxy,
148 138 bp::args("model", "data", "constraint_jacobian"),
149 "Computes the inverse of the constraint matrix [[M Jt], [J 0]].\n forwardDynamics or "
150 "impulseDynamics must have been called first.\n"
151 "Note: the constraint Jacobian should be the same that was provided to "
152 "forwardDynamics or impulseDynamics.");
153 69 }
154
155 } // namespace python
156 } // namespace pinocchio
157