GCC Code Coverage Report


Directory: ./
File: include/pinocchio/algorithm/center-of-mass.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 2 2 100.0%
Branches: 0 0 -%

Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_center_of_mass_hpp__
6 #define __pinocchio_algorithm_center_of_mass_hpp__
7
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10
11 namespace pinocchio
12 {
13
14 ///
15 /// \brief Compute the total mass of the model and return it.
16 ///
17 /// \param[in] model The model structure of the rigid body system.
18 ///
19 /// \return Total mass of the model.
20 ///
21 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
22 inline Scalar computeTotalMass(const ModelTpl<Scalar, Options, JointCollectionTpl> & model);
23
24 ///
25 /// \brief Compute the total mass of the model, put it in data.mass[0] and return it.
26 ///
27 /// \param[in] model The model structure of the rigid body system.
28 /// \param[in] data The data structure of the rigid body system.
29 ///
30 /// \warning This method does not fill the whole data.mass vector. Only data.mass[0] is updated.
31 /// If you need the whole data.mass vector to be computed, use computeSubtreeMasses
32 ///
33 /// \return Total mass of the model.
34 ///
35 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
36 Scalar computeTotalMass(
37 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
38 DataTpl<Scalar, Options, JointCollectionTpl> & data);
39
40 ///
41 /// \brief Compute the mass of each kinematic subtree and store it in data.mass. The element
42 /// mass[0] corresponds to the total mass of the model.
43 ///
44 /// \param[in] model The model structure of the rigid body system.
45 /// \param[in] data The data structure of the rigid body system.
46 ///
47 /// \note If you are only interested in knowing the total mass of the model, computeTotalMass will
48 /// probably be slightly faster.
49 ///
50 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
51 void computeSubtreeMasses(
52 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
53 DataTpl<Scalar, Options, JointCollectionTpl> & data);
54
55 ///
56 /// \brief Computes the center of mass position of a given model according to a particular joint
57 /// configuration.
58 /// The result is accessible through data.com[0] for the full body com and data.com[i] for
59 /// the subtree supported by joint i (expressed in the joint i frame).
60 ///
61 /// \tparam JointCollection Collection of Joint types.
62 /// \tparam ConfigVectorType Type of the joint configuration vector.
63 ///
64 /// \param[in] model The model structure of the rigid body system.
65 /// \param[in] data The data structure of the rigid body system.
66 /// \param[in] q The joint configuration vector (dim model.nq).
67 /// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the
68 /// subtrees.
69 ///
70 /// \return The center of mass position of the full rigid body system expressed in the world
71 /// frame.
72 ///
73 template<
74 typename Scalar,
75 int Options,
76 template<typename, int> class JointCollectionTpl,
77 typename ConfigVectorType>
78 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
79 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
80 DataTpl<Scalar, Options, JointCollectionTpl> & data,
81 const Eigen::MatrixBase<ConfigVectorType> & q,
82 const bool computeSubtreeComs = true);
83
84 ///
85 /// \brief Computes the center of mass position and velocity of a given model according to a
86 /// particular joint configuration and velocity.
87 /// The result is accessible through data.com[0], data.vcom[0] for the full body com
88 /// position and velocity. And data.com[i] and data.vcom[i] for the subtree supported by
89 /// joint i (expressed in the joint i frame).
90 ///
91 /// \tparam JointCollection Collection of Joint types.
92 /// \tparam ConfigVectorType Type of the joint configuration vector.
93 /// \tparam TangentVectorType Type of the joint velocity vector.
94 ///
95 /// \param[in] model The model structure of the rigid body system.
96 /// \param[in] data The data structure of the rigid body system.
97 /// \param[in] q The joint configuration vector (dim model.nq).
98 /// \param[in] v The joint velocity vector (dim model.nv).
99 /// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the
100 /// subtrees.
101 ///
102 /// \return The center of mass position of the full rigid body system expressed in the world
103 /// frame.
104 ///
105 template<
106 typename Scalar,
107 int Options,
108 template<typename, int> class JointCollectionTpl,
109 typename ConfigVectorType,
110 typename TangentVectorType>
111 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
112 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
113 DataTpl<Scalar, Options, JointCollectionTpl> & data,
114 const Eigen::MatrixBase<ConfigVectorType> & q,
115 const Eigen::MatrixBase<TangentVectorType> & v,
116 const bool computeSubtreeComs = true);
117
118 ///
119 /// \brief Computes the center of mass position, velocity and acceleration of a given model
120 /// according to a particular joint configuration, velocity and acceleration.
121 /// The result is accessible through data.com[0], data.vcom[0], data.acom[0] for the full
122 /// body com position, velocity and acceleation. And data.com[i], data.vcom[i] and
123 /// data.acom[i] for the subtree supported by joint i (expressed in the joint i frame).
124 ///
125 /// \tparam JointCollection Collection of Joint types.
126 /// \tparam ConfigVectorType Type of the joint configuration vector.
127 /// \tparam TangentVectorType1 Type of the joint velocity vector.
128 /// \tparam TangentVectorType2 Type of the joint acceleration vector.
129 ///
130 /// \param[in] model The model structure of the rigid body system.
131 /// \param[in] data The data structure of the rigid body system.
132 /// \param[in] q The joint configuration vector (dim model.nq).
133 /// \param[in] v The joint velocity vector (dim model.nv).
134 /// \param[in] a The joint acceleration vector (dim model.nv).
135 /// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the
136 /// subtrees.
137 ///
138 /// \return The center of mass position of the full rigid body system expressed in the world
139 /// frame.
140 ///
141 template<
142 typename Scalar,
143 int Options,
144 template<typename, int> class JointCollectionTpl,
145 typename ConfigVectorType,
146 typename TangentVectorType1,
147 typename TangentVectorType2>
148 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
149 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
150 DataTpl<Scalar, Options, JointCollectionTpl> & data,
151 const Eigen::MatrixBase<ConfigVectorType> & q,
152 const Eigen::MatrixBase<TangentVectorType1> & v,
153 const Eigen::MatrixBase<TangentVectorType2> & a,
154 const bool computeSubtreeComs = true);
155
156 ///
157 /// \brief Computes the center of mass position, velocity and acceleration of a given model
158 /// according to the current kinematic values contained in data and the requested kinematic_level.
159 /// The result is accessible through data.com[0], data.vcom[0] and data.acom[0] for the
160 /// full body com position and velocity. And data.com[i] and data.vcom[i] for the subtree
161 /// supported by joint i (expressed in the joint i frame).
162 ///
163 /// \tparam JointCollection Collection of Joint types.
164 ///
165 /// \param[in] model The model structure of the rigid body system.
166 /// \param[in] data The data structure of the rigid body system.
167 /// \param[in] kinematic_level if = POSITION, computes the CoM position, if = VELOCITY, also
168 /// computes the CoM velocity and if = ACCELERATION, it also computes the CoM acceleration.
169 /// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the
170 /// subtrees.
171 ///
172 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
173 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
174 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
175 DataTpl<Scalar, Options, JointCollectionTpl> & data,
176 KinematicLevel kinematic_level,
177 const bool computeSubtreeComs = true);
178
179 ///
180 /// \brief Computes the center of mass position, velocity and acceleration of a given model
181 /// according to the current kinematic values contained in data.
182 /// The result is accessible through data.com[0], data.vcom[0] and data.acom[0] for the
183 /// full body com position and velocity. And data.com[i] and data.vcom[i] for the subtree
184 /// supported by joint i (expressed in the joint i frame).
185 ///
186 /// \tparam JointCollection Collection of Joint types.
187 ///
188 /// \param[in] model The model structure of the rigid body system.
189 /// \param[in] data The data structure of the rigid body system.
190 /// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the
191 /// subtrees, expressed in the local coordinate frame of each joint.
192 ///
193 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
194 7 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
195 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
196 DataTpl<Scalar, Options, JointCollectionTpl> & data,
197 const bool computeSubtreeComs = true)
198 {
199 7 return centerOfMass(model, data, ACCELERATION, computeSubtreeComs);
200 }
201
202 ///
203 /// \brief Computes both the jacobian and the the center of mass position of a given model
204 /// according to a particular joint configuration.
205 /// The results are accessible through data.Jcom and data.com[0] and are both expressed in
206 /// the world frame. In addition, the algorithm also computes the Jacobian of all the
207 /// joints (\sa pinocchio::computeJointJacobians). And data.com[i] gives the center of mass
208 /// of the subtree supported by joint i (expressed in the world frame).
209 ///
210 /// \tparam JointCollection Collection of Joint types.
211 /// \tparam ConfigVectorType Type of the joint configuration vector.
212 ///
213 /// \param[in] model The model structure of the rigid body system.
214 /// \param[in] data The data structure of the rigid body system.
215 /// \param[in] q The joint configuration vector (dim model.nq).
216 /// \param[in] computeSubtreeComs If true, the algorithm also computes the centers of mass of the
217 /// subtrees, expressed in the world coordinate frame.
218 ///
219 /// \return The jacobian of center of mass position of the rigid body system expressed in the
220 /// world frame (matrix 3 x model.nv).
221 ///
222 template<
223 typename Scalar,
224 int Options,
225 template<typename, int> class JointCollectionTpl,
226 typename ConfigVectorType>
227 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass(
228 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
229 DataTpl<Scalar, Options, JointCollectionTpl> & data,
230 const Eigen::MatrixBase<ConfigVectorType> & q,
231 const bool computeSubtreeComs = true);
232
233 ///
234 /// \brief Computes both the jacobian and the the center of mass position of a given model
235 /// according to the current value stored in data.
236 /// It assumes that forwardKinematics has been called first.
237 /// The results are accessible through data.Jcom and data.com[0] and are both expressed in
238 /// the world frame. In addition, the algorithm also computes the Jacobian of all the
239 /// joints (\sa pinocchio::computeJointJacobians). And data.com[i] gives the center of mass
240 /// of the subtree supported by joint i (expressed in the world frame).
241 ///
242 /// \tparam JointCollection Collection of Joint types.
243 /// \tparam ConfigVectorType Type of the joint configuration vector.
244 ///
245 /// \param[in] model The model structure of the rigid body system.
246 /// \param[in] data The data structure of the rigid body system.
247 /// \param[in] computeSubtreeComs If true, the algorithm also computes the center of mass of the
248 /// subtrees, expressed in the world coordinate frame.
249 ///
250 /// \return The jacobian of center of mass position of the rigid body system expressed in the
251 /// world frame (matrix 3 x model.nv).
252 ///
253 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
254 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass(
255 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
256 DataTpl<Scalar, Options, JointCollectionTpl> & data,
257 const bool computeSubtreeComs = true);
258
259 ///
260 /// \brief Computes the Jacobian of the center of mass of the given subtree according to a
261 /// particular joint configuration.
262 /// In addition, the algorithm also computes the Jacobian of all the joints (\sa
263 /// pinocchio::computeJointJacobians).
264 ///
265 /// \tparam JointCollection Collection of Joint types.
266 /// \tparam ConfigVectorType Type of the joint configuration vector.
267 /// \tparam Matrix3xLike Type of the output Jacobian matrix.
268 ///
269 /// \param[in] model The model structure of the rigid body system.
270 /// \param[in] data The data structure of the rigid body system.
271 /// \param[in] q The joint configuration vector (dim model.nq).
272 /// \param[in] rootSubtreeId Index of the parent joint supporting the subtree.
273 /// \param[out] res The Jacobian matrix where the results will be stored in (dim 3 x model.nv).
274 /// You must first fill J with zero elements, e.g. J.setZero().
275 ///
276 template<
277 typename Scalar,
278 int Options,
279 template<typename, int> class JointCollectionTpl,
280 typename ConfigVectorType,
281 typename Matrix3xLike>
282 void jacobianSubtreeCenterOfMass(
283 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
284 DataTpl<Scalar, Options, JointCollectionTpl> & data,
285 const Eigen::MatrixBase<ConfigVectorType> & q,
286 const JointIndex & rootSubtreeId,
287 const Eigen::MatrixBase<Matrix3xLike> & res);
288
289 ///
290 /// \brief Computes the Jacobian of the center of mass of the given subtree according to the
291 /// current value stored in data.
292 /// It assumes that forwardKinematics has been called first.
293 ///
294 /// \tparam JointCollection Collection of Joint types.
295 /// \tparam Matrix3xLike Type of the output Jacobian matrix.
296 ///
297 /// \param[in] model The model structure of the rigid body system.
298 /// \param[in] data The data structure of the rigid body system.
299 /// \param[in] rootSubtreeId Index of the parent joint supporting the subtree.
300 /// \param[out] res The Jacobian matrix where the results will be stored in (dim 3 x model.nv).
301 /// You must first fill J with zero elements, e.g. J.setZero().
302 ///
303 template<
304 typename Scalar,
305 int Options,
306 template<typename, int> class JointCollectionTpl,
307 typename Matrix3xLike>
308 void jacobianSubtreeCenterOfMass(
309 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
310 DataTpl<Scalar, Options, JointCollectionTpl> & data,
311 const JointIndex & rootSubtreeId,
312 const Eigen::MatrixBase<Matrix3xLike> & res);
313
314 ///
315 /// \brief Retrieves the Jacobian of the center of mass of the given subtree according to the
316 /// current value stored in data.
317 /// It assumes that pinocchio::jacobianCenterOfMass has been called first with
318 /// computeSubtreeComs equals to true.
319 ///
320 /// \tparam JointCollection Collection of Joint types.
321 /// \tparam Matrix3xLike Type of the output Jacobian matrix.
322 ///
323 /// \param[in] model The model structure of the rigid body system.
324 /// \param[in] data The data structure of the rigid body system.
325 /// \param[in] rootSubtreeId Index of the parent joint supporting the subtree.
326 /// \param[out] res The Jacobian matrix where the results will be stored in (dim 3 x model.nv).
327 /// You must first fill J with zero elements, e.g. J.setZero().
328 ///
329 template<
330 typename Scalar,
331 int Options,
332 template<typename, int> class JointCollectionTpl,
333 typename Matrix3xLike>
334 void getJacobianSubtreeCenterOfMass(
335 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
336 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
337 const JointIndex & rootSubtreeId,
338 const Eigen::MatrixBase<Matrix3xLike> & res);
339
340 /* If the CRBA has been run, then both COM and Jcom are easily available from
341 * the joint space mass matrix (data.M).
342 * Use the following function to infer them directly. In that case,
343 * the COM subtrees (also easily available from CRBA data) are not
344 * explicitely set. Use data.Ycrb[i].lever() to get them. */
345 ///
346 /// \brief Extracts the center of mass position from the joint space inertia matrix (also called
347 /// the mass matrix).
348 ///
349 /// \tparam JointCollection Collection of Joint types.
350 /// \tparam Matrix3xLike Type of the output Jacobian matrix.
351 ///
352 /// \param[in] model The model structure of the rigid body system.
353 /// \param[in] data The data structure of the rigid body system.
354 ///
355 /// \return The center of mass position of the rigid body system expressed in the world frame
356 /// (vector 3).
357 ///
358 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
359 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & getComFromCrba(
360 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
361 DataTpl<Scalar, Options, JointCollectionTpl> & data);
362
363 ///
364 /// \brief Extracts both the jacobian of the center of mass (CoM), the total mass of the system
365 /// and the CoM position from the joint space inertia matrix (also called the mass matrix).
366 /// The results are accessible through data.Jcom, data.mass[0] and data.com[0] and are both
367 /// expressed in the world frame.
368 ///
369 /// \tparam JointCollection Collection of Joint types.
370 ///
371 /// \param[in] model The model structure of the rigid body system.
372 /// \param[in] data The data structure of the rigid body system.
373 ///
374 /// \return The jacobian of the CoM expressed in the world frame (matrix 3 x model.nv).
375 ///
376 /// \remarks This extraction of inertial quantities is only valid for free-floating base systems.
377 ///
378 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
379 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & getJacobianComFromCrba(
380 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
381 DataTpl<Scalar, Options, JointCollectionTpl> & data);
382
383 } // namespace pinocchio
384
385 /* --- Details -------------------------------------------------------------------- */
386 /* --- Details -------------------------------------------------------------------- */
387 /* --- Details -------------------------------------------------------------------- */
388 #include "pinocchio/algorithm/center-of-mass.hxx"
389
390 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
391 #include "pinocchio/algorithm/center-of-mass.txx"
392 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
393
394 #endif // ifndef __pinocchio_algorithm_center_of_mass_hpp__
395