GCC Code Coverage Report


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