| Directory: | ./ |
|---|---|
| File: | bindings/python/algorithm/expose-frames.cpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 72 | 84 | 85.7% |
| Branches: | 78 | 162 | 48.1% |
| 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/frames.hpp" | ||
| 7 | |||
| 8 | namespace pinocchio | ||
| 9 | { | ||
| 10 | namespace python | ||
| 11 | { | ||
| 12 | |||
| 13 | 1 | static context::Data::Matrix6x get_frame_jacobian_proxy1( | |
| 14 | const context::Model & model, | ||
| 15 | context::Data & data, | ||
| 16 | const context::Data::FrameIndex frame_id, | ||
| 17 | ReferenceFrame rf = LOCAL) | ||
| 18 | { | ||
| 19 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | context::Data::Matrix6x J(6, model.nv); |
| 20 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | J.setZero(); |
| 21 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | getFrameJacobian(model, data, frame_id, rf, J); |
| 22 | |||
| 23 | 1 | return J; | |
| 24 | } | ||
| 25 | |||
| 26 | ✗ | static context::Data::Matrix6x get_frame_jacobian_proxy2( | |
| 27 | const context::Model & model, | ||
| 28 | context::Data & data, | ||
| 29 | const context::Data::JointIndex joint_id, | ||
| 30 | const context::SE3 & placement, | ||
| 31 | ReferenceFrame rf = LOCAL) | ||
| 32 | { | ||
| 33 | ✗ | context::Data::Matrix6x J(6, model.nv); | |
| 34 | ✗ | J.setZero(); | |
| 35 | ✗ | getFrameJacobian(model, data, joint_id, placement, rf, J); | |
| 36 | ✗ | return J; | |
| 37 | } | ||
| 38 | |||
| 39 | 4 | static context::Data::Motion get_frame_velocity_proxy1( | |
| 40 | const context::Model & model, | ||
| 41 | context::Data & data, | ||
| 42 | const context::Data::FrameIndex frame_id, | ||
| 43 | ReferenceFrame rf = LOCAL) | ||
| 44 | { | ||
| 45 | 4 | return getFrameVelocity(model, data, frame_id, rf); | |
| 46 | } | ||
| 47 | |||
| 48 | ✗ | static context::Data::Motion get_frame_velocity_proxy2( | |
| 49 | const context::Model & model, | ||
| 50 | context::Data & data, | ||
| 51 | const context::Data::JointIndex joint_id, | ||
| 52 | const context::SE3 & placement, | ||
| 53 | ReferenceFrame rf = LOCAL) | ||
| 54 | { | ||
| 55 | ✗ | return getFrameVelocity(model, data, joint_id, placement, rf); | |
| 56 | } | ||
| 57 | |||
| 58 | 4 | static context::Data::Motion get_frame_acceleration_proxy1( | |
| 59 | const context::Model & model, | ||
| 60 | context::Data & data, | ||
| 61 | const context::Data::FrameIndex frame_id, | ||
| 62 | ReferenceFrame rf = LOCAL) | ||
| 63 | { | ||
| 64 | 4 | return getFrameAcceleration(model, data, frame_id, rf); | |
| 65 | } | ||
| 66 | |||
| 67 | ✗ | static context::Data::Motion get_frame_acceleration_proxy2( | |
| 68 | const context::Model & model, | ||
| 69 | context::Data & data, | ||
| 70 | const context::Data::JointIndex joint_id, | ||
| 71 | const context::SE3 & placement, | ||
| 72 | ReferenceFrame rf = LOCAL) | ||
| 73 | { | ||
| 74 | ✗ | return getFrameAcceleration(model, data, joint_id, placement, rf); | |
| 75 | } | ||
| 76 | |||
| 77 | 4 | static context::Data::Motion get_frame_classical_acceleration_proxy1( | |
| 78 | const context::Model & model, | ||
| 79 | context::Data & data, | ||
| 80 | const context::Data::FrameIndex frame_id, | ||
| 81 | ReferenceFrame rf = LOCAL) | ||
| 82 | { | ||
| 83 | 4 | return getFrameClassicalAcceleration(model, data, frame_id, rf); | |
| 84 | } | ||
| 85 | |||
| 86 | ✗ | static context::Data::Motion get_frame_classical_acceleration_proxy2( | |
| 87 | const context::Model & model, | ||
| 88 | context::Data & data, | ||
| 89 | const context::Data::JointIndex joint_id, | ||
| 90 | const context::SE3 & placement, | ||
| 91 | ReferenceFrame rf = LOCAL) | ||
| 92 | { | ||
| 93 | ✗ | return getFrameClassicalAcceleration(model, data, joint_id, placement, rf); | |
| 94 | } | ||
| 95 | |||
| 96 | 1 | static context::Data::Matrix6x compute_frame_jacobian_proxy( | |
| 97 | const context::Model & model, | ||
| 98 | context::Data & data, | ||
| 99 | const context::VectorXs & q, | ||
| 100 | context::Data::FrameIndex frame_id) | ||
| 101 | { | ||
| 102 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | context::Data::Matrix6x J(6, model.nv); |
| 103 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | J.setZero(); |
| 104 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | computeFrameJacobian(model, data, q, frame_id, J); |
| 105 | |||
| 106 | 1 | return J; | |
| 107 | } | ||
| 108 | |||
| 109 | 5 | static context::Data::Matrix6x compute_frame_jacobian_proxy( | |
| 110 | const context::Model & model, | ||
| 111 | context::Data & data, | ||
| 112 | const context::VectorXs & q, | ||
| 113 | context::Data::FrameIndex frame_id, | ||
| 114 | ReferenceFrame reference_frame) | ||
| 115 | { | ||
| 116 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | context::Data::Matrix6x J(6, model.nv); |
| 117 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | J.setZero(); |
| 118 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | computeFrameJacobian(model, data, q, frame_id, reference_frame, J); |
| 119 | |||
| 120 | 5 | return J; | |
| 121 | } | ||
| 122 | |||
| 123 | 2 | static context::Data::Matrix6x get_frame_jacobian_time_variation_proxy( | |
| 124 | const context::Model & model, | ||
| 125 | context::Data & data, | ||
| 126 | context::Data::FrameIndex jointId, | ||
| 127 | ReferenceFrame rf) | ||
| 128 | { | ||
| 129 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | context::Data::Matrix6x dJ(6, model.nv); |
| 130 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | dJ.setZero(); |
| 131 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobianTimeVariation(model, data, jointId, rf, dJ); |
| 132 | |||
| 133 | 2 | return dJ; | |
| 134 | } | ||
| 135 | |||
| 136 | 1 | static context::Data::Matrix6x frame_jacobian_time_variation_proxy( | |
| 137 | const context::Model & model, | ||
| 138 | context::Data & data, | ||
| 139 | const context::VectorXs & q, | ||
| 140 | const context::VectorXs & v, | ||
| 141 | const context::Data::FrameIndex frame_id, | ||
| 142 | const ReferenceFrame rf) | ||
| 143 | { | ||
| 144 | 1 | computeJointJacobiansTimeVariation(model, data, q, v); | |
| 145 | 1 | updateFramePlacements(model, data); | |
| 146 | |||
| 147 | 1 | return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf); | |
| 148 | } | ||
| 149 | |||
| 150 | 69 | void exposeFramesAlgo() | |
| 151 | { | ||
| 152 | typedef context::Scalar Scalar; | ||
| 153 | typedef context::VectorXs VectorXs; | ||
| 154 | enum | ||
| 155 | { | ||
| 156 | Options = context::Options | ||
| 157 | }; | ||
| 158 | |||
| 159 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 160 | "updateFramePlacements", &updateFramePlacements<Scalar, Options, JointCollectionDefaultTpl>, | ||
| 161 | 138 | bp::args("model", "data"), | |
| 162 | "Computes the placements of all the operational frames according to the current " | ||
| 163 | "joint placement stored in data" | ||
| 164 | "and puts the results in data."); | ||
| 165 | |||
| 166 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 167 | "updateFramePlacement", &updateFramePlacement<Scalar, Options, JointCollectionDefaultTpl>, | ||
| 168 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
138 | bp::args("model", "data", "frame_id"), |
| 169 | "Computes the placement of the given operational frame (frame_id) according to the " | ||
| 170 | "current joint placement stored in data, stores the results in data and returns it.", | ||
| 171 | ✗ | bp::return_value_policy<bp::return_by_value>()); | |
| 172 | |||
| 173 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 174 | "getFrameVelocity", &get_frame_velocity_proxy1, | ||
| 175 |
4/8✓ 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.
|
207 | (bp::arg("model"), bp::arg("data"), bp::arg("frame_id"), |
| 176 |
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("reference_frame") = LOCAL), |
| 177 | "Returns the spatial velocity of the frame expressed in the coordinate system given " | ||
| 178 | "by reference_frame.\n" | ||
| 179 | "forwardKinematics(model,data,q,v[,a]) should be called first to compute the joint " | ||
| 180 | "spatial velocity stored in data.v"); | ||
| 181 | |||
| 182 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 183 | "getFrameVelocity", &get_frame_velocity_proxy2, | ||
| 184 |
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("joint_id"), bp::arg("placement"), |
| 185 |
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("reference_frame") = LOCAL), |
| 186 | "Returns the spatial velocity of the frame expressed in the coordinate system given " | ||
| 187 | "by reference_frame.\n" | ||
| 188 | "forwardKinematics(model,data,q,v[,a]) should be called first to compute the joint " | ||
| 189 | "spatial velocity stored in data.v"); | ||
| 190 | |||
| 191 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 192 | "getFrameAcceleration", &get_frame_acceleration_proxy1, | ||
| 193 |
4/8✓ 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.
|
207 | (bp::arg("model"), bp::arg("data"), bp::arg("frame_id"), |
| 194 |
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("reference_frame") = LOCAL), |
| 195 | "Returns the spatial acceleration of the frame expressed in the coordinate system " | ||
| 196 | "given by reference_frame.\n" | ||
| 197 | "forwardKinematics(model,data,q,v,a) should be called first to compute the joint " | ||
| 198 | "spatial acceleration stored in data.a ."); | ||
| 199 | |||
| 200 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 201 | "getFrameAcceleration", &get_frame_acceleration_proxy2, | ||
| 202 |
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("joint_id"), bp::arg("placement"), |
| 203 |
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("reference_frame") = LOCAL), |
| 204 | "Returns the spatial acceleration of the frame expressed in the coordinate system " | ||
| 205 | "given by reference_frame.\n" | ||
| 206 | "forwardKinematics(model,data,q,v,a) should be called first to compute the joint " | ||
| 207 | "spatial acceleration stored in data.a ."); | ||
| 208 | |||
| 209 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 210 | "getFrameClassicalAcceleration", &get_frame_classical_acceleration_proxy1, | ||
| 211 |
4/8✓ 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.
|
207 | (bp::arg("model"), bp::arg("data"), bp::arg("frame_id"), |
| 212 |
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("reference_frame") = LOCAL), |
| 213 | "Returns the \"classical\" acceleration of the frame expressed in the coordinate " | ||
| 214 | "system given by reference_frame.\n" | ||
| 215 | "forwardKinematics(model,data,q,v,a) should be called first to compute the joint " | ||
| 216 | "spatial acceleration stored in data.a ."); | ||
| 217 | |||
| 218 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 219 | "getFrameClassicalAcceleration", &get_frame_classical_acceleration_proxy2, | ||
| 220 |
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("joint_id"), bp::arg("placement"), |
| 221 |
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("reference_frame") = LOCAL), |
| 222 | "Returns the \"classical\" acceleration of the frame expressed in the coordinate " | ||
| 223 | "system given by reference_frame.\n" | ||
| 224 | "forwardKinematics(model,data,q,v,a) should be called first to compute the joint " | ||
| 225 | "spatial acceleration stored in data.a ."); | ||
| 226 | |||
| 227 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 228 | "framesForwardKinematics", | ||
| 229 | &framesForwardKinematics<Scalar, Options, JointCollectionDefaultTpl, VectorXs>, | ||
| 230 | 138 | bp::args("model", "data", "q"), | |
| 231 | "Calls first the forwardKinematics(model,data,q) and then update the Frame placement " | ||
| 232 | "quantities (data.oMf)."); | ||
| 233 | |||
| 234 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 235 | "computeFrameJacobian", | ||
| 236 | (context::Data::Matrix6x(*)( | ||
| 237 | const context::Model &, context::Data &, const context::VectorXs &, | ||
| 238 | context::Data::FrameIndex, ReferenceFrame))&compute_frame_jacobian_proxy, | ||
| 239 | 138 | bp::args("model", "data", "q", "frame_id", "reference_frame"), | |
| 240 | "Computes the Jacobian of the frame given by its frame_id in the coordinate system " | ||
| 241 | "given by reference_frame.\n"); | ||
| 242 | |||
| 243 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 244 | "computeFrameJacobian", | ||
| 245 | (context::Data::Matrix6x(*)( | ||
| 246 | const context::Model &, context::Data &, const context::VectorXs &, | ||
| 247 | context::Data::FrameIndex))&compute_frame_jacobian_proxy, | ||
| 248 | 138 | bp::args("model", "data", "q", "frame_id"), | |
| 249 | "Computes the Jacobian of the frame given by its frame_id.\n" | ||
| 250 | "The columns of the Jacobian are expressed in the coordinates system of the Frame itself.\n" | ||
| 251 | "In other words, the velocity of the frame vF expressed in the local coordinate is given " | ||
| 252 | "by J*v," | ||
| 253 | "where v is the joint velocity."); | ||
| 254 | |||
| 255 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 256 | "getFrameJacobian", &get_frame_jacobian_proxy1, | ||
| 257 | 138 | bp::args("model", "data", "frame_id", "reference_frame"), | |
| 258 | "Computes the Jacobian of the frame given by its ID either in the LOCAL, " | ||
| 259 | "LOCAL_WORLD_ALIGNED or the WORLD coordinates systems.\n" | ||
| 260 | "In other words, the velocity of the frame vF expressed in the reference frame is " | ||
| 261 | "given by J*v," | ||
| 262 | "where v is the joint velocity vector.\n" | ||
| 263 | "remarks: computeJointJacobians(model,data,q) must have been called first."); | ||
| 264 | |||
| 265 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 266 | "getFrameJacobian", &get_frame_jacobian_proxy2, | ||
| 267 | 138 | bp::args("model", "data", "joint_id", "placement", "reference_frame"), | |
| 268 | "Computes the Jacobian of the frame given by its placement with respect to the Joint " | ||
| 269 | "frame and expressed the solution either in the LOCAL, LOCAL_WORLD_ALIGNED or the " | ||
| 270 | "WORLD coordinates systems.\n" | ||
| 271 | "In other words, the velocity of the frame vF expressed in the reference frame is " | ||
| 272 | "given by J*v," | ||
| 273 | "where v is the joint velocity vector.\n\n" | ||
| 274 | "remarks: computeJointJacobians(model,data,q) must have been called first."); | ||
| 275 | |||
| 276 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 277 | "frameJacobianTimeVariation", &frame_jacobian_time_variation_proxy, | ||
| 278 | 138 | bp::args("model", "data", "q", "v", "frame_id", "reference_frame"), | |
| 279 | "Computes the Jacobian Time Variation of the frame given by its frame_id either in " | ||
| 280 | "the reference frame provided by reference_frame.\n"); | ||
| 281 | |||
| 282 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 283 | "getFrameJacobianTimeVariation", get_frame_jacobian_time_variation_proxy, | ||
| 284 | 138 | bp::args("model", "data", "frame_id", "reference_frame"), | |
| 285 | "Returns the Jacobian time variation of the frame given by its frame_id either in " | ||
| 286 | "the reference frame provided by reference_frame.\n" | ||
| 287 | "You have to call computeJointJacobiansTimeVariation(model,data,q,v) and " | ||
| 288 | "updateFramePlacements(model,data) first."); | ||
| 289 | |||
| 290 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 291 | "computeSupportedInertiaByFrame", | ||
| 292 | &computeSupportedInertiaByFrame<double, 0, JointCollectionDefaultTpl>, | ||
| 293 | 138 | bp::args("model", "data", "frame_id", "with_subtree"), | |
| 294 | "Computes the supported inertia by the frame (given by frame_id) and returns it.\n" | ||
| 295 | "The supported inertia corresponds to the sum of the inertias of all the child frames " | ||
| 296 | "(that belongs to the same joint body) and the child joints, if with_subtree=True.\n" | ||
| 297 | "You must first call pinocchio::forwardKinematics to update placement values in data " | ||
| 298 | "structure."); | ||
| 299 | |||
| 300 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | bp::def( |
| 301 | "computeSupportedForceByFrame", | ||
| 302 | &computeSupportedForceByFrame<double, 0, JointCollectionDefaultTpl>, | ||
| 303 | 138 | bp::args("model", "data", "frame_id"), | |
| 304 | "Computes the supported force of the frame (given by frame_id) and returns it.\n" | ||
| 305 | "The supported force corresponds to the sum of all the forces experienced after the " | ||
| 306 | "given frame.\n" | ||
| 307 | "You must first call pinocchio::rnea to update placement values in data structure."); | ||
| 308 | 69 | } | |
| 309 | } // namespace python | ||
| 310 | |||
| 311 | } // namespace pinocchio | ||
| 312 |