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 |