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> &,const DataTpl<Scalar,Options,JointCollectionTpl> &, const JointIndex, const ReferenceFrame, const SE3Tpl<Scalar,Options> &) |
16 |
|
|
/// |
17 |
|
|
/// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0. |
18 |
|
|
/// |
19 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType> |
20 |
|
|
void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
21 |
|
|
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
22 |
|
|
const JointIndex joint_id, |
23 |
|
|
const ReferenceFrame rf, |
24 |
|
|
const SE3Tpl<Scalar,Options> & placement, |
25 |
|
|
const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor); |
26 |
|
|
|
27 |
|
|
|
28 |
|
|
/// |
29 |
|
|
/// \brief Computes the kinematic regressor that links the joint placements variations of the whole kinematic tree |
30 |
|
|
/// to the placement variation of the frame rigidly attached to the joint and given by its placement w.r.t. to the joint frame. |
31 |
|
|
/// |
32 |
|
|
/// \remarks It assumes that the \ref forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const Eigen::MatrixBase<ConfigVectorType> &) has been called first. |
33 |
|
|
/// |
34 |
|
|
/// \param[in] model The model structure of the rigid body system. |
35 |
|
|
/// \param[in] data The data structure of the rigid body system. |
36 |
|
|
/// \param[in] joint_id Index of the joint. |
37 |
|
|
/// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD). |
38 |
|
|
/// \param[in] placement Relative placement to the joint frame. |
39 |
|
|
/// |
40 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
41 |
|
|
typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x |
42 |
|
|
computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
43 |
|
|
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
44 |
|
|
const JointIndex joint_id, |
45 |
|
|
const ReferenceFrame rf, |
46 |
|
|
const SE3Tpl<Scalar,Options> & placement) |
47 |
|
|
{ |
48 |
|
|
typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType; |
49 |
|
|
ReturnType res(ReturnType::Zero(6,(model.njoints-1)*6)); |
50 |
|
|
|
51 |
|
|
computeJointKinematicRegressor(model,data,joint_id,rf,placement,res); |
52 |
|
|
|
53 |
|
|
return res; |
54 |
|
|
} |
55 |
|
|
|
56 |
|
|
/// |
57 |
|
|
/// \copydoc computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> &,const DataTpl<Scalar,Options,JointCollectionTpl> &, const JointIndex, const ReferenceFrame) |
58 |
|
|
/// |
59 |
|
|
/// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0. |
60 |
|
|
/// |
61 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType> |
62 |
|
|
void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
63 |
|
|
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
64 |
|
|
const JointIndex joint_id, |
65 |
|
|
const ReferenceFrame rf, |
66 |
|
|
const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor); |
67 |
|
|
|
68 |
|
|
/// |
69 |
|
|
/// \brief Computes the kinematic regressor that links the joint placement variations of the whole kinematic tree |
70 |
|
|
/// to the placement variation of the joint given as input. |
71 |
|
|
/// |
72 |
|
|
/// \remarks It assumes that the \ref forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const Eigen::MatrixBase<ConfigVectorType> &) has been called first. |
73 |
|
|
/// |
74 |
|
|
/// \param[in] model The model structure of the rigid body system. |
75 |
|
|
/// \param[in] data The data structure of the rigid body system. |
76 |
|
|
/// \param[in] joint_id Index of the joint. |
77 |
|
|
/// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD). |
78 |
|
|
/// |
79 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
80 |
|
|
typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x |
81 |
|
|
computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
82 |
|
|
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
83 |
|
|
const JointIndex joint_id, |
84 |
|
|
const ReferenceFrame rf) |
85 |
|
|
{ |
86 |
|
|
typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType; |
87 |
|
|
ReturnType res(ReturnType::Zero(6,(model.njoints-1)*6)); |
88 |
|
|
|
89 |
|
|
computeJointKinematicRegressor(model,data,joint_id,rf,res); |
90 |
|
|
|
91 |
|
|
return res; |
92 |
|
|
} |
93 |
|
|
|
94 |
|
|
/// |
95 |
|
|
/// \copydoc computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const FrameIndex, const ReferenceFrame) |
96 |
|
|
/// |
97 |
|
|
/// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0. |
98 |
|
|
/// |
99 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType> |
100 |
|
|
void computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
101 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
102 |
|
|
const FrameIndex frame_id, |
103 |
|
|
const ReferenceFrame rf, |
104 |
|
|
const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor); |
105 |
|
|
|
106 |
|
|
/// |
107 |
|
|
/// \brief Computes the kinematic regressor that links the joint placement variations of the whole kinematic tree |
108 |
|
|
/// to the placement variation of the frame given as input. |
109 |
|
|
/// |
110 |
|
|
/// \remarks It assumes that the \ref framesForwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const Eigen::MatrixBase<ConfigVectorType> &) has been called first. |
111 |
|
|
/// |
112 |
|
|
/// \param[in] model The model structure of the rigid body system. |
113 |
|
|
/// \param[in] data The data structure of the rigid body system. |
114 |
|
|
/// \param[in] frame_id Index of the frame. |
115 |
|
|
/// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD). |
116 |
|
|
/// |
117 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
118 |
|
|
typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x |
119 |
|
|
computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
120 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
121 |
|
|
const FrameIndex frame_id, |
122 |
|
|
const ReferenceFrame rf) |
123 |
|
|
{ |
124 |
|
|
typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType; |
125 |
|
|
ReturnType res(ReturnType::Zero(6,(model.njoints-1)*6)); |
126 |
|
|
|
127 |
|
|
computeFrameKinematicRegressor(model,data,frame_id,rf,res); |
128 |
|
|
|
129 |
|
|
return res; |
130 |
|
|
} |
131 |
|
|
|
132 |
|
|
/// |
133 |
|
|
/// \brief Computes the static regressor that links the center of mass positions of all the links |
134 |
|
|
/// to the center of mass of the complete model according to the current configuration of the robot. |
135 |
|
|
/// |
136 |
|
|
/// The result is stored in `data.staticRegressor` and it corresponds to a matrix \f$ Y \f$ such that |
137 |
|
|
/// \f$ c = Y(q,\dot{q},\ddot{q})\tilde{\pi} \f$ |
138 |
|
|
/// where \f$ \tilde{\pi} = (\tilde{\pi}_1^T\ \dots\ \tilde{\pi}_n^T)^T \f$ and |
139 |
|
|
/// \f$ \tilde{\pi}_i = \text{model.inertias[i].toDynamicParameters().head<4>()} \f$ |
140 |
|
|
/// |
141 |
|
|
/// \tparam JointCollection Collection of Joint types. |
142 |
|
|
/// \tparam ConfigVectorType Type of the joint configuration vector. |
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] q The joint configuration vector (dim model.nq). |
147 |
|
|
/// |
148 |
|
|
/// \return The static regressor of the system. |
149 |
|
|
/// |
150 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
151 |
|
|
inline typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x & |
152 |
|
|
computeStaticRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
153 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
154 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q); |
155 |
|
|
|
156 |
|
|
namespace regressor |
157 |
|
|
{ |
158 |
|
|
|
159 |
|
|
/// |
160 |
|
|
/// \brief Computes the static regressor that links the center of mass positions of all the links |
161 |
|
|
/// to the center of mass of the complete model according to the current configuration of the robot. |
162 |
|
|
/// |
163 |
|
|
/// \tparam JointCollection Collection of Joint types. |
164 |
|
|
/// \tparam ConfigVectorType Type of the joint configuration vector. |
165 |
|
|
/// |
166 |
|
|
/// \param[in] model The model structure of the rigid body system. |
167 |
|
|
/// \param[in] data The data structure of the rigid body system. |
168 |
|
|
/// \param[in] q The joint configuration vector (dim model.nq). |
169 |
|
|
/// |
170 |
|
|
/// \return The static regressor of the system. |
171 |
|
|
/// |
172 |
|
|
/// \deprecated This function is now in the main pinocchio namespace |
173 |
|
|
/// |
174 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
175 |
|
|
PINOCCHIO_DEPRECATED typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x & |
176 |
|
|
computeStaticRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
177 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
178 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q) |
179 |
|
|
{ |
180 |
|
|
return ::pinocchio::computeStaticRegressor(model,data,q); |
181 |
|
|
} |
182 |
|
|
} |
183 |
|
|
|
184 |
|
|
/// |
185 |
|
|
/// \brief Computes the regressor for the dynamic parameters of a single rigid body. |
186 |
|
|
/// |
187 |
|
|
/// The result is such that |
188 |
|
|
/// \f$ I a + v \times I v = bodyRegressor(v,a) * I.toDynamicParameters() \f$ |
189 |
|
|
/// |
190 |
|
|
/// \param[in] v Velocity of the rigid body |
191 |
|
|
/// \param[in] a Acceleration of the rigid body |
192 |
|
|
/// \param[out] regressor The resulting regressor of the body. |
193 |
|
|
/// |
194 |
|
|
template<typename MotionVelocity, typename MotionAcceleration, typename OutputType> |
195 |
|
|
inline void |
196 |
|
|
bodyRegressor(const MotionDense<MotionVelocity> & v, |
197 |
|
|
const MotionDense<MotionAcceleration> & a, |
198 |
|
|
const Eigen::MatrixBase<OutputType> & regressor); |
199 |
|
|
|
200 |
|
|
/// |
201 |
|
|
/// \brief Computes the regressor for the dynamic parameters of a single rigid body. |
202 |
|
|
/// |
203 |
|
|
/// The result is such that |
204 |
|
|
/// \f$ I a + v \times I v = bodyRegressor(v,a) * I.toDynamicParameters() \f$ |
205 |
|
|
/// |
206 |
|
|
/// \param[in] v Velocity of the rigid body |
207 |
|
|
/// \param[in] a Acceleration of the rigid body |
208 |
|
|
/// |
209 |
|
|
/// \return The regressor of the body. |
210 |
|
|
/// |
211 |
|
|
template<typename MotionVelocity, typename MotionAcceleration> |
212 |
|
|
inline Eigen::Matrix<typename MotionVelocity::Scalar,6,10,PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options> |
213 |
|
|
bodyRegressor(const MotionDense<MotionVelocity> & v, |
214 |
|
|
const MotionDense<MotionAcceleration> & a); |
215 |
|
|
|
216 |
|
|
/// |
217 |
|
|
/// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given joint, |
218 |
|
|
/// puts the result in data.bodyRegressor and returns it. |
219 |
|
|
/// |
220 |
|
|
/// This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects. |
221 |
|
|
/// |
222 |
|
|
/// The result is such that |
223 |
|
|
/// \f$ f = \text{jointBodyRegressor(model,data,jointId) * I.toDynamicParameters()} \f$ |
224 |
|
|
/// where \f$ f \f$ is the net force acting on the body, including gravity |
225 |
|
|
/// |
226 |
|
|
/// \param[in] model The model structure of the rigid body system. |
227 |
|
|
/// \param[in] data The data structure of the rigid body system. |
228 |
|
|
/// \param[in] jointId The id of the joint. |
229 |
|
|
/// |
230 |
|
|
/// \return The regressor of the body. |
231 |
|
|
/// |
232 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
233 |
|
|
inline typename DataTpl<Scalar,Options,JointCollectionTpl>::BodyRegressorType & |
234 |
|
|
jointBodyRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
235 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
236 |
|
|
JointIndex jointId); |
237 |
|
|
|
238 |
|
|
/// |
239 |
|
|
/// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given frame, |
240 |
|
|
/// puts the result in data.bodyRegressor and returns it. |
241 |
|
|
/// |
242 |
|
|
/// This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects. |
243 |
|
|
/// |
244 |
|
|
/// The result is such that |
245 |
|
|
/// \f$ f = \text{frameBodyRegressor(model,data,frameId) * I.toDynamicParameters()} \f$ |
246 |
|
|
/// where \f$ f \f$ is the net force acting on the body, including gravity |
247 |
|
|
/// |
248 |
|
|
/// \param[in] model The model structure of the rigid body system. |
249 |
|
|
/// \param[in] data The data structure of the rigid body system. |
250 |
|
|
/// \param[in] frameId The id of the frame. |
251 |
|
|
/// |
252 |
|
|
/// \return The dynamic regressor of the body. |
253 |
|
|
/// |
254 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
255 |
|
|
inline typename DataTpl<Scalar,Options,JointCollectionTpl>::BodyRegressorType & |
256 |
|
|
frameBodyRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
257 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
258 |
|
|
FrameIndex frameId); |
259 |
|
|
|
260 |
|
|
/// |
261 |
|
|
/// \brief Computes the joint torque regressor that links the joint torque |
262 |
|
|
/// to the dynamic parameters of each link according to the current the robot motion. |
263 |
|
|
/// |
264 |
|
|
/// The result is stored in `data.jointTorqueRegressor` and it corresponds to a matrix \f$ Y \f$ such that |
265 |
|
|
/// \f$ \tau = Y(q,\dot{q},\ddot{q})\pi \f$ |
266 |
|
|
/// where \f$ \pi = (\pi_1^T\ \dots\ \pi_n^T)^T \f$ and \f$ \pi_i = \text{model.inertias[i].toDynamicParameters()} \f$ |
267 |
|
|
/// |
268 |
|
|
/// \tparam JointCollection Collection of Joint types. |
269 |
|
|
/// \tparam ConfigVectorType Type of the joint configuration vector. |
270 |
|
|
/// \tparam TangentVectorType1 Type of the joint velocity vector. |
271 |
|
|
/// \tparam TangentVectorType2 Type of the joint acceleration vector. |
272 |
|
|
/// |
273 |
|
|
/// \param[in] model The model structure of the rigid body system. |
274 |
|
|
/// \param[in] data The data structure of the rigid body system. |
275 |
|
|
/// \param[in] q The joint configuration vector (dim model.nq). |
276 |
|
|
/// \param[in] v The joint velocity vector (dim model.nv). |
277 |
|
|
/// \param[in] a The joint acceleration vector (dim model.nv). |
278 |
|
|
/// |
279 |
|
|
/// \return The joint torque regressor of the system. |
280 |
|
|
/// |
281 |
|
|
/// \warning This function writes temporary information in data.bodyRegressor. This means if you have valuable data in it it will be overwritten. |
282 |
|
|
/// |
283 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
284 |
|
|
inline typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs & |
285 |
|
|
computeJointTorqueRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
286 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
287 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
288 |
|
|
const Eigen::MatrixBase<TangentVectorType1> & v, |
289 |
|
|
const Eigen::MatrixBase<TangentVectorType2> & a); |
290 |
|
|
|
291 |
|
|
} // namespace pinocchio |
292 |
|
|
|
293 |
|
|
/* --- Details -------------------------------------------------------------------- */ |
294 |
|
|
#include "pinocchio/algorithm/regressor.hxx" |
295 |
|
|
|
296 |
|
|
#endif // ifndef __pinocchio_algorithm_regressor_hpp__ |