GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/algorithm/frames.hpp Lines: 4 4 100.0 %
Date: 2023-08-09 08:43:58 Branches: 0 0 - %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2020 CNRS INRIA
3
//
4
5
#ifndef __pinocchio_algorithm_frames_hpp__
6
#define __pinocchio_algorithm_frames_hpp__
7
8
#include "pinocchio/multibody/model.hpp"
9
#include "pinocchio/multibody/data.hpp"
10
11
namespace pinocchio
12
{
13
14
  /**
15
   * @brief      Updates the position of each frame contained in the model.
16
   *
17
   * @tparam JointCollection Collection of Joint types.
18
   *
19
   * @param[in]  model  The kinematic model.
20
   * @param      data   Data associated to model.
21
   *
22
   * @warning    One of the algorithms forwardKinematics should have been called first.
23
   */
24
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
25
  inline void updateFramePlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
26
                                    DataTpl<Scalar,Options,JointCollectionTpl> & data);
27
28
  /**
29
   * @brief      Updates the placement of the given frame.
30
   *
31
   * @param[in]  model        The kinematic model.
32
   * @param      data         Data associated to model.
33
   * @param[in]  frame_id     Id of the operational Frame.
34
   *
35
   * @return     A reference to the frame placement stored in data.oMf[frame_id]
36
   *
37
   * @warning    One of the algorithms forwardKinematics should have been called first
38
   */
39
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
40
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::SE3 &
41
  updateFramePlacement(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
42
                       DataTpl<Scalar,Options,JointCollectionTpl> & data,
43
                       const FrameIndex frame_id);
44
45
46
  /**
47
   * @brief      First calls the forwardKinematics on the model, then computes the placement of each frame.
48
   *             /sa pinocchio::forwardKinematics.
49
   *
50
   * @tparam JointCollection Collection of Joint types.
51
   * @tparam ConfigVectorType Type of the joint configuration vector.
52
   *
53
   * @param[in]  model                    The kinematic model.
54
   * @param      data                     Data associated to model.
55
   * @param[in]  q                        Configuration vector.
56
   */
57
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
58
  inline void framesForwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
59
                                      DataTpl<Scalar,Options,JointCollectionTpl> & data,
60
                                      const Eigen::MatrixBase<ConfigVectorType> & q);
61
62
63
  /**
64
   * @brief      Updates the position of each frame contained in the model.
65
   *             This function is now deprecated and has been renamed updateFramePlacements.
66
   *
67
   * @tparam JointCollection Collection of Joint types.
68
   *
69
   * @param[in]  model  The kinematic model.
70
   * @param      data   Data associated to model.
71
   *
72
   * @warning    One of the algorithms forwardKinematics should have been called first.
73
  */
74
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
75
  PINOCCHIO_DEPRECATED
76
  inline void framesForwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
77
                                      DataTpl<Scalar,Options,JointCollectionTpl> & data)
78
  {
79
    updateFramePlacements(model,data);
80
  }
81
82
83
  /**
84
   * @brief      Returns the spatial velocity of the Frame expressed in the desired reference frame.
85
   *             You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure.
86
   *
87
   * @param[in]  model       The kinematic model
88
   * @param[in]  data        Data associated to model
89
   * @param[in]  frame_id    Id of the operational Frame
90
   * @param[in]  rf          Reference frame in which the velocity is expressed.
91
   *
92
   * @return     The spatial velocity of the Frame expressed in the desired reference frame.
93
   *
94
   * @warning    Fist or second order forwardKinematics should have been called first
95
   */
96
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
97
  inline MotionTpl<Scalar, Options>
98
  getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
99
                   const DataTpl<Scalar,Options,JointCollectionTpl> & data,
100
                   const FrameIndex frame_id,
101
                   const ReferenceFrame rf = LOCAL);
