regressor.hpp 12.3 KB
Newer Older
1
//
2
// Copyright (c) 2018-2020 CNRS INRIA
3
4
//

Justin Carpentier's avatar
Justin Carpentier committed
5
6
#ifndef __pinocchio_algorithm_regressor_hpp__
#define __pinocchio_algorithm_regressor_hpp__
7
8

#include "pinocchio/multibody/model.hpp"
jcarpent's avatar
jcarpent committed
9
#include "pinocchio/multibody/data.hpp"
10

11
namespace pinocchio
12
{
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
  
  ///
  /// \brief Computes the kinematic regressor that links the joint placements variations of the whole kinematic tree
  ///        to the placement variation of the frame rigidly attached to the joint and given by its placement w.r.t. to the joint frame.
  ///
  /// \remarks It assumes that the forwardKinematics has been performed first.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] joint_id Index of the joint.
  /// \param[in] placement Relative placement to the joint frame.
  /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).  ///
  /// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0.
  ///
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
  void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                      const JointIndex joint_id,
                                      const ReferenceFrame rf,
                                      const SE3Tpl<Scalar,Options> & placement,
                                      const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor);

  ///
  /// \brief Computes the kinematic regressor that links the joint placements variations of the whole kinematic tree
  ///        to the placement variation of the joint given as input.
  ///
  /// \remarks It assumes that the forwardKinematics has been performed first.     
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] joint_id Index of the joint.
  /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).
  /// \param[out] kinematic_regressor The kinematic regressor containing the result.  Matrix of size 6*(model.njoints-1) initialized to 0.
  ///
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
  void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                      const JointIndex joint_id,
                                      const ReferenceFrame rf,
                                      const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor);
  
  ///
  /// \brief Computes the kinematic regressor that links the joint placements variations of the whole kinematic tree
  ///        to the placement variation of the frame given as input.
  ///
  /// \remarks It assumes that the forwardKinematics has been performed first.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] frame_id Index of the frame.
  /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).
  /// \param[out] kinematic_regressor The kinematic regressor containing the result.  Matrix of size 6*(model.njoints-1) initialized to 0.
  ///
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
  void computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                      const FrameIndex frame_id,
                                      const ReferenceFrame rf,
                                      const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor);

73
74
75
76
  ///
  /// \brief Computes the static regressor that links the center of mass positions of all the links
  ///        to the center of mass of the complete model according to the current configuration of the robot.
  ///
77
78
79
80
81
  /// The result is stored in `data.staticRegressor` and it corresponds to a matrix \f$ Y \f$ such that
  /// \f$ c = Y(q,\dot{q},\ddot{q})\tilde{\pi} \f$
  /// where \f$ \tilde{\pi} = (\tilde{\pi}_1^T\ \dots\ \tilde{\pi}_n^T)^T \f$ and
  /// \f$ \tilde{\pi}_i = \text{model.inertias[i].toDynamicParameters().head<4>()} \f$
  ///
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration vector (dim model.nq).
  ///
  /// \return The static regressor of the system.
  ///
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
  computeStaticRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                         DataTpl<Scalar,Options,JointCollectionTpl> & data,
                         const Eigen::MatrixBase<ConfigVectorType> & q);

97
98
99
100
101
102
103
  namespace regressor
  {
    
    ///
    /// \brief Computes the static regressor that links the center of mass positions of all the links
    ///        to the center of mass of the complete model according to the current configuration of the robot.
    ///
104
105
106
    /// \tparam JointCollection Collection of Joint types.
    /// \tparam ConfigVectorType Type of the joint configuration vector.
    ///
107
108
109
110
111
112
    /// \param[in] model The model structure of the rigid body system.
    /// \param[in] data The data structure of the rigid body system.
    /// \param[in] q The joint configuration vector (dim model.nq).
    ///
    /// \return The static regressor of the system.
    ///
113
114
    /// \deprecated This function is now in the main pinocchio namespace
    ///
115
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
116
    inline PINOCCHIO_DEPRECATED typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
117
118
    computeStaticRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                           DataTpl<Scalar,Options,JointCollectionTpl> & data,
119
120
121
122
                           const Eigen::MatrixBase<ConfigVectorType> & q)
    {
        return ::pinocchio::computeStaticRegressor(model,data,q);
    }
123
  }
Gabriele Buondonno's avatar
Gabriele Buondonno committed
124

125
126
127
128
129
130
131
132
133
134
135
136
  ///
  /// \brief Computes the regressor for the dynamic parameters of a single rigid body.
  ///
  /// The result is such that
  /// \f$ I a + v \times I v = bodyRegressor(v,a) * I.toDynamicParameters() \f$
  ///
  /// \param[in] v Velocity of the rigid body
  /// \param[in] a Acceleration of the rigid body
  /// \param[out] regressor The resulting regressor of the body.
  ///
  template<typename MotionVelocity, typename MotionAcceleration, typename OutputType>
  inline void
