Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/frames-derivatives.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 43 | 43 | 100.0% |
Branches: | 6 | 18 | 33.3% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2020 INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_frames_derivatives_hpp__ | ||
6 | #define __pinocchio_algorithm_frames_derivatives_hpp__ | ||
7 | |||
8 | #include "pinocchio/multibody/model.hpp" | ||
9 | #include "pinocchio/multibody/data.hpp" | ||
10 | |||
11 | namespace pinocchio | ||
12 | { | ||
13 | |||
14 | /** | ||
15 | * @brief Computes the partial derivatives of the spatial velocity of a frame given by its | ||
16 | * relative placement, with respect to q and v. You must first call | ||
17 | * pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. | ||
18 | * | ||
19 | * @tparam JointCollection Collection of Joint types. | ||
20 | * @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity | ||
21 | * with respect to the joint configuration vector. | ||
22 | * @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial velocity | ||
23 | * with respect to the joint velocity vector. | ||
24 | * | ||
25 | * @param[in] model The kinematic model | ||
26 | * @param[in] data Data associated to model | ||
27 | * @param[in] joint_id Index of the supporting joint | ||
28 | * @param[in] placement Placement of the Frame w.r.t. the joint frame. | ||
29 | * @param[in] rf Reference frame in which the velocity is expressed. | ||
30 | * @param[out] v_partial_dq Partial derivative of the frame spatial velocity w.r.t. \f$ q \f$. | ||
31 | * @param[out] v_partial_dv Partial derivative of the frame spatial velociy w.r.t. \f$ v \f$. | ||
32 | * | ||
33 | */ | ||
34 | template< | ||
35 | typename Scalar, | ||
36 | int Options, | ||
37 | template<typename, int> | ||
38 | class JointCollectionTpl, | ||
39 | typename Matrix6xOut1, | ||
40 | typename Matrix6xOut2> | ||
41 | void getFrameVelocityDerivatives( | ||
42 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
43 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
44 | const JointIndex joint_id, | ||
45 | const SE3Tpl<Scalar, Options> & placement, | ||
46 | const ReferenceFrame rf, | ||
47 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
48 | const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv); | ||
49 | |||
50 | /** | ||
51 | * @brief Computes the partial derivatives of the frame spatial velocity with respect to q | ||
52 | * and v. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the | ||
53 | * required quantities. | ||
54 | * | ||
55 | * @tparam JointCollection Collection of Joint types. | ||
56 | * @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity | ||
57 | * with respect to the joint configuration vector. | ||
58 | * @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial velocity | ||
59 | * with respect to the joint velocity vector. | ||
60 | * | ||
61 | * @param[in] model The kinematic model | ||
62 | * @param[in] data Data associated to model | ||
63 | * @param[in] frame_id Id of the operational Frame | ||
64 | * @param[in] rf Reference frame in which the velocity is expressed. | ||
65 | * @param[out] v_partial_dq Partial derivative of the frame spatial velocity w.r.t. \f$ q \f$. | ||
66 | * @param[out] v_partial_dv Partial derivative of the frame spatial velociy w.r.t. \f$ v \f$. | ||
67 | * | ||
68 | */ | ||
69 | template< | ||
70 | typename Scalar, | ||
71 | int Options, | ||
72 | template<typename, int> | ||
73 | class JointCollectionTpl, | ||
74 | typename Matrix6xOut1, | ||
75 | typename Matrix6xOut2> | ||
76 | 6 | void getFrameVelocityDerivatives( | |
77 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
78 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
79 | const FrameIndex frame_id, | ||
80 | const ReferenceFrame rf, | ||
81 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
82 | const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv) | ||
83 | { | ||
84 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
6 | PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid."); |
85 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
86 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
87 | typedef typename Model::Frame Frame; | ||
88 | |||
89 | 6 | const Frame & frame = model.frames[frame_id]; | |
90 | 6 | typename Data::SE3 & oMframe = data.oMf[frame_id]; | |
91 |
1/2✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
6 | oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility |
92 | 6 | getFrameVelocityDerivatives( | |
93 | 6 | model, data, frame.parentJoint, frame.placement, rf, | |
94 | 6 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq), | |
95 | 6 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dv)); | |
96 | 6 | } | |
97 | |||
98 | /** | ||
99 | * @brief Computes the partial derivatives of the spatial acceleration of a frame given by | ||
100 | * its relative placement, with respect to q, v and a. You must first call | ||
101 | * pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. It is | ||
102 | * important to notice that a direct outcome (for free) of this algo is v_partial_dq and | ||
103 | * v_partial_dv which is equal to a_partial_da. | ||
104 | * | ||
105 | * @tparam JointCollection Collection of Joint types. | ||
106 | * @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity | ||
107 | * with respect to the joint configuration vector. | ||
108 | * @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial | ||
109 | * acceleration with respect to the joint configuration vector. | ||
110 | * @tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the frame spatial | ||
111 | * acceleration with respect to the joint velocity vector. | ||
112 | * @tparam Matrix6xOut4 Matrix6x containing the partial derivatives of the frame spatial | ||
113 | * acceleration with respect to the joint acceleration vector. | ||
114 | * | ||
115 | * @param[in] model The kinematic model | ||
116 | * @param[in] data Data associated to model | ||
117 | * @param[in] joint_id Index of the supporting joint | ||
118 | * @param[in] placement Placement of the Frame w.r.t. the joint frame. | ||
119 | * @param[in] rf Reference frame in which the velocity is expressed. | ||
120 | * @param[out] v_partial_dq Partial derivative of the frame spatial velocity w.r.t. \f$ q \f$. | ||
121 | * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ q | ||
122 | * \f$. | ||
123 | * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ v | ||
124 | * \f$. | ||
125 | * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ | ||
126 | * \dot{v} \f$. | ||
127 | * | ||
128 | */ | ||
129 | template< | ||
130 | typename Scalar, | ||
131 | int Options, | ||
132 | template<typename, int> | ||
133 | class JointCollectionTpl, | ||
134 | typename Matrix6xOut1, | ||
135 | typename Matrix6xOut2, | ||
136 | typename Matrix6xOut3, | ||
137 | typename Matrix6xOut4> | ||
138 | void getFrameAccelerationDerivatives( | ||
139 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
140 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
141 | const JointIndex joint_id, | ||
142 | const SE3Tpl<Scalar, Options> & placement, | ||
143 | const ReferenceFrame rf, | ||
144 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
145 | const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq, | ||
146 | const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv, | ||
147 | const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da); | ||
148 | |||
149 | /** | ||
150 | * @brief Computes the partial derivatives of the frame acceleration quantity with respect to | ||
151 | * q, v and a. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all | ||
152 | * the required quantities. It is important to notice that a direct outcome (for free) of this | ||
153 | * algo is v_partial_dq and v_partial_dv which is equal to a_partial_da. | ||
154 | * | ||
155 | * @tparam JointCollection Collection of Joint types. | ||
156 | * @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity | ||
157 | * with respect to the joint configuration vector. | ||
158 | * @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial | ||
159 | * acceleration with respect to the joint configuration vector. | ||
160 | * @tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the frame spatial | ||
161 | * acceleration with respect to the joint velocity vector. | ||
162 | * @tparam Matrix6xOut4 Matrix6x containing the partial derivatives of the frame spatial | ||
163 | * acceleration with respect to the joint acceleration vector. | ||
164 | * | ||
165 | * @param[in] model The kinematic model | ||
166 | * @param[in] data Data associated to model | ||
167 | * @param[in] frame_id Id of the operational Frame | ||
168 | * @param[in] rf Reference frame in which the velocity is expressed. | ||
169 | * @param[out] v_partial_dq Partial derivative of the frame spatial velocity w.r.t. \f$ q \f$. | ||
170 | * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ q \f$. | ||
171 | * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ v \f$. | ||
172 | * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ | ||
173 | * \dot{v} \f$. | ||
174 | * | ||
175 | */ | ||
176 | template< | ||
177 | typename Scalar, | ||
178 | int Options, | ||
179 | template<typename, int> | ||
180 | class JointCollectionTpl, | ||
181 | typename Matrix6xOut1, | ||
182 | typename Matrix6xOut2, | ||
183 | typename Matrix6xOut3, | ||
184 | typename Matrix6xOut4> | ||
185 | 6 | void getFrameAccelerationDerivatives( | |
186 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
187 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
188 | const FrameIndex frame_id, | ||
189 | const ReferenceFrame rf, | ||
190 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
191 | const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq, | ||
192 | const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv, | ||
193 | const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da) | ||
194 | { | ||
195 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
6 | PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid."); |
196 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
197 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
198 | typedef typename Model::Frame Frame; | ||
199 | |||
200 | 6 | const Frame & frame = model.frames[frame_id]; | |
201 | 6 | typename Data::SE3 & oMframe = data.oMf[frame_id]; | |
202 |
1/2✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
6 | oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility |
203 | 6 | getFrameAccelerationDerivatives( | |
204 | 6 | model, data, frame.parentJoint, frame.placement, rf, | |
205 | 6 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq), | |
206 | 6 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, a_partial_dq), | |
207 | 6 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dv), | |
208 | 6 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da)); | |
209 | 6 | } | |
210 | |||
211 | /** | ||
212 | * @brief Computes the partial derivatives of the frame acceleration quantity with respect to | ||
213 | * q, v and a. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all | ||
214 | * the required quantities. It is important to notice that a direct outcome (for free) of this | ||
215 | * algo is v_partial_dq and v_partial_dv which is equal to a_partial_da. | ||
216 | * | ||
217 | * @tparam JointCollection Collection of Joint types. | ||
218 | * @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity | ||
219 | * with respect to the joint configuration vector. | ||
220 | * @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial velocity | ||
221 | * with respect to the joint velocity vector. | ||
222 | * @tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the frame spatial | ||
223 | * acceleration with respect to the joint configuration vector. | ||
224 | * @tparam Matrix6xOut4 Matrix6x containing the partial derivatives of the frame spatial | ||
225 | * acceleration with respect to the joint velocity vector. | ||
226 | * @tparam Matrix6xOut5 Matrix6x containing the partial derivatives of the frame spatial | ||
227 | * acceleration with respect to the joint acceleration vector. | ||
228 | * | ||
229 | * @param[in] model The kinematic model | ||
230 | * @param[in] data Data associated to model | ||
231 | * @param[in] joint_id Index of the supporting joint | ||
232 | * @param[in] placement Placement of the Frame w.r.t. the joint frame. | ||
233 | * @param[in] rf Reference frame in which the velocity is expressed. | ||
234 | * @param[out] v_partial_dq Partial derivative of the frame spatial velocity w.r.t. \f$ q \f$. | ||
235 | * @param[out] v_partial_dv Partial derivative of the frame spatial velociy w.r.t. \f$ v \f$. | ||
236 | * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ q | ||
237 | * \f$. | ||
238 | * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ v | ||
239 | * \f$. | ||
240 | * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ | ||
241 | * \dot{v} \f$. | ||
242 | * | ||
243 | */ | ||
244 | template< | ||
245 | typename Scalar, | ||
246 | int Options, | ||
247 | template<typename, int> | ||
248 | class JointCollectionTpl, | ||
249 | typename Matrix6xOut1, | ||
250 | typename Matrix6xOut2, | ||
251 | typename Matrix6xOut3, | ||
252 | typename Matrix6xOut4, | ||
253 | typename Matrix6xOut5> | ||
254 | 3 | void getFrameAccelerationDerivatives( | |
255 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
256 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
257 | const JointIndex joint_id, | ||
258 | const SE3Tpl<Scalar, Options> & placement, | ||
259 | const ReferenceFrame rf, | ||
260 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
261 | const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv, | ||
262 | const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq, | ||
263 | const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv, | ||
264 | const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da) | ||
265 | { | ||
266 | 3 | getFrameAccelerationDerivatives( | |
267 | 3 | model, data, joint_id, placement, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq), | |
268 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq), | |
269 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv), | |
270 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da)); | |
271 | |||
272 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv) = a_partial_da; | |
273 | 3 | } | |
274 | |||
275 | /** | ||
276 | * @brief Computes the partial derivatives of the frame acceleration quantity with respect to | ||
277 | * q, v and a. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all | ||
278 | * the required quantities. It is important to notice that a direct outcome (for free) of this | ||
279 | * algo is v_partial_dq and v_partial_dv which is equal to a_partial_da. | ||
280 | * | ||
281 | * @tparam JointCollection Collection of Joint types. | ||
282 | * @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity | ||
283 | * with respect to the joint configuration vector. | ||
284 | * @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial velocity | ||
285 | * with respect to the joint velocity vector. | ||
286 | * @tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the frame spatial | ||
287 | * acceleration with respect to the joint configuration vector. | ||
288 | * @tparam Matrix6xOut4 Matrix6x containing the partial derivatives of the frame spatial | ||
289 | * acceleration with respect to the joint velocity vector. | ||
290 | * @tparam Matrix6xOut5 Matrix6x containing the partial derivatives of the frame spatial | ||
291 | * acceleration with respect to the joint acceleration vector. | ||
292 | * | ||
293 | * @param[in] model The kinematic model | ||
294 | * @param[in] data Data associated to model | ||
295 | * @param[in] frame_id Id of the operational Frame | ||
296 | * @param[in] rf Reference frame in which the velocity is expressed. | ||
297 | * @param[out] v_partial_dq Partial derivative of the frame spatial velocity w.r.t. \f$ q \f$. | ||
298 | * @param[out] v_partial_dv Partial derivative of the frame spatial velociy w.r.t. \f$ v \f$. | ||
299 | * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ q \f$. | ||
300 | * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ v \f$. | ||
301 | * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ | ||
302 | * \dot{v} \f$. | ||
303 | * | ||
304 | */ | ||
305 | template< | ||
306 | typename Scalar, | ||
307 | int Options, | ||
308 | template<typename, int> | ||
309 | class JointCollectionTpl, | ||
310 | typename Matrix6xOut1, | ||
311 | typename Matrix6xOut2, | ||
312 | typename Matrix6xOut3, | ||
313 | typename Matrix6xOut4, | ||
314 | typename Matrix6xOut5> | ||
315 | 3 | void getFrameAccelerationDerivatives( | |
316 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
317 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
318 | const FrameIndex frame_id, | ||
319 | const ReferenceFrame rf, | ||
320 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
321 | const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv, | ||
322 | const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq, | ||
323 | const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv, | ||
324 | const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da) | ||
325 | { | ||
326 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
3 | PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid."); |
327 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
328 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
329 | typedef typename Model::Frame Frame; | ||
330 | |||
331 | 3 | const Frame & frame = model.frames[frame_id]; | |
332 | 3 | typename Data::SE3 & oMframe = data.oMf[frame_id]; | |
333 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility |
334 | 3 | getFrameAccelerationDerivatives( | |
335 | 3 | model, data, frame.parentJoint, frame.placement, rf, | |
336 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq), | |
337 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv), | |
338 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq), | |
339 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv), | |
340 | 3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da)); | |
341 | 3 | } | |
342 | } // namespace pinocchio | ||
343 | |||
344 | #include "pinocchio/algorithm/frames-derivatives.hxx" | ||
345 | |||
346 | #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
347 | #include "pinocchio/algorithm/frames-derivatives.txx" | ||
348 | #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
349 | |||
350 | #endif // ifndef __pinocchio_algorithm_frames_derivatives_hpp__ | ||
351 |