102
103
  /**
104
   * @brief      Returns the spatial acceleration of the Frame expressed in the desired reference frame.
105
   *             You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure.
106
   *
107
   * @param[in]  model       The kinematic model
108
   * @param[in]  data        Data associated to model
109
   * @param[in]  frame_id    Id of the operational Frame
110
   * @param[in]  rf          Reference frame in which the acceleration is expressed.
111
   *
112
   * @return The spatial acceleration of the Frame expressed in the desired reference frame.
113
   *
114
   * @warning    Second order forwardKinematics should have been called first
115
   */
116
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
117
  inline MotionTpl<Scalar, Options>
118
  getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
119
                       const DataTpl<Scalar,Options,JointCollectionTpl> & data,
120
                       const FrameIndex frame_id,
121
                       const ReferenceFrame rf = LOCAL);
122
123
  /**
124
   * @brief      Returns the "classical" acceleration of the Frame expressed in the desired reference frame.
125
   *             This is different from the "spatial" acceleration in that centrifugal effects are accounted for.
126
   *             You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure.
127
   *
128
   * @param[in]  model       The kinematic model
129
   * @param[in]  data        Data associated to model
130
   * @param[in]  frame_id    Id of the operational Frame
131
   * @param[in]  rf          Reference frame in which the acceleration is expressed.
132
   *
133
   * @return The classical acceleration of the Frame expressed in the desired reference frame.
134
   *
135
   * @warning    Second order forwardKinematics should have been called first
136
   */
137
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
138
  inline MotionTpl<Scalar, Options>
139
  getFrameClassicalAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
140
                                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
141
                                const FrameIndex frame_id,
142
                                const ReferenceFrame rf = LOCAL);
143
144
  /**
145
   * @brief      Returns the jacobian of the frame expressed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
146
   *             depending on the value of rf.
147
   *             You must first call pinocchio::computeJointJacobians followed by pinocchio::framesForwardKinematics to update placement values in data structure.
148
   *
149
   * @remark     Similarly to pinocchio::getJointJacobian with LOCAL or WORLD parameters, if rf == LOCAL, this function returns the Jacobian of the frame expressed
150
   *             in the local coordinates of the frame, or if rl == WORLD, it returns the Jacobian expressed of the point coincident with the origin
151
   *             and expressed in a coordinate system aligned with the WORLD.
152
   *
153
   * @tparam JointCollection Collection of Joint types.
154
   * @tparam Matrix6xLike Type of the matrix containing the joint Jacobian.
155
   *
156
   * @param[in]  model       The kinematic model
157
   * @param[in]  data        Data associated to model
158
   * @param[in]  frame_id    Id of the operational Frame
159
   * @param[in]  rf          Reference frame in which the Jacobian is expressed.
160
   * @param[out] J           The Jacobian of the Frame expressed in the coordinates Frame.
161
   *
162
   * @warning    The function pinocchio::computeJointJacobians should have been called first.
163
   */
164
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
165
  inline void getFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
166
                               DataTpl<Scalar,Options,JointCollectionTpl> & data,
167
                               const FrameIndex frame_id,
168
                               const ReferenceFrame rf,
169
                               const Eigen::MatrixBase<Matrix6xLike> & J);
170
171
  ///
172
  /// \brief Computes the Jacobian of a specific Frame expressed in the desired reference_frame given as argument.
173
  ///
174
  /// \tparam JointCollection Collection of Joint types.
175
  /// \tparam ConfigVectorType Type of the joint configuration vector.
176
  /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian.
177
  ///
178
  /// \param[in] model                                The model structure of the rigid body system.
179
  /// \param[in] data                                  The data structure of the rigid body system.
180
  /// \param[in] q                                         The joint configuration vector (dim model.nq).
181
  /// \param[in] frameId                            The id of the Frame refering to model.frames[frameId].
182
  /// \param[in] reference_frame          Reference frame in which the Jacobian is expressed.
183
  /// \param[out] J                                       A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.setZero().
