| Directory: | ./ |
|---|---|
| File: | bindings/python/algorithm/expose-jacobian.cpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 19 | 32 | 59.4% |
| Branches: | 12 | 36 | 33.3% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #include "pinocchio/bindings/python/algorithm/algorithms.hpp" | ||
| 6 | #include "pinocchio/algorithm/jacobian.hpp" | ||
| 7 | |||
| 8 | namespace pinocchio | ||
| 9 | { | ||
| 10 | namespace python | ||
| 11 | { | ||
| 12 | |||
| 13 | 89 | static context::Data::Matrix6x compute_jacobian_proxy( | |
| 14 | const context::Model & model, | ||
| 15 | context::Data & data, | ||
| 16 | const context::VectorXs & q, | ||
| 17 | JointIndex jointId) | ||
| 18 | { | ||
| 19 |
1/2✓ Branch 1 taken 89 times.
✗ Branch 2 not taken.
|
89 | context::Data::Matrix6x J(6, model.nv); |
| 20 |
1/2✓ Branch 1 taken 89 times.
✗ Branch 2 not taken.
|
89 | J.setZero(); |
| 21 |
1/2✓ Branch 1 taken 89 times.
✗ Branch 2 not taken.
|
89 | computeJointJacobian(model, data, q, jointId, J); |
| 22 | |||
| 23 | 89 | return J; | |
| 24 | } | ||
| 25 | |||
| 26 | ✗ | static context::Data::Matrix6x get_jacobian_proxy( | |
| 27 | const context::Model & model, context::Data & data, JointIndex jointId, ReferenceFrame rf) | ||
| 28 | { | ||
| 29 | ✗ | context::Data::Matrix6x J(6, model.nv); | |
| 30 | ✗ | J.setZero(); | |
| 31 | ✗ | getJointJacobian(model, data, jointId, rf, J); | |
| 32 | |||
| 33 | ✗ | return J; | |
| 34 | } | ||
| 35 | |||
| 36 | ✗ | static context::Data::Matrix6x get_jacobian_time_variation_proxy( | |
| 37 | const context::Model & model, context::Data & data, JointIndex jointId, ReferenceFrame rf) | ||
| 38 | { | ||
| 39 | ✗ | context::Data::Matrix6x dJ(6, model.nv); | |
| 40 | ✗ | dJ.setZero(); | |
| 41 | ✗ | getJointJacobianTimeVariation(model, data, jointId, rf, dJ); | |
| 42 | |||
| 43 | ✗ | return dJ; | |
| 44 | } | ||
| 45 | |||
| 46 | 20 | void exposeJacobian() | |
| 47 | { | ||
| 48 | typedef context::Scalar Scalar; | ||
| 49 | typedef context::VectorXs VectorXs; | ||
| 50 | enum | ||
| 51 | { | ||
| 52 | Options = context::Options | ||
| 53 | }; | ||
| 54 | |||
| 55 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
| 56 | "computeJointJacobians", | ||
| 57 | &computeJointJacobians<Scalar, Options, JointCollectionDefaultTpl, VectorXs>, | ||
| 58 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | bp::args("model", "data", "q"), |
| 59 | "Computes the full model Jacobian, i.e. the stack of all the motion subspaces " | ||
| 60 | "expressed in the coordinate world frame.\n" | ||
| 61 | "The result is accessible through data.J. This function computes also the forward " | ||
| 62 | "kinematics of the model.\n\n" | ||
| 63 | "Parameters:\n" | ||
| 64 | "\tmodel: model of the kinematic tree\n" | ||
| 65 | "\tdata: data related to the model\n" | ||
| 66 | "\tq: the joint configuration vector (size model.nq)\n", | ||
| 67 | ✗ | bp::return_value_policy<bp::return_by_value>()); | |
| 68 | |||
| 69 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
| 70 | "computeJointJacobians", &computeJointJacobians<Scalar, Options, JointCollectionDefaultTpl>, | ||
| 71 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | bp::args("model", "data"), |
| 72 | "Computes the full model Jacobian, i.e. the stack of all motion subspace expressed " | ||
| 73 | "in the world frame.\n" | ||
| 74 | "The result is accessible through data.J. This function assumes that forward " | ||
| 75 | "kinematics (pinocchio.forwardKinematics) has been called first.\n\n" | ||
| 76 | "Parameters:\n" | ||
| 77 | "\tmodel: model of the kinematic tree\n" | ||
| 78 | "\tdata: data related to the model\n", | ||
| 79 | ✗ | bp::return_value_policy<bp::return_by_value>()); | |
| 80 | |||
| 81 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
| 82 | 40 | "computeJointJacobian", compute_jacobian_proxy, bp::args("model", "data", "q", "joint_id"), | |
| 83 | "Computes the Jacobian of a specific joint frame expressed in the local frame of the " | ||
| 84 | "joint according to the given input configuration.\n\n" | ||
| 85 | "Parameters:\n" | ||
| 86 | "\tmodel: model of the kinematic tree\n" | ||
| 87 | "\tdata: data related to the model\n" | ||
| 88 | "\tq: the joint configuration vector (size model.nq)\n" | ||
| 89 | "\tjoint_id: index of the joint\n"); | ||
| 90 | |||
| 91 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
| 92 | "getJointJacobian", get_jacobian_proxy, | ||
| 93 | 40 | bp::args("model", "data", "joint_id", "reference_frame"), | |
| 94 | "Computes the jacobian of a given given joint according to the given entries in data.\n" | ||
| 95 | "If reference_frame is set to LOCAL, it returns the Jacobian expressed in the local " | ||
| 96 | "coordinate system of the joint.\n" | ||
| 97 | "If reference_frame is set to LOCAL_WORLD_ALIGNED, it returns the Jacobian expressed in " | ||
| 98 | "the coordinate system of the frame centered on the joint, but aligned with the WORLD " | ||
| 99 | "axes.\n" | ||
| 100 | "If reference_frame is set to WORLD, it returns the Jacobian expressed in the coordinate " | ||
| 101 | "system of the frame associated to the WORLD.\n\n" | ||
| 102 | "Parameters:\n" | ||
| 103 | "\tmodel: model of the kinematic tree\n" | ||
| 104 | "\tdata: data related to the model\n" | ||
| 105 | "\tjoint_id: index of the joint\n" | ||
| 106 | "\treference_frame: reference frame in which the resulting derivatives are expressed\n"); | ||
| 107 | |||
| 108 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
| 109 | "computeJointJacobiansTimeVariation", | ||
| 110 | computeJointJacobiansTimeVariation< | ||
| 111 | Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>, | ||
| 112 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | bp::args("model", "data", "q", "v"), |
| 113 | "Computes the full model Jacobian variations with respect to time. It corresponds to " | ||
| 114 | "dJ/dt which depends both on q and v. It also computes the joint Jacobian of the " | ||
| 115 | "model (similar to computeJointJacobians)." | ||
| 116 | "The result is accessible through data.dJ and data.J.\n\n" | ||
| 117 | "Parameters:\n" | ||
| 118 | "\tmodel: model of the kinematic tree\n" | ||
| 119 | "\tdata: data related to the model\n" | ||
| 120 | "\tq: the joint configuration vector (size model.nq)\n" | ||
| 121 | "\tv: the joint velocity vector (size model.nv)\n", | ||
| 122 | ✗ | bp::return_value_policy<bp::return_by_value>()); | |
| 123 | |||
| 124 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
| 125 | "getJointJacobianTimeVariation", get_jacobian_time_variation_proxy, | ||
| 126 | 40 | bp::args("model", "data", "joint_id", "reference_frame"), | |
| 127 | "Computes the Jacobian time variation of a specific joint expressed in the requested frame " | ||
| 128 | "provided by the value of reference_frame." | ||
| 129 | "You have to call computeJointJacobiansTimeVariation first. This function also computes " | ||
| 130 | "the full model Jacobian contained in data.J.\n" | ||
| 131 | "If reference_frame is set to LOCAL, it returns the Jacobian expressed in the local " | ||
| 132 | "coordinate system of the joint.\n" | ||
| 133 | "If reference_frame is set to LOCAL_WORLD_ALIGNED, it returns the Jacobian expressed in " | ||
| 134 | "the coordinate system of the frame centered on the joint, but aligned with the WORLD " | ||
| 135 | "axes.\n" | ||
| 136 | "If reference_frame is set to WORLD, it returns the Jacobian expressed in the coordinate " | ||
| 137 | "system of the frame associated to the WORLD.\n\n" | ||
| 138 | "Parameters:\n" | ||
| 139 | "\tmodel: model of the kinematic tree\n" | ||
| 140 | "\tdata: data related to the model\n" | ||
| 141 | "\tjoint_id: index of the joint\n" | ||
| 142 | "\treference_frame: reference frame in which the resulting derivatives are expressed\n"); | ||
| 143 | 20 | } | |
| 144 | |||
| 145 | } // namespace python | ||
| 146 | } // namespace pinocchio | ||
| 147 |