pinocchio  3.3.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
regressor.hpp
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 
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 
51  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
52  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  ReturnType res(ReturnType::Zero(6, (model.njoints - 1) * 6));
61 
62  computeJointKinematicRegressor(model, data, joint_id, rf, placement, res);
63 
64  return res;
65  }
66 
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 
101  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
102  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  ReturnType res(ReturnType::Zero(6, (model.njoints - 1) * 6));
110 
111  computeJointKinematicRegressor(model, data, joint_id, rf, res);
112 
113  return res;
114  }
115 
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 
150  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
151  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  ReturnType res(ReturnType::Zero(6, (model.njoints - 1) * 6));
159 
160  computeFrameKinematicRegressor(model, data, frame_id, rf, res);
161 
162  return res;
163  }
164 
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 
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 
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 
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 
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 
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 
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 
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__
pinocchio::ReferenceFrame
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Definition: fwd.hpp:47
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::bodyRegressor
void bodyRegressor(const MotionDense< MotionVelocity > &v, const MotionDense< MotionAcceleration > &a, const Eigen::MatrixBase< OutputType > &regressor)
Computes the regressor for the dynamic parameters of a single rigid body.
pinocchio::computeStaticRegressor
DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x & computeStaticRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the static regressor that links the center of mass positions of all the links to the center ...
pinocchio::computeJointKinematicRegressor
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)
pinocchio::computeFrameKinematicRegressor
void computeFrameKinematicRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xReturnType > &kinematic_regressor)
pinocchio::computeJointTorqueRegressor
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)
Computes the joint torque regressor that links the joint torque to the dynamic parameters of each lin...
pinocchio::frameBodyRegressor
DataTpl< Scalar, Options, JointCollectionTpl >::BodyRegressorType & frameBodyRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, FrameIndex frame_id)
Computes the regressor for the dynamic parameters of a rigid body attached to a given frame,...
pinocchio::computePotentialEnergyRegressor
const DataTpl< Scalar, Options, JointCollectionTpl >::RowVectorXs & computePotentialEnergyRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
pinocchio::jointBodyRegressor
DataTpl< Scalar, Options, JointCollectionTpl >::BodyRegressorType & jointBodyRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, JointIndex joint_id)
Computes the regressor for the dynamic parameters of a rigid body attached to a given joint,...
pinocchio::DataTpl::BodyRegressorType
Eigen::Matrix< Scalar, 6, 10, Options > BodyRegressorType
The type of the body regressor.
Definition: data.hpp:102
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: data.hpp:92
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: data.hpp:94