184
  ///
185
  /// \return The Jacobian of the specific Frame expressed in the desired reference frame (matrix 6 x model.nv).
186
  ///
187
  /// \remarks The result of this function is equivalent to call first computeJointJacobians(model,data,q), then updateFramePlacements(model,data) and then call getJointJacobian(model,data,jointId,rf,J),
188
  ///         but forwardKinematics and updateFramePlacements are not fully computed.
189
  ///
190
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
191
  inline void computeFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
192
                                   DataTpl<Scalar,Options,JointCollectionTpl> & data,
193
                                   const Eigen::MatrixBase<ConfigVectorType> & q,
194
                                   const FrameIndex frameId,
195
                                   const ReferenceFrame reference_frame,
196
                                   const Eigen::MatrixBase<Matrix6xLike> & J);
197
198
  ///
199
  /// \brief Computes the Jacobian of a specific Frame expressed in the LOCAL frame coordinate system.
200
  ///
201
  /// \tparam JointCollection Collection of Joint types.
202
  /// \tparam ConfigVectorType Type of the joint configuration vector.
203
  /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian.
204
  ///
205
  /// \param[in] model The model structure of the rigid body system.
206
  /// \param[in] data The data structure of the rigid body system.
207
  /// \param[in] q The joint configuration vector (dim model.nq).
208
  /// \param[in] frameId The id of the Frame refering to model.frames[frameId].
209
  ///
210
  /// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.setZero().
211
  ///
212
  /// \return The Jacobian of the specific Frame expressed in the LOCAL frame coordinate system (matrix 6 x model.nv).
213
  ///
214
  /// \remarks The result of this function is equivalent to call first computeJointJacobians(model,data,q), then updateFramePlacements(model,data) and then call getJointJacobian(model,data,jointId,LOCAL,J),
215
  ///         but forwardKinematics and updateFramePlacements are not fully computed.
216
  ///
217
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
218
1
  inline void computeFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
219
                                   DataTpl<Scalar,Options,JointCollectionTpl> & data,
220
                                   const Eigen::MatrixBase<ConfigVectorType> & q,
221
                                   const FrameIndex frameId,
222
                                   const Eigen::MatrixBase<Matrix6xLike> & J)
223
  {
224
1
    computeFrameJacobian(model,data,q.derived(),frameId,LOCAL,
225
1
                         PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J));
226
1
  }
227
228
  ///
229
  /// \brief This function is now deprecated and has been renamed computeFrameJacobian. This signature will be removed in future release of Pinocchio.
230
  ///
231
  /// \copydoc pinocchio::computeFrameJacobian
232
  ///
233
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
234
  PINOCCHIO_DEPRECATED
235
  inline void frameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
236
                            DataTpl<Scalar,Options,JointCollectionTpl> & data,
237
                            const Eigen::MatrixBase<ConfigVectorType> & q,
238
                            const FrameIndex frameId,
239
                            const Eigen::MatrixBase<Matrix6xLike> & J)
240
  {
241
    computeFrameJacobian(model,data,q,frameId,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J));
242
  }
243
244
  ///
245
  /// \brief Computes the Jacobian time variation of a specific frame (given by frame_id) expressed either in the LOCAL frame.
246
  ///
247
  /// \note This jacobian is extracted from data.dJ. You have to run pinocchio::computeJointJacobiansTimeVariation before calling it.
248
  ///
249
  /// \tparam JointCollection Collection of Joint types.
250
  /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian.
251
  ///
252
  /// \param[in] model The model structure of the rigid body system.
253
  /// \param[in] data The data structure of the rigid body system.
254
  /// \param[in] frameId The index of the frame.
255
  ///
256
  /// \param[out] dJ A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.).
257
  ///
258
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
259
  void getFrameJacobianTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
260
                                     DataTpl<Scalar,Options,JointCollectionTpl> & data,
