GCC Code Coverage Report

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