Directory: | ./ |
---|---|
File: | bindings/python/algorithm/expose-joints.cpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 36 | 65 | 55.4% |
Branches: | 21 | 84 | 25.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2021 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #include "pinocchio/bindings/python/algorithm/algorithms.hpp" | ||
6 | #include "pinocchio/algorithm/joint-configuration.hpp" | ||
7 | |||
8 | namespace pinocchio | ||
9 | { | ||
10 | namespace python | ||
11 | { | ||
12 | |||
13 | static context::VectorXs | ||
14 | ✗ | normalize_proxy(const context::Model & model, const context::VectorXs & config) | |
15 | { | ||
16 | ✗ | context::VectorXs q(config); | |
17 | ✗ | normalize(model, q); | |
18 | ✗ | return q; | |
19 | } | ||
20 | |||
21 | 5 | static context::VectorXs randomConfiguration_proxy(const context::Model & model) | |
22 | { | ||
23 | 5 | return randomConfiguration(model); | |
24 | } | ||
25 | |||
26 | ✗ | bp::tuple dIntegrate_proxy( | |
27 | const context::Model & model, const context::VectorXs & q, const context::VectorXs & v) | ||
28 | { | ||
29 | ✗ | context::MatrixXs J0(context::MatrixXs::Zero(model.nv, model.nv)); | |
30 | ✗ | context::MatrixXs J1(context::MatrixXs::Zero(model.nv, model.nv)); | |
31 | |||
32 | ✗ | dIntegrate(model, q, v, J0, ARG0); | |
33 | ✗ | dIntegrate(model, q, v, J1, ARG1); | |
34 | |||
35 | ✗ | return bp::make_tuple(J0, J1); | |
36 | } | ||
37 | |||
38 | ✗ | context::MatrixXs dIntegrate_arg_proxy( | |
39 | const context::Model & model, | ||
40 | const context::VectorXs & q, | ||
41 | const context::VectorXs & v, | ||
42 | const ArgumentPosition arg) | ||
43 | { | ||
44 | ✗ | context::MatrixXs J(context::MatrixXs::Zero(model.nv, model.nv)); | |
45 | |||
46 | ✗ | dIntegrate(model, q, v, J, arg, SETTO); | |
47 | |||
48 | ✗ | return J; | |
49 | } | ||
50 | |||
51 | ✗ | context::MatrixXs dIntegrateTransport_proxy( | |
52 | const context::Model & model, | ||
53 | const context::VectorXs & q, | ||
54 | const context::VectorXs & v, | ||
55 | const context::MatrixXs & Jin, | ||
56 | const ArgumentPosition arg) | ||
57 | { | ||
58 | ✗ | int ncols = Jin.cols(); | |
59 | ✗ | context::MatrixXs Jout(context::MatrixXs::Zero(model.nv, ncols)); | |
60 | ✗ | dIntegrateTransport(model, q, v, Jin, Jout, arg); | |
61 | ✗ | return Jout; | |
62 | } | ||
63 | |||
64 | ✗ | bp::tuple dDifference_proxy( | |
65 | const context::Model & model, const context::VectorXs & q1, const context::VectorXs & q2) | ||
66 | { | ||
67 | ✗ | context::MatrixXs J0(context::MatrixXs::Zero(model.nv, model.nv)); | |
68 | ✗ | context::MatrixXs J1(context::MatrixXs::Zero(model.nv, model.nv)); | |
69 | |||
70 | ✗ | dDifference(model, q1, q2, J0, ARG0); | |
71 | ✗ | dDifference(model, q1, q2, J1, ARG1); | |
72 | |||
73 | ✗ | return bp::make_tuple(J0, J1); | |
74 | } | ||
75 | |||
76 | ✗ | context::MatrixXs dDifference_arg_proxy( | |
77 | const context::Model & model, | ||
78 | const context::VectorXs & q1, | ||
79 | const context::VectorXs & q2, | ||
80 | const ArgumentPosition arg) | ||
81 | { | ||
82 | ✗ | context::MatrixXs J(context::MatrixXs::Zero(model.nv, model.nv)); | |
83 | |||
84 | ✗ | dDifference(model, q1, q2, J, arg); | |
85 | |||
86 | ✗ | return J; | |
87 | } | ||
88 | |||
89 | 20 | void exposeJointsAlgo() | |
90 | { | ||
91 | typedef context::Scalar Scalar; | ||
92 | typedef context::VectorXs VectorXs; | ||
93 | enum | ||
94 | { | ||
95 | Options = context::Options | ||
96 | }; | ||
97 | |||
98 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
99 | "integrate", &integrate<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
100 | 40 | bp::args("model", "q", "v"), | |
101 | "Integrate the joint configuration vector q with a tangent vector v during one unit time.\n" | ||
102 | "This is the canonical integrator of a Configuration Space composed of Lie groups, such as " | ||
103 | "most robots.\n\n" | ||
104 | "Parameters:\n" | ||
105 | "\tmodel: model of the kinematic tree\n" | ||
106 | "\tq: the joint configuration vector (size model.nq)\n" | ||
107 | "\tv: the joint velocity vector (size model.nv)\n"); | ||
108 | |||
109 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
110 | 40 | "dIntegrate", &dIntegrate_proxy, bp::args("model", "q", "v"), | |
111 | "Computes the partial derivatives of the integrate function with respect to the first " | ||
112 | "and the second argument, and returns the two Jacobians as a tuple.\n\n" | ||
113 | "Parameters:\n" | ||
114 | "\tmodel: model of the kinematic tree\n" | ||
115 | "\tq: the joint configuration vector (size model.nq)\n" | ||
116 | "\tv: the joint velocity vector (size model.nv)\n"); | ||
117 | |||
118 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
119 | 40 | "dIntegrate", &dIntegrate_arg_proxy, bp::args("model", "q", "v", "argument_position"), | |
120 | "Computes the partial derivatives of the integrate function with respect to the " | ||
121 | "first (arg == ARG0) " | ||
122 | "or the second argument (arg == ARG1).\n\n" | ||
123 | "Parameters:\n" | ||
124 | "\tmodel: model of the kinematic tree\n" | ||
125 | "\tq: the joint configuration vector (size model.nq)\n" | ||
126 | "\tv: the joint velocity vector (size model.nv)\n" | ||
127 | "\targument_position: either pinocchio.ArgumentPosition.ARG0 or " | ||
128 | "pinocchio.ArgumentPosition.ARG1, depending on the desired Jacobian value.\n"); | ||
129 | |||
130 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
131 | "dIntegrateTransport", &dIntegrateTransport_proxy, | ||
132 | 40 | bp::args("model", "q", "v", "Jin", "argument_position"), | |
133 | "Takes a matrix expressed at q (+) v and uses parallel transport to express it in " | ||
134 | "the tangent space at q." | ||
135 | "\tThis operation does the product of the matrix by the Jacobian of the integration " | ||
136 | "operation, but more efficiently." | ||
137 | "Parameters:\n" | ||
138 | "\tmodel: model of the kinematic tree\n" | ||
139 | "\tq: the joint configuration vector (size model.nq)\n" | ||
140 | "\tv: the joint velocity vector (size model.nv)\n" | ||
141 | "\tJin: the input matrix (row size model.nv)" | ||
142 | "\targument_position: either pinocchio.ArgumentPosition.ARG0 (q) or " | ||
143 | "pinocchio.ArgumentPosition.ARG1 (v), depending on the desired Jacobian value.\n"); | ||
144 | |||
145 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
146 | "interpolate", &interpolate<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
147 | 40 | bp::args("model", "q1", "q2", "alpha"), | |
148 | "Interpolate between two given joint configuration vectors q1 and q2.\n\n" | ||
149 | "Parameters:\n" | ||
150 | "\tmodel: model of the kinematic tree\n" | ||
151 | "\tq1: the initial joint configuration vector (size model.nq)\n" | ||
152 | "\tq2: the terminal joint configuration vector (size model.nq)\n" | ||
153 | "\talpha: the interpolation coefficient in [0,1]\n"); | ||
154 | |||
155 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
156 | "difference", &difference<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
157 | 40 | bp::args("model", "q1", "q2"), | |
158 | "Difference between two joint configuration vectors, i.e. the tangent vector that " | ||
159 | "must be integrated during one unit time" | ||
160 | "to go from q1 to q2.\n\n" | ||
161 | "Parameters:\n" | ||
162 | "\tmodel: model of the kinematic tree\n" | ||
163 | "\tq1: the initial joint configuration vector (size model.nq)\n" | ||
164 | "\tq2: the terminal joint configuration vector (size model.nq)\n"); | ||
165 | |||
166 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
167 | "squaredDistance", | ||
168 | &squaredDistance<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
169 | 40 | bp::args("model", "q1", "q2"), | |
170 | "Squared distance vector between two joint configuration vectors.\n\n" | ||
171 | "Parameters:\n" | ||
172 | "\tmodel: model of the kinematic tree\n" | ||
173 | "\tq1: the initial joint configuration vector (size model.nq)\n" | ||
174 | "\tq2: the terminal joint configuration vector (size model.nq)\n"); | ||
175 | |||
176 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
177 | "distance", &distance<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
178 | 40 | bp::args("model", "q1", "q2"), | |
179 | "Distance between two joint configuration vectors.\n\n" | ||
180 | "Parameters:\n" | ||
181 | "\tmodel: model of the kinematic tree\n" | ||
182 | "\tq1: the initial joint configuration vector (size model.nq)\n" | ||
183 | "\tq2: the terminal joint configuration vector (size model.nq)\n"); | ||
184 | |||
185 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
186 | 40 | "dDifference", &dDifference_proxy, bp::args("model", "q1", "q2"), | |
187 | "Computes the partial derivatives of the difference function with respect to the first " | ||
188 | "and the second argument, and returns the two Jacobians as a tuple.\n\n" | ||
189 | "Parameters:\n" | ||
190 | "\tmodel: model of the kinematic tree\n" | ||
191 | "\tq1: the initial joint configuration vector (size model.nq)\n" | ||
192 | "\tq2: the terminal joint configuration vector (size model.nq)\n"); | ||
193 | |||
194 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
195 | 40 | "dDifference", &dDifference_arg_proxy, bp::args("model", "q1", "q2", "argument_position"), | |
196 | "Computes the partial derivatives of the difference function with respect to the " | ||
197 | "first (arg == ARG0) " | ||
198 | "or the second argument (arg == ARG1).\n\n" | ||
199 | "Parameters:\n" | ||
200 | "\tmodel: model of the kinematic tree\n" | ||
201 | "\tq1: the initial joint configuration vector (size model.nq)\n" | ||
202 | "\tq2: the terminal joint configuration vector (size model.nq)\n" | ||
203 | "\targument_position: either pinocchio.ArgumentPosition.ARG0 or " | ||
204 | "pinocchio.ArgumentPosition.ARG1, depending on the desired Jacobian value.\n"); | ||
205 | |||
206 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
207 | 40 | "randomConfiguration", &randomConfiguration_proxy, bp::arg("model"), | |
208 | "Generate a random configuration in the bounds given by the lower and upper limits " | ||
209 | "contained in model.\n\n" | ||
210 | "Parameters:\n" | ||
211 | "\tmodel: model of the kinematic tree\n"); | ||
212 | |||
213 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
214 | "randomConfiguration", | ||
215 | &randomConfiguration<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
216 | 40 | bp::args("model", "lower_bound", "upper_bound"), | |
217 | "Generate a random configuration in the bounds given by the Joint lower and upper limits " | ||
218 | "arguments.\n\n" | ||
219 | "Parameters:\n" | ||
220 | "\tmodel: model of the kinematic tree\n" | ||
221 | "\tlower_bound: the lower bound on the joint configuration vectors (size model.nq)\n" | ||
222 | "\tupper_bound: the upper bound on the joint configuration vectors (size model.nq)\n"); | ||
223 | |||
224 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
225 | 40 | "neutral", &neutral<Scalar, Options, JointCollectionDefaultTpl>, bp::arg("model"), | |
226 | "Returns the neutral configuration vector associated to the model.\n\n" | ||
227 | "Parameters:\n" | ||
228 | "\tmodel: model of the kinematic tree\n"); | ||
229 | |||
230 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
231 | 40 | "normalize", normalize_proxy, bp::args("model", "q"), | |
232 | "Returns the configuration normalized.\n" | ||
233 | "For instance, when the configuration vectors contains some quaternion values, it must be " | ||
234 | "required to renormalize these components to keep orthonormal rotation values.\n\n" | ||
235 | "Parameters:\n" | ||
236 | "\tmodel: model of the kinematic tree\n" | ||
237 | "\tq: a joint configuration vector to normalize (size model.nq)\n"); | ||
238 | |||
239 | #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS | ||
240 | |||
241 | static const Scalar dummy_precision = Eigen::NumTraits<Scalar>::dummy_precision(); | ||
242 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
243 | "isSameConfiguration", | ||
244 | &isSameConfiguration<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
245 | 40 | bp::args("model", "q1", "q2", "prec"), | |
246 | "Return true if two configurations are equivalent within the given precision " | ||
247 | "provided by prec.\n\n" | ||
248 | "Parameters:\n" | ||
249 | "\tmodel: model of the kinematic tree\n" | ||
250 | "\tq1: a joint configuration vector (size model.nq)\n" | ||
251 | "\tq2: a joint configuration vector (size model.nq)\n" | ||
252 | "\tprec: requested accuracy for the comparison\n"); | ||
253 | |||
254 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
255 | "isNormalized", &isNormalized<Scalar, Options, JointCollectionDefaultTpl, VectorXs>, | ||
256 |
5/10✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 20 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 20 times.
✗ Branch 15 not taken.
|
40 | (bp::arg("model"), bp::arg("q"), bp::arg("prec") = dummy_precision), |
257 | "Check whether a configuration vector is normalized within the given precision " | ||
258 | "provided by prec.\n\n" | ||
259 | "Parameters:\n" | ||
260 | "\tmodel: model of the kinematic tree\n" | ||
261 | "\tq: a joint configuration vector (size model.nq)\n" | ||
262 | "\tprec: requested accuracy for the check\n"); | ||
263 | #endif | ||
264 | 20 | } | |
265 | |||
266 | } // namespace python | ||
267 | } // namespace pinocchio | ||
268 |