261
                                     const FrameIndex frame_id,
262
                                     const ReferenceFrame rf,
263
                                     const Eigen::MatrixBase<Matrix6xLike> & dJ);
264
265
/**
266
  * @brief Compute the inertia supported by a specific frame (given by frame_id) expressed in the LOCAL frame.
267
  *        The total supported inertia corresponds to the sum of all the inertia after the given frame, i.e :
268
  *         * The frame inertia
269
  *         * The child frames inertia ('Child frames' refers to frames that share the same parent joint and are placed after the given frame)
270
  *         * The child joints inertia (if with_subtree == true)
271
  *        You must first call pinocchio::forwardKinematics to update placement values in data structure.
272
  *
273
  * @note Physically speaking, if the robot were to be cut in two parts at that given frame, this supported inertia would represents the inertia of the part that was after the frame.
274
  *       with_subtree determines if the childs joints must be taken into consideration (if true) or only the current joint (if false).
275
  *
276
  * @note The equivalent function for a joint would be :
277
  *       * to read `data.Ycrb[joint_id]`, after having called pinocchio::crba (if with_subtree == true).
278
  *       * to read `model.inertia[joint_id]` (if with_subtree == false).
279
  *
280
  * @tparam JointCollection Collection of Joint types.
281
  *
282
  * @param[in] model The model structure of the rigid body system.
283
  * @param[in] data The data structure of the rigid body system.
284
  * @param[in] frameId The index of the frame.
285
  * @param[in] with_subtree If false, compute the inertia only inside the frame parent joint if false. If true, include child joints inertia.
286
  *
287
  * @return The computed inertia.
288
  *
289
  * @warning forwardKinematics should have been called first
290
  */
291
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
292
  InertiaTpl<Scalar, Options>
293
  computeSupportedInertiaByFrame(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
294
                                 const DataTpl<Scalar,Options,JointCollectionTpl> & data,
295
                                 const FrameIndex frame_id,
296
                                 bool with_subtree);
297
298
/**
299
  * @brief Computes the force supported by a specific frame (given by frame_id) expressed in the LOCAL frame.
300
  *        The supported force corresponds to the sum of all the forces experienced after the given frame, i.e :
301
  *         * The inertial forces and gravity (applied on the supported inertia in body)
302
  *         * The forces applied by child joints
303
  *         * (The external forces)
304
  *        You must first call pinocchio::rnea to update placements, velocities and efforts values in data structure.
305
  *
306
  * @note If an external force is applied to the frame parent joint (during rnea), it won't be taken in consideration in this function
307
  *       (it will be considered to be applied before the frame in the joint and not after. However external forces applied to child joints will be taken into account).
308
  *
309
  * @note Physically speaking, if the robot were to be separated in two parts glued together at that given frame, the supported force represents the internal forces applide from the part after the cut/frame to the part before.
310
  *       This compute what a force-torque sensor would measures if it would be placed at that frame.
311
  *
312
  * @note The equivalent function for a joint would be to read `data.f[joint_id]`, after having call pinocchio::rnea.
313
  *
314
  * @tparam JointCollection Collection of Joint types.
315
  *
316
  * @param[in] model The model structure of the rigid body system.
317
  * @param[in] data The data structure of the rigid body system.
318
  * @param[in] frameId The index of the frame.
319
  *
320
  * @return The computed force.
321
  *
322
  * @warning pinocchio::rnea should have been called first
323
  */
324
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
325
  ForceTpl<Scalar, Options>
326
  computeSupportedForceByFrame(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
327
                               const DataTpl<Scalar,Options,JointCollectionTpl> & data,
328
                               const FrameIndex frame_id);
329
330
} // namespace pinocchio
331
332
/* --- Details -------------------------------------------------------------------- */
333
#include "pinocchio/algorithm/frames.hxx"
334
335
#endif // ifndef __pinocchio_algorithm_frames_hpp__