Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/frames.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 25 | 29 | 86.2% |
Branches: | 4 | 14 | 28.6% |
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( | ||
26 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
27 | DataTpl<Scalar, Options, JointCollectionTpl> & data); | ||
28 | |||
29 | /** | ||
30 | * @brief Updates the placement of the given frame. | ||
31 | * | ||
32 | * @param[in] model The kinematic model. | ||
33 | * @param data Data associated to model. | ||
34 | * @param[in] frame_id Id of the operational Frame. | ||
35 | * | ||
36 | * @return A reference to the frame placement stored in data.oMf[frame_id] | ||
37 | * | ||
38 | * @warning One of the algorithms forwardKinematics should have been called first | ||
39 | */ | ||
40 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
41 | inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::SE3 & updateFramePlacement( | ||
42 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
43 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
44 | const FrameIndex frame_id); | ||
45 | |||
46 | /** | ||
47 | * @brief First calls the forwardKinematics on the model, then computes the placement of each | ||
48 | * frame. /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< | ||
58 | typename Scalar, | ||
59 | int Options, | ||
60 | template<typename, int> | ||
61 | class JointCollectionTpl, | ||
62 | typename ConfigVectorType> | ||
63 | inline void framesForwardKinematics( | ||
64 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
65 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
66 | const Eigen::MatrixBase<ConfigVectorType> & q); | ||
67 | |||
68 | /** | ||
69 | * @brief Returns the spatial velocity of the Frame expressed in the desired reference frame. | ||
70 | * You must first call pinocchio::forwardKinematics to update placement and velocity | ||
71 | * values in data structure. | ||
72 | * | ||
73 | * @param[in] model The kinematic model | ||
74 | * @param[in] data Data associated to model | ||
75 | * @param[in] joint_id Id of the parent joint | ||
76 | * @param[in] placement frame placement with respect to the parent joint | ||
77 | * @param[in] rf Reference frame in which the velocity is expressed. | ||
78 | * | ||
79 | * @return The spatial velocity of the Frame expressed in the desired reference frame. | ||
80 | * | ||
81 | * @warning Fist or second order forwardKinematics should have been called first | ||
82 | */ | ||
83 | |||
84 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
85 | inline MotionTpl<Scalar, Options> getFrameVelocity( | ||
86 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
87 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
88 | const JointIndex joint_id, | ||
89 | const SE3Tpl<Scalar, Options> & placement, | ||
90 | const ReferenceFrame rf = LOCAL); | ||
91 | |||
92 | /** | ||
93 | * @brief Returns the spatial velocity of the Frame expressed in the desired reference frame. | ||
94 | * You must first call pinocchio::forwardKinematics to update placement and velocity | ||
95 | * values in data structure. | ||
96 | * | ||
97 | * @param[in] model The kinematic model | ||
98 | * @param[in] data Data associated to model | ||
99 | * @param[in] frame_id Id of the operational Frame | ||
100 | * @param[in] rf Reference frame in which the velocity is expressed. | ||
101 | * | ||
102 | * @return The spatial velocity of the Frame expressed in the desired reference frame. | ||
103 | * | ||
104 | * @warning Fist or second order forwardKinematics should have been called first | ||
105 | */ | ||
106 | |||
107 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
108 | 213 | inline MotionTpl<Scalar, Options> getFrameVelocity( | |
109 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
110 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
111 | const FrameIndex frame_id, | ||
112 | const ReferenceFrame rf = LOCAL) | ||
113 | { | ||
114 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
115 | 213 | const typename Model::Frame & frame = model.frames[frame_id]; | |
116 | |||
117 | 213 | return getFrameVelocity(model, data, frame.parentJoint, frame.placement, rf); | |
118 | } | ||
119 | |||
120 | /** | ||
121 | * @brief Returns the spatial acceleration of the Frame expressed in the desired reference | ||
122 | * frame. You must first call pinocchio::forwardKinematics to update placement, velocity and | ||
123 | * acceleration values in data structure. | ||
124 | * | ||
125 | * @param[in] model The kinematic model | ||
126 | * @param[in] data Data associated to model | ||
127 | * @param[in] joint_id Id of the parent joint | ||
128 | * @param[in] placement frame placement with respect to the parent joint | ||
129 | * @param[in] rf Reference frame in which the acceleration is expressed. | ||
130 | * | ||
131 | * @return The spatial acceleration of the Frame expressed in the desired reference frame. | ||
132 | * | ||
133 | * @warning Second order forwardKinematics should have been called first | ||
134 | */ | ||
135 | |||
136 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
137 | inline MotionTpl<Scalar, Options> getFrameAcceleration( | ||
138 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
139 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
140 | const JointIndex joint_id, | ||
141 | const SE3Tpl<Scalar, Options> & placement, | ||
142 | const ReferenceFrame rf = LOCAL); | ||
143 | |||
144 | /** | ||
145 | * @brief Returns the spatial acceleration of the Frame expressed in the desired reference | ||
146 | * frame. You must first call pinocchio::forwardKinematics to update placement, velocity and | ||
147 | * acceleration values in the data structure. | ||
148 | * | ||
149 | * @param[in] model The kinematic model | ||
150 | * @param[in] data Data associated to model | ||
151 | * @param[in] frame_id Id of the operational Frame | ||
152 | * @param[in] rf Reference frame in which the acceleration is expressed. | ||
153 | * | ||
154 | * @return The spatial acceleration of the Frame expressed in the desired reference frame. | ||
155 | * | ||
156 | * @warning Second order @ref forwardKinematics should have been called first | ||
157 | * | ||
158 | * @remark In the context of a frame placement constraint \f$J(q) a + \dot{J}(q, v) v = 0\f$, | ||
159 | * one way to compute the second term \f$\dot{J}(q, v) v\f$ is to call second-order | ||
160 | * @ref forwardKinematics with a zero acceleration, then read the remaining \f$\dot{J}(q, v) v\f$ | ||
161 | * by calling this function. This is significantly more efficient than applying the matrix | ||
162 | * \f$\dot{J}(q, v)\f$ (from @ref getFrameJacobianTimeVariation) to the velocity vector \f$v\f$. | ||
163 | */ | ||
164 | |||
165 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
166 | 306 | inline MotionTpl<Scalar, Options> getFrameAcceleration( | |
167 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
168 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
169 | const FrameIndex frame_id, | ||
170 | const ReferenceFrame rf = LOCAL) | ||
171 | { | ||
172 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
173 | 306 | const typename Model::Frame & frame = model.frames[frame_id]; | |
174 | 306 | return getFrameAcceleration(model, data, frame.parentJoint, frame.placement, rf); | |
175 | } | ||
176 | |||
177 | /** | ||
178 | * @brief Returns the "classical" acceleration of the Frame expressed in the desired | ||
179 | * reference frame. This is different from the "spatial" acceleration in that centrifugal effects | ||
180 | * are accounted for. You must first call pinocchio::forwardKinematics to update placement, | ||
181 | * velocity and acceleration values in data structure. | ||
182 | * | ||
183 | * @param[in] model The kinematic model | ||
184 | * @param[in] data Data associated to model | ||
185 | * @param[in] joint_id Id of the parent joint | ||
186 | * @param[in] placement frame placement with respect to the parent joint | ||
187 | * @param[in] rf Reference frame in which the acceleration is expressed. | ||
188 | * | ||
189 | * @return The classical acceleration of the Frame expressed in the desired reference frame. | ||
190 | * | ||
191 | * @warning Second order forwardKinematics should have been called first | ||
192 | */ | ||
193 | |||
194 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
195 | inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration( | ||
196 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
197 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
198 | const JointIndex joint_id, | ||
199 | const SE3Tpl<Scalar, Options> & placement, | ||
200 | const ReferenceFrame rf = LOCAL); | ||
201 | |||
202 | /** | ||
203 | * @brief Returns the "classical" acceleration of the Frame expressed in the desired | ||
204 | * reference frame. This is different from the "spatial" acceleration in that centrifugal effects | ||
205 | * are accounted for. You must first call pinocchio::forwardKinematics to update placement, | ||
206 | * velocity and acceleration values in data structure. | ||
207 | * | ||
208 | * @param[in] model The kinematic model | ||
209 | * @param[in] data Data associated to model | ||
210 | * @param[in] frame_id Id of the operational Frame | ||
211 | * @param[in] rf Reference frame in which the acceleration is expressed. | ||
212 | * | ||
213 | * @return The classical acceleration of the Frame expressed in the desired reference frame. | ||
214 | * | ||
215 | * @warning Second order @ref forwardKinematics should have been called first | ||
216 | * | ||
217 | * @remark In the context of a frame placement constraint \f$J(q) a + \dot{J}(q, v) v = 0\f$, | ||
218 | * one way to compute the second term \f$\dot{J}(q, v) v\f$ is to call second-order | ||
219 | * @ref forwardKinematics with a zero acceleration, then read the remaining \f$\dot{J}(q, v) v\f$ | ||
220 | * by calling this function. This is significantly more efficient than applying the matrix | ||
221 | * \f$\dot{J}(q, v)\f$ (from @ref getFrameJacobianTimeVariation) to the velocity vector \f$v\f$. | ||
222 | * | ||
223 | */ | ||
224 | |||
225 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
226 | 9 | inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration( | |
227 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
228 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
229 | const FrameIndex frame_id, | ||
230 | const ReferenceFrame rf = LOCAL) | ||
231 | { | ||
232 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
233 | 9 | const typename Model::Frame & frame = model.frames[frame_id]; | |
234 | 9 | return getFrameClassicalAcceleration(model, data, frame.parentJoint, frame.placement, rf); | |
235 | } | ||
236 | |||
237 | /** | ||
238 | * @brief Returns the jacobian of the frame given by its relative placement w.r.t. a joint | ||
239 | * frame, and whose columns are either expressed in the LOCAL frame coordinate system, in the | ||
240 | * local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the WORLD coordinate system, | ||
241 | * depending on the value of reference_frame. You must first call | ||
242 | * pinocchio::computeJointJacobians. | ||
243 | * | ||
244 | * @remarks Similarly to pinocchio::getJointJacobian: | ||
245 | * - if rf == LOCAL, this function returns the Jacobian of the frame expressed in | ||
246 | * the local coordinate system of the frame | ||
247 | * - if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame | ||
248 | * centered on the frame origin and expressed in a coordinate system aligned with the WORLD. | ||
249 | * - if rf == WORLD, this function returns the Jacobian of the frame expressed at | ||
250 | * the point coincident with the origin and expressed in a coordinate system aligned with the | ||
251 | * WORLD. | ||
252 | * | ||
253 | * @tparam JointCollection Collection of Joint types. | ||
254 | * @tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
255 | * | ||
256 | * @param[in] model The kinematic model | ||
257 | * @param[in] data Data associated to model | ||
258 | * @param[in] joint_id Index of the joint. | ||
259 | * @param[in] reference_frame Reference frame in which the Jacobian is expressed. | ||
260 | * @param[out] J The Jacobian of the Frame expressed in the | ||
261 | * reference_frame coordinate system. | ||
262 | * | ||
263 | * @warning The function pinocchio::computeJointJacobians should have been called first. | ||
264 | */ | ||
265 | template< | ||
266 | typename Scalar, | ||
267 | int Options, | ||
268 | template<typename, int> | ||
269 | class JointCollectionTpl, | ||
270 | typename Matrix6xLike> | ||
271 | void getFrameJacobian( | ||
272 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
273 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
274 | const JointIndex joint_id, | ||
275 | const SE3Tpl<Scalar, Options> & placement, | ||
276 | const ReferenceFrame reference_frame, | ||
277 | const Eigen::MatrixBase<Matrix6xLike> & J); | ||
278 | |||
279 | /** | ||
280 | * @brief Returns the jacobian of the frame given by its relative placement w.r.t. a joint | ||
281 | * frame, and whose columns are either expressed in the LOCAL frame coordinate system, in the | ||
282 | * local world aligned frame or in the WORLD coordinate system, depending on the value of | ||
283 | * reference_frame. You must first call pinocchio::computeJointJacobians. | ||
284 | * | ||
285 | * @remarks Similarly to pinocchio::getJointJacobian: | ||
286 | * - if rf == LOCAL, this function returns the Jacobian of the frame expressed in | ||
287 | * the local coordinate system of the frame | ||
288 | * - if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame | ||
289 | * centered on the frame origin and expressed in a coordinate system aligned with the WORLD. | ||
290 | * - if rf == WORLD, this function returns the Jacobian of the frame expressed at | ||
291 | * the point coincident with the origin and expressed in a coordinate system aligned with the | ||
292 | * WORLD. | ||
293 | * | ||
294 | * @tparam JointCollection Collection of Joint types. | ||
295 | * | ||
296 | * @param[in] model The kinematic model | ||
297 | * @param[in] data Data associated to model | ||
298 | * @param[in] joint_id Index of the joint. | ||
299 | * @param[in] reference_frame Reference frame in which the Jacobian is expressed. | ||
300 | * | ||
301 | * @warning The function pinocchio::computeJointJacobians should have been called first. | ||
302 | */ | ||
303 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
304 | ✗ | Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian( | |
305 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
306 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
307 | const JointIndex joint_id, | ||
308 | const SE3Tpl<Scalar, Options> & placement, | ||
309 | const ReferenceFrame reference_frame) | ||
310 | { | ||
311 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x; | ||
312 | ✗ | Matrix6x res(Matrix6x::Zero(6, model.nv)); | |
313 | |||
314 | ✗ | getFrameJacobian(model, data, joint_id, placement, reference_frame, res); | |
315 | ✗ | return res; | |
316 | } | ||
317 | |||
318 | /** | ||
319 | * @brief Returns the jacobian of the frame expressed either expressed in the local frame | ||
320 | * coordinate system, in the local world aligned frame or in the WORLD coordinate system, | ||
321 | * depending on the value of reference_frame. You must first call | ||
322 | * pinocchio::computeJointJacobians. | ||
323 | * | ||
324 | * @remarks Similarly to pinocchio::getJointJacobian: | ||
325 | * - if rf == LOCAL, this function returns the Jacobian of the frame expressed in | ||
326 | * the local coordinate system of the frame | ||
327 | * - if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame | ||
328 | * centered on the frame origin and expressed in a coordinate system aligned with the WORLD. | ||
329 | * - if rf == WORLD, this function returns the Jacobian of the frame expressed at | ||
330 | * the point coincident with the origin and expressed in a coordinate system aligned with the | ||
331 | * WORLD. | ||
332 | * | ||
333 | * @tparam JointCollection Collection of Joint types. | ||
334 | * @tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
335 | * | ||
336 | * @param[in] model The kinematic model | ||
337 | * @param[in] data Data associated to model | ||
338 | * @param[in] frame_id Index of the frame | ||
339 | * @param[in] reference_frame Reference frame in which the Jacobian is expressed. | ||
340 | * @param[out] J The Jacobian of the Frame expressed in the | ||
341 | * coordinates Frame. | ||
342 | * | ||
343 | * @warning The function pinocchio::computeJointJacobians should have been called first. | ||
344 | */ | ||
345 | template< | ||
346 | typename Scalar, | ||
347 | int Options, | ||
348 | template<typename, int> | ||
349 | class JointCollectionTpl, | ||
350 | typename Matrix6xLike> | ||
351 | 24 | inline void getFrameJacobian( | |
352 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
353 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
354 | const FrameIndex frame_id, | ||
355 | const ReferenceFrame reference_frame, | ||
356 | const Eigen::MatrixBase<Matrix6xLike> & J) | ||
357 | { | ||
358 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 24 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
24 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
359 | frame_id < (FrameIndex)model.nframes, "The index of the Frame is outside the bounds."); | ||
360 | |||
361 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
362 | typedef typename Model::Frame Frame; | ||
363 | |||
364 | 24 | const Frame & frame = model.frames[frame_id]; | |
365 |
1/2✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
|
24 | data.oMf[frame_id] = data.oMi[frame.parentJoint] * frame.placement; |
366 | |||
367 | 24 | getFrameJacobian( | |
368 | 24 | model, data, frame.parentJoint, frame.placement, reference_frame, | |
369 | 24 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)); | |
370 | 24 | } | |
371 | |||
372 | /** | ||
373 | * @brief Returns the jacobian of the frame expressed either expressed in the local frame | ||
374 | * coordinate system, in the local world aligned frame or in the WORLD coordinate system, | ||
375 | * depending on the value of reference_frame. You must first call | ||
376 | * pinocchio::computeJointJacobians. | ||
377 | * | ||
378 | * @remarks Similarly to pinocchio::getJointJacobian: | ||
379 | * - if rf == LOCAL, this function returns the Jacobian of the frame expressed in | ||
380 | * the local coordinate system of the frame | ||
381 | * - if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame | ||
382 | * centered on the frame origin and expressed in a coordinate system aligned with the WORLD. | ||
383 | * - if rf == WORLD, this function returns the Jacobian of the frame expressed at | ||
384 | * the point coincident with the origin and expressed in a coordinate system aligned with the | ||
385 | * WORLD. | ||
386 | * | ||
387 | * @tparam JointCollection Collection of Joint types. | ||
388 | * | ||
389 | * @param[in] model The kinematic model | ||
390 | * @param[in] data Data associated to model | ||
391 | * @param[in] frame_id Index of the frame | ||
392 | * @param[in] reference_frame Reference frame in which the Jacobian is expressed. | ||
393 | * | ||
394 | * @warning The function pinocchio::computeJointJacobians should have been called first. | ||
395 | */ | ||
396 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
397 | 3 | Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian( | |
398 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
399 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
400 | const FrameIndex frame_id, | ||
401 | const ReferenceFrame reference_frame) | ||
402 | { | ||
403 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x; | ||
404 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | Matrix6x res(Matrix6x::Zero(6, model.nv)); |
405 | |||
406 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | getFrameJacobian(model, data, frame_id, reference_frame, res); |
407 | 3 | return res; | |
408 | } | ||
409 | |||
410 | /// | ||
411 | /// \brief Computes the Jacobian of a specific Frame expressed in the desired reference_frame | ||
412 | /// given as argument. | ||
413 | /// | ||
414 | /// \tparam JointCollection Collection of Joint types. | ||
415 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
416 | /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
417 | /// | ||
418 | /// \param[in] model The model structure of the rigid body system. | ||
419 | /// \param[in] data The data structure of the rigid body system. | ||
420 | /// \param[in] q The joint configuration vector (dim | ||
421 | /// model.nq). \param[in] frameId The id of the Frame refering to | ||
422 | /// model.frames[frameId]. \param[in] reference_frame Reference frame in which the | ||
423 | /// Jacobian is expressed. \param[out] J A reference on the | ||
424 | /// Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with | ||
425 | /// zero elements, e.g. J.setZero(). | ||
426 | /// | ||
427 | /// \return The Jacobian of the specific Frame expressed in the desired reference frame (matrix 6 | ||
428 | /// x model.nv). | ||
429 | /// | ||
430 | /// \remarks The result of this function is equivalent to call first | ||
431 | /// computeJointJacobians(model,data,q), then updateFramePlacements(model,data) and then call | ||
432 | /// getJointJacobian(model,data,jointId,rf,J), | ||
433 | /// but forwardKinematics and updateFramePlacements are not fully computed. | ||
434 | /// | ||
435 | template< | ||
436 | typename Scalar, | ||
437 | int Options, | ||
438 | template<typename, int> | ||
439 | class JointCollectionTpl, | ||
440 | typename ConfigVectorType, | ||
441 | typename Matrix6xLike> | ||
442 | inline void computeFrameJacobian( | ||
443 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
444 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
445 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
446 | const FrameIndex frameId, | ||
447 | const ReferenceFrame reference_frame, | ||
448 | const Eigen::MatrixBase<Matrix6xLike> & J); | ||
449 | |||
450 | /// | ||
451 | /// \brief Computes the Jacobian of a specific Frame expressed in the LOCAL frame coordinate | ||
452 | /// system. | ||
453 | /// | ||
454 | /// \tparam JointCollection Collection of Joint types. | ||
455 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
456 | /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
457 | /// | ||
458 | /// \param[in] model The model structure of the rigid body system. | ||
459 | /// \param[in] data The data structure of the rigid body system. | ||
460 | /// \param[in] q The joint configuration vector (dim model.nq). | ||
461 | /// \param[in] frameId The id of the Frame refering to model.frames[frameId]. | ||
462 | /// | ||
463 | /// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x | ||
464 | /// model.nv). You must fill J with zero elements, e.g. J.setZero(). | ||
465 | /// | ||
466 | /// \return The Jacobian of the specific Frame expressed in the LOCAL frame coordinate system | ||
467 | /// (matrix 6 x model.nv). | ||
468 | /// | ||
469 | /// \remarks The result of this function is equivalent to call first | ||
470 | /// computeJointJacobians(model,data,q), then updateFramePlacements(model,data) and then call | ||
471 | /// getJointJacobian(model,data,jointId,LOCAL,J), | ||
472 | /// but forwardKinematics and updateFramePlacements are not fully computed. | ||
473 | /// | ||
474 | template< | ||
475 | typename Scalar, | ||
476 | int Options, | ||
477 | template<typename, int> | ||
478 | class JointCollectionTpl, | ||
479 | typename ConfigVectorType, | ||
480 | typename Matrix6xLike> | ||
481 | 1 | inline void computeFrameJacobian( | |
482 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
483 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
484 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
485 | const FrameIndex frameId, | ||
486 | const Eigen::MatrixBase<Matrix6xLike> & J) | ||
487 | { | ||
488 | 1 | computeFrameJacobian( | |
489 | 1 | model, data, q.derived(), frameId, LOCAL, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)); | |
490 | 1 | } | |
491 | |||
492 | /// | ||
493 | /// \brief Computes the Jacobian time variation of a specific frame (given by frame_id) expressed | ||
494 | /// either in the WORLD frame (rf = WORLD), in the local world aligned (rf = LOCAL_WORLD_ALIGNED) | ||
495 | /// frame or in the LOCAL frame (rf = LOCAL). | ||
496 | /// | ||
497 | /// \note This jacobian is extracted from data.dJ. You have to run | ||
498 | /// pinocchio::computeJointJacobiansTimeVariation before calling it. | ||
499 | /// | ||
500 | /// \tparam JointCollection Collection of Joint types. | ||
501 | /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
502 | /// | ||
503 | /// \param[in] model The model structure of the rigid body system. | ||
504 | /// \param[in] data The data structure of the rigid body system. | ||
505 | /// \param[in] frameId The index of the frame. | ||
506 | /// | ||
507 | /// \param[out] dJ A reference on the Jacobian matrix where the results will be stored in (dim 6 x | ||
508 | /// model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.). | ||
509 | /// | ||
510 | template< | ||
511 | typename Scalar, | ||
512 | int Options, | ||
513 | template<typename, int> | ||
514 | class JointCollectionTpl, | ||
515 | typename Matrix6xLike> | ||
516 | void getFrameJacobianTimeVariation( | ||
517 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
518 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
519 | const FrameIndex frame_id, | ||
520 | const ReferenceFrame rf, | ||
521 | const Eigen::MatrixBase<Matrix6xLike> & dJ); | ||
522 | |||
523 | /** | ||
524 | * @brief Compute the inertia supported by a specific frame (given by frame_id) expressed in the | ||
525 | * LOCAL frame. The total supported inertia corresponds to the sum of all the inertia after the | ||
526 | * given frame, i.e : | ||
527 | * * The frame inertia | ||
528 | * * The child frames inertia ('Child frames' refers to frames that share the same parent | ||
529 | * joint and are placed after the given frame) | ||
530 | * * The child joints inertia (if with_subtree == true) | ||
531 | * You must first call pinocchio::forwardKinematics to update placement values in data | ||
532 | * structure. | ||
533 | * | ||
534 | * @note Physically speaking, if the robot were to be cut in two parts at that given frame, this | ||
535 | * supported inertia would represents the inertia of the part that was after the frame. | ||
536 | * with_subtree determines if the childs joints must be taken into consideration (if true) | ||
537 | * or only the current joint (if false). | ||
538 | * | ||
539 | * @note The equivalent function for a joint would be : | ||
540 | * * to read `data.Ycrb[joint_id]`, after having called pinocchio::crba (if with_subtree == | ||
541 | * true). | ||
542 | * * to read `model.inertia[joint_id]` (if with_subtree == false). | ||
543 | * | ||
544 | * @tparam JointCollection Collection of Joint types. | ||
545 | * | ||
546 | * @param[in] model The model structure of the rigid body system. | ||
547 | * @param[in] data The data structure of the rigid body system. | ||
548 | * @param[in] frameId The index of the frame. | ||
549 | * @param[in] with_subtree If false, compute the inertia only inside the frame parent joint if | ||
550 | * false. If true, include child joints inertia. | ||
551 | * | ||
552 | * @return The computed inertia. | ||
553 | * | ||
554 | * @warning forwardKinematics should have been called first | ||
555 | */ | ||
556 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
557 | InertiaTpl<Scalar, Options> computeSupportedInertiaByFrame( | ||
558 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
559 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
560 | const FrameIndex frame_id, | ||
561 | bool with_subtree); | ||
562 | |||
563 | /** | ||
564 | * @brief Computes the force supported by a specific frame (given by frame_id) expressed in the | ||
565 | * LOCAL frame. The supported force corresponds to the sum of all the forces experienced after the | ||
566 | * given frame, i.e : | ||
567 | * * The inertial forces and gravity (applied on the supported inertia in body) | ||
568 | * * The forces applied by child joints | ||
569 | * * (The external forces) | ||
570 | * You must first call pinocchio::rnea to update placements, velocities and efforts values | ||
571 | * in data structure. | ||
572 | * | ||
573 | * @note If an external force is applied to the frame parent joint (during rnea), it won't be | ||
574 | * taken in consideration in this function (it will be considered to be applied before the frame | ||
575 | * in the joint and not after. However external forces applied to child joints will be taken into | ||
576 | * account). | ||
577 | * | ||
578 | * @note Physically speaking, if the robot were to be separated in two parts glued together at | ||
579 | * that given frame, the supported force represents the internal forces applide from the part | ||
580 | * after the cut/frame to the part before. This compute what a force-torque sensor would measures | ||
581 | * if it would be placed at that frame. | ||
582 | * | ||
583 | * @note The equivalent function for a joint would be to read `data.f[joint_id]`, after having | ||
584 | * call pinocchio::rnea. | ||
585 | * | ||
586 | * @tparam JointCollection Collection of Joint types. | ||
587 | * | ||
588 | * @param[in] model The model structure of the rigid body system. | ||
589 | * @param[in] data The data structure of the rigid body system. | ||
590 | * @param[in] frameId The index of the frame. | ||
591 | * | ||
592 | * @return The computed force. | ||
593 | * | ||
594 | * @warning pinocchio::rnea should have been called first | ||
595 | */ | ||
596 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
597 | ForceTpl<Scalar, Options> computeSupportedForceByFrame( | ||
598 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
599 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
600 | const FrameIndex frame_id); | ||
601 | |||
602 | } // namespace pinocchio | ||
603 | |||
604 | /* --- Details -------------------------------------------------------------------- */ | ||
605 | #include "pinocchio/algorithm/frames.hxx" | ||
606 | |||
607 | #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
608 | #include "pinocchio/algorithm/frames.txx" | ||
609 | #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
610 | |||
611 | #endif // ifndef __pinocchio_algorithm_frames_hpp__ | ||
612 |