pinocchio  2.7.0
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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__
pinocchio::frameBodyRegressor
DataTpl< Scalar, Options, JointCollectionTpl >::BodyRegressorType & frameBodyRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, FrameIndex frameId)
Computes the regressor for the dynamic parameters of a rigid body attached to a given frame,...
pinocchio::DataTpl
Definition: data.hpp:29
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::ReferenceFrame
ReferenceFrame
List of Reference Frames supported by Pinocchio.
Definition: fwd.hpp:44
pinocchio::jointBodyRegressor
DataTpl< Scalar, Options, JointCollectionTpl >::BodyRegressorType & jointBodyRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, JointIndex jointId)
Computes the regressor for the dynamic parameters of a rigid body attached to a given joint,...
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: data.hpp:73
pinocchio::DataTpl::BodyRegressorType
Eigen::Matrix< Scalar, 6, 10, Options > BodyRegressorType
The type of the body regressor.
Definition: data.hpp:82
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: data.hpp:75
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)
Computes the kinematic regressor that links the joint placements variations of the whole kinematic tr...
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)
Computes the kinematic regressor that links the joint placement variations of the whole kinematic tre...
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 ...
Definition: regressor.hpp:176
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl >
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
Main pinocchio namespace.
Definition: treeview.dox:11