Justin Carpentier's avatar
Justin Carpentier committed
137
138
139
  bodyRegressor(const MotionDense<MotionVelocity> & v,
                const MotionDense<MotionAcceleration> & a,
                const Eigen::MatrixBase<OutputType> & regressor);
140

Gabriele Buondonno's avatar
Gabriele Buondonno committed
141
142
143
144
145
146
147
148
149
150
151
152
  ///
  /// \brief Computes the regressor for the dynamic parameters of a single rigid body.
  ///
  /// The result is such that
  /// \f$ I a + v \times I v = bodyRegressor(v,a) * I.toDynamicParameters() \f$
  ///
  /// \param[in] v Velocity of the rigid body
  /// \param[in] a Acceleration of the rigid body
  ///
  /// \return The regressor of the body.
  ///
  template<typename MotionVelocity, typename MotionAcceleration>
153
  inline Eigen::Matrix<typename MotionVelocity::Scalar,6,10,PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options>
Justin Carpentier's avatar
Justin Carpentier committed
154
155
  bodyRegressor(const MotionDense<MotionVelocity> & v,
                const MotionDense<MotionAcceleration> & a);
156
157

  ///
158
159
  /// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given joint,
  ///        puts the result in data.bodyRegressor and returns it.
160
161
162
163
  ///
  /// This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects.
  ///
  /// The result is such that
164
  /// \f$ f = \text{jointBodyRegressor(model,data,jointId) * I.toDynamicParameters()} \f$
165
166
167
168
169
170
171
172
173
  /// where \f$ f \f$ is the net force acting on the body, including gravity
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] jointId The id of the joint.
  ///
  /// \return The regressor of the body.
  ///
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
174
  inline typename DataTpl<Scalar,Options,JointCollectionTpl>::BodyRegressorType &
175
176
177
  jointBodyRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                     DataTpl<Scalar,Options,JointCollectionTpl> & data,
                     JointIndex jointId);
178
179

  ///
180
181
  /// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given frame,
  ///        puts the result in data.bodyRegressor and returns it.
182
183
184
185
  ///
  /// This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects.
  ///
  /// The result is such that
186
  /// \f$ f = \text{frameBodyRegressor(model,data,frameId) * I.toDynamicParameters()} \f$
187
188
189
190
191
192
  /// where \f$ f \f$ is the net force acting on the body, including gravity
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] frameId The id of the frame.
  ///
Justin Carpentier's avatar
Justin Carpentier committed
193
  /// \return The dynamic regressor of the body.
194
195
  ///
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
196
  inline typename DataTpl<Scalar,Options,JointCollectionTpl>::BodyRegressorType &
197
198
199
  frameBodyRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                     DataTpl<Scalar,Options,JointCollectionTpl> & data,
                     FrameIndex frameId);
200

201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
  ///
  /// \brief Computes the joint torque regressor that links the joint torque
  ///        to the dynamic parameters of each link according to the current the robot motion.
  ///
  /// The result is stored in `data.jointTorqueRegressor` and it corresponds to a matrix \f$ Y \f$ such that
  /// \f$ \tau = Y(q,\dot{q},\ddot{q})\pi \f$
  /// where \f$ \pi = (\pi_1^T\ \dots\ \pi_n^T)^T \f$ and \f$ \pi_i = \text{model.inertias[i].toDynamicParameters()} \f$
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
  /// \tparam TangentVectorType1 Type of the joint velocity vector.
  /// \tparam TangentVectorType2 Type of the joint acceleration vector.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration vector (dim model.nq).
  /// \param[in] v The joint velocity vector (dim model.nv).
  /// \param[in] a The joint acceleration vector (dim model.nv).
  ///
  /// \return The joint torque regressor of the system.
  ///
222
223
  /// \warning This function writes temporary information in data.bodyRegressor. This means if you have valuable data in it it will be overwritten.
  ///
224
225
226
227
228
229
230
231
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
  inline typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
  computeJointTorqueRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                              DataTpl<Scalar,Options,JointCollectionTpl> & data,
                              const Eigen::MatrixBase<ConfigVectorType> & q,
                              const Eigen::MatrixBase<TangentVectorType1> & v,
                              const Eigen::MatrixBase<TangentVectorType2> & a);

232
} // namespace pinocchio
233
234
235
236

/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/regressor.hxx"

Justin Carpentier's avatar
Justin Carpentier committed
237
#endif // ifndef __pinocchio_algorithm_regressor_hpp__