Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/regressor.hpp |
Date: | 2025-02-12 21:03:38 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 12 | 12 | 100.0% |
Branches: | 6 | 12 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2018-2020 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_regressor_hpp__ | ||
6 | #define __pinocchio_algorithm_regressor_hpp__ | ||
7 | |||
8 | #include "pinocchio/multibody/model.hpp" | ||
9 | #include "pinocchio/multibody/data.hpp" | ||
10 | |||
11 | namespace pinocchio | ||
12 | { | ||
13 | |||
14 | /// | ||
15 | /// \copydoc computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> | ||
16 | /// &,const DataTpl<Scalar,Options,JointCollectionTpl> &, const JointIndex, const ReferenceFrame, | ||
17 | /// const SE3Tpl<Scalar,Options> &) | ||
18 | /// | ||
19 | /// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size | ||
20 | /// 6*(model.njoints-1) initialized to 0. | ||
21 | /// | ||
22 | template< | ||
23 | typename Scalar, | ||
24 | int Options, | ||
25 | template<typename, int> class JointCollectionTpl, | ||
26 | typename Matrix6xReturnType> | ||
27 | void computeJointKinematicRegressor( | ||
28 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
29 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
30 | const JointIndex joint_id, | ||
31 | const ReferenceFrame rf, | ||
32 | const SE3Tpl<Scalar, Options> & placement, | ||
33 | const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor); | ||
34 | |||
35 | /// | ||
36 | /// \brief Computes the kinematic regressor that links the joint placements variations of the | ||
37 | /// whole kinematic tree | ||
38 | /// to the placement variation of the frame rigidly attached to the joint and given by its | ||
39 | /// placement w.r.t. to the joint frame. | ||
40 | /// | ||
41 | /// \remarks It assumes that the \ref forwardKinematics(const | ||
42 | /// ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, | ||
43 | /// const Eigen::MatrixBase<ConfigVectorType> &) has been called first. | ||
44 | /// | ||
45 | /// \param[in] model The model structure of the rigid body system. | ||
46 | /// \param[in] data The data structure of the rigid body system. | ||
47 | /// \param[in] joint_id Index of the joint. | ||
48 | /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or | ||
49 | /// WORLD). \param[in] placement Relative placement to the joint frame. | ||
50 | /// | ||
51 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
52 | 1 | typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x computeJointKinematicRegressor( | |
53 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
54 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
55 | const JointIndex joint_id, | ||
56 | const ReferenceFrame rf, | ||
57 | const SE3Tpl<Scalar, Options> & placement) | ||
58 | { | ||
59 | typedef typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x ReturnType; | ||
60 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | ReturnType res(ReturnType::Zero(6, (model.njoints - 1) * 6)); |
61 | |||
62 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | computeJointKinematicRegressor(model, data, joint_id, rf, placement, res); |
63 | |||
64 | 1 | return res; | |
65 | } | ||
66 | |||
67 | /// | ||
68 | /// \copydoc computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> | ||
69 | /// &,const DataTpl<Scalar,Options,JointCollectionTpl> &, const JointIndex, const ReferenceFrame) | ||
70 | /// | ||
71 | /// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size | ||
72 | /// 6*(model.njoints-1) initialized to 0. | ||
73 | /// | ||
74 | template< | ||
75 | typename Scalar, | ||
76 | int Options, | ||
77 | template<typename, int> class JointCollectionTpl, | ||
78 | typename Matrix6xReturnType> | ||
79 | void computeJointKinematicRegressor( | ||
80 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
81 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
82 | const JointIndex joint_id, | ||
83 | const ReferenceFrame rf, | ||
84 | const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor); | ||
85 | |||
86 | /// | ||
87 | /// \brief Computes the kinematic regressor that links the joint placement variations of the whole | ||
88 | /// kinematic tree | ||
89 | /// to the placement variation of the joint given as input. | ||
90 | /// | ||
91 | /// \remarks It assumes that the \ref forwardKinematics(const | ||
92 | /// ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, | ||
93 | /// const Eigen::MatrixBase<ConfigVectorType> &) has been called first. | ||
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] joint_id Index of the joint. | ||
98 | /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or | ||
99 | /// WORLD). | ||
100 | /// | ||
101 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
102 | 1 | typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x computeJointKinematicRegressor( | |
103 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
104 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
105 | const JointIndex joint_id, | ||
106 | const ReferenceFrame rf) | ||
107 | { | ||
108 | typedef typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x ReturnType; | ||
109 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | ReturnType res(ReturnType::Zero(6, (model.njoints - 1) * 6)); |
110 | |||
111 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | computeJointKinematicRegressor(model, data, joint_id, rf, res); |
112 | |||
113 | 1 | return res; | |
114 | } | ||
115 | |||
116 | /// | ||
117 | /// \copydoc computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> &, | ||
118 | /// DataTpl<Scalar,Options,JointCollectionTpl> &, const FrameIndex, const ReferenceFrame) | ||
119 | /// | ||
120 | /// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size | ||
121 | /// 6*(model.njoints-1) initialized to 0. | ||
122 | /// | ||
123 | template< | ||
124 | typename Scalar, | ||
125 | int Options, | ||
126 | template<typename, int> class JointCollectionTpl, | ||
127 | typename Matrix6xReturnType> | ||
128 | void computeFrameKinematicRegressor( | ||
129 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
130 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
131 | const FrameIndex frame_id, | ||
132 | const ReferenceFrame rf, | ||
133 | const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor); | ||
134 | |||
135 | /// | ||
136 | /// \brief Computes the kinematic regressor that links the joint placement variations of the whole | ||
137 | /// kinematic tree | ||
138 | /// to the placement variation of the frame given as input. | ||
139 | /// | ||
140 | /// \remarks It assumes that the \ref framesForwardKinematics(const | ||
141 | /// ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, | ||
142 | /// const Eigen::MatrixBase<ConfigVectorType> &) has been called first. | ||
143 | /// | ||
144 | /// \param[in] model The model structure of the rigid body system. | ||
145 | /// \param[in] data The data structure of the rigid body system. | ||
146 | /// \param[in] frame_id Index of the frame. | ||
147 | /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or | ||
148 | /// WORLD). | ||
149 | /// | ||
150 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
151 | 1 | typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x computeFrameKinematicRegressor( | |
152 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
153 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
154 | const FrameIndex frame_id, | ||
155 | const ReferenceFrame rf) | ||
156 | { | ||
157 | typedef typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x ReturnType; | ||
158 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | ReturnType res(ReturnType::Zero(6, (model.njoints - 1) * 6)); |
159 | |||
160 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | computeFrameKinematicRegressor(model, data, frame_id, rf, res); |
161 | |||
162 | 1 | return res; | |
163 | } | ||
164 | |||
165 | /// | ||
166 | /// \brief Computes the static regressor that links the center of mass positions of all the links | ||
167 | /// to the center of mass of the complete model according to the current configuration of | ||
168 | /// the robot. | ||
169 | /// | ||
170 | /// The result is stored in `data.staticRegressor` and it corresponds to a matrix \f$ Y \f$ such | ||
171 | /// that \f$ c = Y(q,\dot{q},\ddot{q})\tilde{\pi} \f$ where \f$ \tilde{\pi} = | ||
172 | /// (\tilde{\pi}_1^T\ \dots\ \tilde{\pi}_n^T)^T \f$ and \f$ \tilde{\pi}_i = | ||
173 | /// \text{model.inertias[i].toDynamicParameters().head<4>()} \f$ | ||
174 | /// | ||
175 | /// \tparam JointCollection Collection of Joint types. | ||
176 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
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 | /// | ||
182 | /// \return The static regressor of the system. | ||
183 | /// | ||
184 | template< | ||
185 | typename Scalar, | ||
186 | int Options, | ||
187 | template<typename, int> class JointCollectionTpl, | ||
188 | typename ConfigVectorType> | ||
189 | inline typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & computeStaticRegressor( | ||
190 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
191 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
192 | const Eigen::MatrixBase<ConfigVectorType> & q); | ||
193 | |||
194 | /// | ||
195 | /// \brief Computes the regressor for the dynamic parameters of a single rigid body. | ||
196 | /// | ||
197 | /// The result is such that | ||
198 | /// \f$ I a + v \times I v = bodyRegressor(v,a) * I.toDynamicParameters() \f$ | ||
199 | /// | ||
200 | /// \param[in] v Velocity of the rigid body | ||
201 | /// \param[in] a Acceleration of the rigid body | ||
202 | /// \param[out] regressor The resulting regressor of the body. | ||
203 | /// | ||
204 | template<typename MotionVelocity, typename MotionAcceleration, typename OutputType> | ||
205 | inline void bodyRegressor( | ||
206 | const MotionDense<MotionVelocity> & v, | ||
207 | const MotionDense<MotionAcceleration> & a, | ||
208 | const Eigen::MatrixBase<OutputType> & regressor); | ||
209 | |||
210 | /// | ||
211 | /// \brief Computes the regressor for the dynamic parameters of a single rigid body. | ||
212 | /// | ||
213 | /// The result is such that | ||
214 | /// \f$ I a + v \times I v = bodyRegressor(v,a) * I.toDynamicParameters() \f$ | ||
215 | /// | ||
216 | /// \param[in] v Velocity of the rigid body | ||
217 | /// \param[in] a Acceleration of the rigid body | ||
218 | /// | ||
219 | /// \return The regressor of the body. | ||
220 | /// | ||
221 | template<typename MotionVelocity, typename MotionAcceleration> | ||
222 | inline Eigen::Matrix< | ||
223 | typename MotionVelocity::Scalar, | ||
224 | 6, | ||
225 | 10, | ||
226 | PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options> | ||
227 | bodyRegressor(const MotionDense<MotionVelocity> & v, const MotionDense<MotionAcceleration> & a); | ||
228 | |||
229 | /// | ||
230 | /// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given | ||
231 | /// joint, | ||
232 | /// puts the result in data.bodyRegressor and returns it. | ||
233 | /// | ||
234 | /// This algorithm assumes RNEA has been run to compute the acceleration and gravitational | ||
235 | /// effects. | ||
236 | /// | ||
237 | /// The result is such that | ||
238 | /// \f$ f = \text{jointBodyRegressor(model,data,joint_id) * I.toDynamicParameters()} \f$ | ||
239 | /// where \f$ f \f$ is the net force acting on the body, including gravity | ||
240 | /// | ||
241 | /// \param[in] model The model structure of the rigid body system. | ||
242 | /// \param[in] data The data structure of the rigid body system. | ||
243 | /// \param[in] joint_id The id of the joint. | ||
244 | /// | ||
245 | /// \return The regressor of the body. | ||
246 | /// | ||
247 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
248 | inline typename DataTpl<Scalar, Options, JointCollectionTpl>::BodyRegressorType & | ||
249 | jointBodyRegressor( | ||
250 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
251 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
252 | JointIndex joint_id); | ||
253 | |||
254 | /// | ||
255 | /// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given | ||
256 | /// frame, | ||
257 | /// puts the result in data.bodyRegressor and returns it. | ||
258 | /// | ||
259 | /// This algorithm assumes RNEA has been run to compute the acceleration and gravitational | ||
260 | /// effects. | ||
261 | /// | ||
262 | /// The result is such that | ||
263 | /// \f$ f = \text{frameBodyRegressor(model,data,frame_id) * I.toDynamicParameters()} \f$ | ||
264 | /// where \f$ f \f$ is the net force acting on the body, including gravity | ||
265 | /// | ||
266 | /// \param[in] model The model structure of the rigid body system. | ||
267 | /// \param[in] data The data structure of the rigid body system. | ||
268 | /// \param[in] frame_id The id of the frame. | ||
269 | /// | ||
270 | /// \return The dynamic regressor of the body. | ||
271 | /// | ||
272 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
273 | inline typename DataTpl<Scalar, Options, JointCollectionTpl>::BodyRegressorType & | ||
274 | frameBodyRegressor( | ||
275 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
276 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
277 | FrameIndex frame_id); | ||
278 | |||
279 | /// | ||
280 | /// \brief Computes the joint torque regressor that links the joint torque | ||
281 | /// to the dynamic parameters of each link according to the current the robot motion. | ||
282 | /// | ||
283 | /// The result is stored in `data.jointTorqueRegressor` and it corresponds to a matrix \f$ Y \f$ | ||
284 | /// such that \f$ \tau = Y(q,\dot{q},\ddot{q})\pi \f$ where \f$ \pi = (\pi_1^T\ \dots\ \pi_n^T)^T | ||
285 | /// \f$ and \f$ \pi_i = \text{model.inertias[i].toDynamicParameters()} \f$ | ||
286 | /// | ||
287 | /// \tparam JointCollection Collection of Joint types. | ||
288 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
289 | /// \tparam TangentVectorType1 Type of the joint velocity vector. | ||
290 | /// \tparam TangentVectorType2 Type of the joint acceleration vector. | ||
291 | /// | ||
292 | /// \param[in] model The model structure of the rigid body system. | ||
293 | /// \param[in] data The data structure of the rigid body system. | ||
294 | /// \param[in] q The joint configuration vector (dim model.nq). | ||
295 | /// \param[in] v The joint velocity vector (dim model.nv). | ||
296 | /// \param[in] a The joint acceleration vector (dim model.nv). | ||
297 | /// | ||
298 | /// \return The joint torque regressor of the system. | ||
299 | /// | ||
300 | /// \warning This function writes temporary information in data.bodyRegressor. This means if you | ||
301 | /// have valuable data in it it will be overwritten. | ||
302 | /// | ||
303 | template< | ||
304 | typename Scalar, | ||
305 | int Options, | ||
306 | template<typename, int> class JointCollectionTpl, | ||
307 | typename ConfigVectorType, | ||
308 | typename TangentVectorType1, | ||
309 | typename TangentVectorType2> | ||
310 | inline typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & | ||
311 | computeJointTorqueRegressor( | ||
312 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
313 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
314 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
315 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
316 | const Eigen::MatrixBase<TangentVectorType2> & a); | ||
317 | |||
318 | /// | ||
319 | /// \brief Computes the kinetic energy regressor that links the kinetic energy of the system to | ||
320 | /// the dynamic parameters of each link according to the current robot motion. | ||
321 | /// | ||
322 | /// The result is stored in `data.kineticEnergyRegressor` and it corresponds to a matrix | ||
323 | /// \f$ Y_e \f$ such that \f$ K = Y_e(q,\dot{q})\pi \f$ where | ||
324 | /// \pi_i = \text{model.inertias[i].toDynamicParameters()} \f$ | ||
325 | /// | ||
326 | /// \tparam JointCollection Collection of Joint types. | ||
327 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
328 | /// \tparam TangentVectorType Type of the joint velocity vector. | ||
329 | /// | ||
330 | /// \param[in] model The model structure of the rigid body system. | ||
331 | /// \param[in] data The data structure of the rigid body system. | ||
332 | /// \param[in] q The joint configuration vector (dim model.nq). | ||
333 | /// \param[in] v The joint velocity vector (dim model.nv). | ||
334 | /// | ||
335 | /// \return The kinetic energy regressor of the system. | ||
336 | template< | ||
337 | typename Scalar, | ||
338 | int Options, | ||
339 | template<typename, int> class JointCollectionTpl, | ||
340 | typename ConfigVectorType, | ||
341 | typename TangentVectorType> | ||
342 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowVectorXs & | ||
343 | computeKineticEnergyRegressor( | ||
344 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
345 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
346 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
347 | const Eigen::MatrixBase<TangentVectorType> & v); | ||
348 | |||
349 | /// | ||
350 | /// \brief Computes the potential energy regressor that links the potential energy | ||
351 | /// of the system to the dynamic parameters of each link according to the current robot motion. | ||
352 | /// | ||
353 | /// The result is stored in `data.potentialEnergyRegressor` and it corresponds to a matrix | ||
354 | /// \f$ Y_p \f$ such that \f$ P = Y_p(q)\pi \f$ where | ||
355 | /// \pi_i = \text{model.inertias[i].toDynamicParameters()} \f$ | ||
356 | /// | ||
357 | /// \tparam JointCollection Collection of Joint types. | ||
358 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
359 | /// | ||
360 | /// \param[in] model The model structure of the rigid body system. | ||
361 | /// \param[in] data The data structure of the rigid body system. | ||
362 | /// \param[in] q The joint configuration vector (dim model.nq). | ||
363 | /// | ||
364 | /// \return The kinetic energy regressor of the system. | ||
365 | template< | ||
366 | typename Scalar, | ||
367 | int Options, | ||
368 | template<typename, int> class JointCollectionTpl, | ||
369 | typename ConfigVectorType> | ||
370 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowVectorXs & | ||
371 | computePotentialEnergyRegressor( | ||
372 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
373 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
374 | const Eigen::MatrixBase<ConfigVectorType> & q); | ||
375 | } // namespace pinocchio | ||
376 | |||
377 | /* --- Details -------------------------------------------------------------------- */ | ||
378 | #include "pinocchio/algorithm/regressor.hxx" | ||
379 | |||
380 | #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
381 | #include "pinocchio/algorithm/regressor.txx" | ||
382 | #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
383 | |||
384 | #endif // ifndef __pinocchio_algorithm_regressor_hpp__ | ||
385 |