| Directory: | ./ |
|---|---|
| File: | bindings/python/algorithm/expose-joints.cpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 67 | 67 | 100.0% |
| Branches: | 42 | 84 | 50.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 | 3 | normalize_proxy(const context::Model & model, const context::VectorXs & config) | |
| 15 | { | ||
| 16 | 3 | context::VectorXs q(config); | |
| 17 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | normalize(model, q); |
| 18 | 3 | return q; | |
| 19 | } | ||
| 20 | |||
| 21 | 17 | static context::VectorXs randomConfiguration_proxy(const context::Model & model) | |
| 22 | { | ||
| 23 | 17 | return randomConfiguration(model); | |
| 24 | } | ||
| 25 | |||
| 26 | 3 | bp::tuple dIntegrate_proxy( | |
| 27 | const context::Model & model, const context::VectorXs & q, const context::VectorXs & v) | ||
| 28 | { | ||
| 29 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | context::MatrixXs J0(context::MatrixXs::Zero(model.nv, model.nv)); |
| 30 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | context::MatrixXs J1(context::MatrixXs::Zero(model.nv, model.nv)); |
| 31 | |||
| 32 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | dIntegrate(model, q, v, J0, ARG0); |
| 33 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | dIntegrate(model, q, v, J1, ARG1); |
| 34 | |||
| 35 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | return bp::make_tuple(J0, J1); |
| 36 | 3 | } | |
| 37 | |||
| 38 | 4 | 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 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | context::MatrixXs J(context::MatrixXs::Zero(model.nv, model.nv)); |
| 45 | |||
| 46 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | dIntegrate(model, q, v, J, arg, SETTO); |
| 47 | |||
| 48 | 4 | return J; | |
| 49 | } | ||
| 50 | |||
| 51 | 2 | 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 | 2 | int ncols = Jin.cols(); | |
| 59 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | context::MatrixXs Jout(context::MatrixXs::Zero(model.nv, ncols)); |
| 60 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | dIntegrateTransport(model, q, v, Jin, Jout, arg); |
| 61 | 2 | return Jout; | |
| 62 | } | ||
| 63 | |||
| 64 | 2 | bp::tuple dDifference_proxy( | |
| 65 | const context::Model & model, const context::VectorXs & q1, const context::VectorXs & q2) | ||
| 66 | { | ||
| 67 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | context::MatrixXs J0(context::MatrixXs::Zero(model.nv, model.nv)); |
| 68 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | context::MatrixXs J1(context::MatrixXs::Zero(model.nv, model.nv)); |
| 69 | |||
| 70 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | dDifference(model, q1, q2, J0, ARG0); |
| 71 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | dDifference(model, q1, q2, J1, ARG1); |
| 72 | |||
| 73 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | return bp::make_tuple(J0, J1); |
| 74 | 2 | } | |
| 75 | |||
| 76 | 4 | 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 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | context::MatrixXs J(context::MatrixXs::Zero(model.nv, model.nv)); |
| 83 | |||
| 84 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | dDifference(model, q1, q2, J, arg); |
| 85 | |||
| 86 | 4 | return J; | |
| 87 | } | ||
| 88 | |||
| 89 | 69 | 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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 99 | "integrate", &integrate<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
| 100 | 138 | 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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 110 | 138 | "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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 119 | 138 | "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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 131 | "dIntegrateTransport", &dIntegrateTransport_proxy, | ||
| 132 | 138 | 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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 146 | "interpolate", &interpolate<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
| 147 | 138 | 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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 156 | "difference", &difference<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
| 157 | 138 | 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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 167 | "squaredDistance", | ||
| 168 | &squaredDistance<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
| 169 | 138 | 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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 177 | "distance", &distance<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
| 178 | 138 | 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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 186 | 138 | "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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 195 | 138 | "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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 207 | 138 | "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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 214 | "randomConfiguration", | ||
| 215 | &randomConfiguration<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
| 216 | 138 | 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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 225 | 138 | "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 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 231 | 138 | "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 65 times.
✗ Branch 2 not taken.
|
65 | bp::def( |
| 243 | "isSameConfiguration", | ||
| 244 | &isSameConfiguration<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
| 245 | 130 | 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 65 times.
✗ Branch 2 not taken.
|
65 | bp::def( |
| 255 | "isNormalized", &isNormalized<Scalar, Options, JointCollectionDefaultTpl, VectorXs>, | ||
| 256 |
5/10✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 65 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 65 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 65 times.
✗ Branch 15 not taken.
|
130 | (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 | 69 | } | |
| 265 | |||
| 266 | } // namespace python | ||
| 267 | } // namespace pinocchio | ||
| 268 |