Directory: | ./ |
---|---|
File: | bindings/python/algorithm/expose-com.cpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 33 | 63 | 52.4% |
Branches: | 71 | 160 | 44.4% |
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/center-of-mass.hpp" | ||
7 | |||
8 | #include <boost/python/overloads.hpp> | ||
9 | |||
10 | namespace pinocchio | ||
11 | { | ||
12 | namespace python | ||
13 | { | ||
14 | |||
15 | 2 | static context::SE3::Vector3 com_0_proxy( | |
16 | const context::Model & model, | ||
17 | context::Data & data, | ||
18 | const context::VectorXs & q, | ||
19 | bool computeSubtreeComs = true) | ||
20 | { | ||
21 | 2 | return centerOfMass(model, data, q, computeSubtreeComs); | |
22 | } | ||
23 | |||
24 | ✗ | static context::SE3::Vector3 com_1_proxy( | |
25 | const context::Model & model, | ||
26 | context::Data & data, | ||
27 | const context::VectorXs & q, | ||
28 | const context::VectorXs & v, | ||
29 | bool computeSubtreeComs = true) | ||
30 | { | ||
31 | ✗ | return centerOfMass(model, data, q, v, computeSubtreeComs); | |
32 | } | ||
33 | |||
34 | ✗ | static context::SE3::Vector3 com_2_proxy( | |
35 | const context::Model & model, | ||
36 | context::Data & data, | ||
37 | const context::VectorXs & q, | ||
38 | const context::VectorXs & v, | ||
39 | const context::VectorXs & a, | ||
40 | bool computeSubtreeComs = true) | ||
41 | { | ||
42 | ✗ | return centerOfMass(model, data, q, v, a, computeSubtreeComs); | |
43 | } | ||
44 | |||
45 | ✗ | static const context::Data::Vector3 & com_level_proxy( | |
46 | const context::Model & model, | ||
47 | context::Data & data, | ||
48 | KinematicLevel kinematic_level, | ||
49 | bool computeSubtreeComs = true) | ||
50 | { | ||
51 | ✗ | return centerOfMass(model, data, kinematic_level, computeSubtreeComs); | |
52 | } | ||
53 | |||
54 | ✗ | static const context::Data::Vector3 & com_default_proxy( | |
55 | const context::Model & model, context::Data & data, bool computeSubtreeComs = true) | ||
56 | { | ||
57 | ✗ | return centerOfMass(model, data, computeSubtreeComs); | |
58 | } | ||
59 | |||
60 | ✗ | static context::Data::Matrix3x jacobian_subtree_com_kinematics_proxy( | |
61 | const context::Model & model, | ||
62 | context::Data & data, | ||
63 | const context::VectorXs & q, | ||
64 | context::Model::JointIndex jointId) | ||
65 | { | ||
66 | ✗ | context::Data::Matrix3x J(3, model.nv); | |
67 | ✗ | J.setZero(); | |
68 | ✗ | jacobianSubtreeCenterOfMass(model, data, q, jointId, J); | |
69 | |||
70 | ✗ | return J; | |
71 | } | ||
72 | |||
73 | ✗ | static context::Data::Matrix3x jacobian_subtree_com_proxy( | |
74 | const context::Model & model, context::Data & data, context::Model::JointIndex jointId) | ||
75 | { | ||
76 | ✗ | context::Data::Matrix3x J(3, model.nv); | |
77 | ✗ | J.setZero(); | |
78 | ✗ | jacobianSubtreeCenterOfMass(model, data, jointId, J); | |
79 | |||
80 | ✗ | return J; | |
81 | } | ||
82 | |||
83 | ✗ | static context::Data::Matrix3x get_jacobian_subtree_com_proxy( | |
84 | const context::Model & model, context::Data & data, context::Model::JointIndex jointId) | ||
85 | { | ||
86 | ✗ | context::Data::Matrix3x J(3, model.nv); | |
87 | ✗ | J.setZero(); | |
88 | ✗ | getJacobianSubtreeCenterOfMass(model, data, jointId, J); | |
89 | |||
90 | ✗ | return J; | |
91 | } | ||
92 | |||
93 | 20 | void exposeCOM() | |
94 | { | ||
95 | typedef context::Scalar Scalar; | ||
96 | typedef context::VectorXs VectorXs; | ||
97 | enum | ||
98 | { | ||
99 | Options = context::Options | ||
100 | }; | ||
101 | |||
102 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
103 | "computeTotalMass", | ||
104 | (Scalar(*)(const context::Model &)) | ||
105 | & computeTotalMass<Scalar, Options, JointCollectionDefaultTpl>, | ||
106 | 40 | bp::args("model"), "Compute the total mass of the model and return it."); | |
107 | |||
108 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
109 | "computeTotalMass", | ||
110 | (Scalar(*)(const context::Model &, context::Data &)) | ||
111 | & computeTotalMass<Scalar, Options, JointCollectionDefaultTpl>, | ||
112 | 40 | bp::args("model", "data"), | |
113 | "Compute the total mass of the model, put it in data.mass[0] and return it."); | ||
114 | |||
115 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
116 | "computeSubtreeMasses", | ||
117 | (void (*)(const context::Model &, context::Data &)) | ||
118 | & computeSubtreeMasses<Scalar, Options, JointCollectionDefaultTpl>, | ||
119 | 40 | bp::args("model", "data"), | |
120 | "Compute the mass of each kinematic subtree and store it in the vector data.mass."); | ||
121 | |||
122 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
123 | "centerOfMass", com_0_proxy, | ||
124 |
8/16✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 20 times.
✗ Branch 23 not taken.
|
40 | (bp::arg("model"), bp::arg("data"), bp::arg("q"), bp::arg("compute_subtree_coms") = true), |
125 | "Compute the center of mass, putting the result in context::Data and return it." | ||
126 | "If compute_subtree_coms is True, the algorithm also computes the center of mass of the " | ||
127 | "subtrees.", | ||
128 | ✗ | bp::return_value_policy<bp::return_by_value>()); | |
129 | |||
130 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
131 | "centerOfMass", com_1_proxy, | ||
132 |
7/14✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
|
60 | (bp::arg("model"), bp::arg("data"), bp::arg("q"), bp::arg("v"), |
133 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
|
40 | bp::arg("compute_subtree_coms") = true), |
134 | "Computes the center of mass position and velocity by storing the result in context::Data. " | ||
135 | "It returns the center of mass position expressed in the WORLD frame.\n" | ||
136 | "If compute_subtree_coms is True, the algorithm also computes the center of mass of the " | ||
137 | "subtrees.", | ||
138 | ✗ | bp::return_value_policy<bp::return_by_value>()); | |
139 | |||
140 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
141 | "centerOfMass", com_2_proxy, | ||
142 |
9/18✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 20 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 20 times.
✗ Branch 26 not taken.
|
60 | (bp::arg("model"), bp::arg("data"), bp::arg("q"), bp::arg("v"), bp::arg("a"), |
143 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
|
40 | bp::arg("compute_subtree_coms") = true), |
144 | "Computes the center of mass position, velocity and acceleration by storing the result in " | ||
145 | "context::Data. It returns the center of mass position expressed in the WORLD frame.\n" | ||
146 | "If compute_subtree_coms is True, the algorithm also computes the center of mass of the " | ||
147 | "subtrees.", | ||
148 | ✗ | bp::return_value_policy<bp::return_by_value>()); | |
149 | |||
150 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
151 | "centerOfMass", com_level_proxy, | ||
152 |
5/10✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
|
60 | (bp::arg("model"), bp::arg("data"), bp::arg("kinematic_level"), |
153 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
|
40 | bp::arg("compute_subtree_coms") = true), |
154 | "Computes the center of mass position, velocity or acceleration of a given model according " | ||
155 | "to the current kinematic values contained in data and the requested kinematic_level.\n" | ||
156 | "If kinematic_level = POSITION, computes the CoM position, if kinematic_level = VELOCITY, " | ||
157 | "also computes the CoM velocity and if kinematic_level = ACCELERATION, it also computes " | ||
158 | "the CoM acceleration.\n" | ||
159 | "If compute_subtree_coms is True, the algorithm also computes the center of mass of the " | ||
160 | "subtrees.", | ||
161 | ✗ | bp::return_value_policy<bp::return_by_value>()); | |
162 | |||
163 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
164 | "centerOfMass", com_default_proxy, | ||
165 |
6/12✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
|
40 | (bp::arg("model"), bp::arg("data"), bp::arg("compute_subtree_coms") = true), |
166 | "Computes the center of mass position, velocity and acceleration of a given model " | ||
167 | "according to the current kinematic values contained in data.\n" | ||
168 | "If compute_subtree_coms is True, the algorithm also computes the center of mass of " | ||
169 | "the subtrees.", | ||
170 | ✗ | bp::return_value_policy<bp::return_by_value>()); | |
171 | |||
172 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
173 | "jacobianCenterOfMass", | ||
174 | (const context::Data::Matrix3x & (*)(const context::Model &, context::Data &, | ||
175 | const Eigen::MatrixBase<VectorXs> &, bool)) | ||
176 | & jacobianCenterOfMass<Scalar, Options, JointCollectionDefaultTpl, VectorXs>, | ||
177 |
8/16✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 20 times.
✗ Branch 23 not taken.
|
40 | (bp::arg("model"), bp::arg("data"), bp::arg("q"), bp::arg("compute_subtree_coms") = true), |
178 | "Computes the Jacobian of the center of mass, puts the result in context::Data and return " | ||
179 | "it.\n" | ||
180 | "If compute_subtree_coms is True, the algorithm also computes the center of mass of the " | ||
181 | "subtrees.", | ||
182 | ✗ | bp::return_value_policy<bp::return_by_value>()); | |
183 | |||
184 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
185 | "jacobianCenterOfMass", | ||
186 | (const context::Data::Matrix3x & (*)(const context::Model &, context::Data &, bool)) | ||
187 | & jacobianCenterOfMass<Scalar, Options, JointCollectionDefaultTpl>, | ||
188 |
6/12✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
|
40 | (bp::arg("model"), bp::arg("data"), bp::arg("compute_subtree_coms") = true), |
189 | "Computes the Jacobian of the center of mass, puts the result in context::Data and " | ||
190 | "return it.\n" | ||
191 | "If compute_subtree_coms is True, the algorithm also computes the center of mass of " | ||
192 | "the subtrees.", | ||
193 | ✗ | bp::return_value_policy<bp::return_by_value>()); | |
194 | |||
195 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
196 | "jacobianSubtreeCenterOfMass", jacobian_subtree_com_kinematics_proxy, | ||
197 | 40 | bp::args("model", "data", "q", "subtree_root_joint_id"), | |
198 | "Computes the Jacobian of the CoM of the given subtree (subtree_root_joint_id) " | ||
199 | "expressed in the WORLD frame, according to the given joint configuration."); | ||
200 | |||
201 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
202 | "jacobianSubtreeCenterOfMass", jacobian_subtree_com_proxy, | ||
203 | 40 | bp::args("model", "data", "subtree_root_joint_id"), | |
204 | "Computes the Jacobian of the CoM of the given subtree (subtree_root_joint_id) " | ||
205 | "expressed in the WORLD frame, according to the given entries in data."); | ||
206 | |||
207 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
208 | "getJacobianSubtreeCenterOfMass", get_jacobian_subtree_com_proxy, | ||
209 | 40 | bp::args("model", "data", "subtree_root_joint_id"), | |
210 | "Get the Jacobian of the CoM of the given subtree expressed in the world frame, " | ||
211 | "according to the given entries in data. It assumes that jacobianCenterOfMass has " | ||
212 | "been called first."); | ||
213 | 20 | } | |
214 | |||
215 | } // namespace python | ||
216 | } // namespace pinocchio | ||
217 |