$darkmode
pinocchio 4.0.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 #pragma once
6 
7 // IWYU pragma: begin_keep
8 #include <Eigen/Core>
9 
10 #include <cassert>
11 #include <vector>
12 
13 #include <boost/fusion/container/vector.hpp>
14 
15 #include "pinocchio/macros.hpp"
16 #include "pinocchio/eigen-common.hpp"
17 
18 #include "pinocchio/math.hpp"
19 
20 #include "pinocchio/spatial.hpp"
21 
22 #include "pinocchio/multibody.hpp"
23 #include "pinocchio/multibody/joint.hpp"
24 
25 #include "pinocchio/algorithm/check.hpp"
26 #include "pinocchio/algorithm/kinematics.hpp"
27 // IWYU pragma: end_keep
28 
29 namespace pinocchio
30 {
31 
40  template<
41  typename Scalar,
42  int Options,
43  template<typename, int> class JointCollectionTpl,
44  typename Matrix6xReturnType>
45  void computeJointKinematicRegressor(
46  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
47  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
48  const JointIndex joint_id,
49  const ReferenceFrame rf,
50  const SE3Tpl<Scalar, Options> & placement,
51  const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor);
52 
69  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
70  typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x computeJointKinematicRegressor(
71  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
72  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
73  const JointIndex joint_id,
74  const ReferenceFrame rf,
75  const SE3Tpl<Scalar, Options> & placement)
76  {
77  typedef typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x ReturnType;
78  ReturnType res(ReturnType::Zero(6, (model.njoints - 1) * 6));
79 
80  computeJointKinematicRegressor(model, data, joint_id, rf, placement, res);
81 
82  return res;
83  }
84 
92  template<
93  typename Scalar,
94  int Options,
95  template<typename, int> class JointCollectionTpl,
96  typename Matrix6xReturnType>
97  void computeJointKinematicRegressor(
98  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
99  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
100  const JointIndex joint_id,
101  const ReferenceFrame rf,
102  const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor);
103 
119  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
120  typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x computeJointKinematicRegressor(
121  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
122  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
123  const JointIndex joint_id,
124  const ReferenceFrame rf)
125  {
126  typedef typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x ReturnType;
127  ReturnType res(ReturnType::Zero(6, (model.njoints - 1) * 6));
128 
129  computeJointKinematicRegressor(model, data, joint_id, rf, res);
130 
131  return res;
132  }
133 
141  template<
142  typename Scalar,
143  int Options,
144  template<typename, int> class JointCollectionTpl,
145  typename Matrix6xReturnType>
146  void computeFrameKinematicRegressor(
147  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
148  DataTpl<Scalar, Options, JointCollectionTpl> & data,
149  const FrameIndex frame_id,
150  const ReferenceFrame rf,
151  const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor);
152 
168  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
169  typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x computeFrameKinematicRegressor(
170  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
171  DataTpl<Scalar, Options, JointCollectionTpl> & data,
172  const FrameIndex frame_id,
173  const ReferenceFrame rf)
174  {
175  typedef typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x ReturnType;
176  ReturnType res(ReturnType::Zero(6, (model.njoints - 1) * 6));
177 
178  computeFrameKinematicRegressor(model, data, frame_id, rf, res);
179 
180  return res;
181  }
182 
202  template<
203  typename Scalar,
204  int Options,
205  template<typename, int> class JointCollectionTpl,
206  typename ConfigVectorType>
207  inline typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & computeStaticRegressor(
208  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
209  DataTpl<Scalar, Options, JointCollectionTpl> & data,
210  const Eigen::MatrixBase<ConfigVectorType> & q);
211 
222  template<typename MotionVelocity, typename MotionAcceleration, typename OutputType>
223  inline void bodyRegressor(
224  const MotionDense<MotionVelocity> & v,
225  const MotionDense<MotionAcceleration> & a,
226  const Eigen::MatrixBase<OutputType> & regressor);
227 
239  template<typename MotionVelocity, typename MotionAcceleration>
240  inline Eigen::Matrix<
241  typename MotionVelocity::Scalar,
242  6,
243  10,
244  PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options>
245  bodyRegressor(const MotionDense<MotionVelocity> & v, const MotionDense<MotionAcceleration> & a);
246 
265  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
266  inline typename DataTpl<Scalar, Options, JointCollectionTpl>::BodyRegressorType &
267  jointBodyRegressor(
268  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
269  DataTpl<Scalar, Options, JointCollectionTpl> & data,
270  JointIndex joint_id);
271 
290  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
291  inline typename DataTpl<Scalar, Options, JointCollectionTpl>::BodyRegressorType &
292  frameBodyRegressor(
293  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
294  DataTpl<Scalar, Options, JointCollectionTpl> & data,
295  FrameIndex frame_id);
296 
321  template<
322  typename Scalar,
323  int Options,
324  template<typename, int> class JointCollectionTpl,
325  typename ConfigVectorType,
326  typename TangentVectorType1,
327  typename TangentVectorType2>
328  inline typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs &
329  computeJointTorqueRegressor(
330  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
331  DataTpl<Scalar, Options, JointCollectionTpl> & data,
332  const Eigen::MatrixBase<ConfigVectorType> & q,
333  const Eigen::MatrixBase<TangentVectorType1> & v,
334  const Eigen::MatrixBase<TangentVectorType2> & a);
335 
354  template<
355  typename Scalar,
356  int Options,
357  template<typename, int> class JointCollectionTpl,
358  typename ConfigVectorType,
359  typename TangentVectorType>
360  const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowVectorXs &
361  computeKineticEnergyRegressor(
362  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
363  DataTpl<Scalar, Options, JointCollectionTpl> & data,
364  const Eigen::MatrixBase<ConfigVectorType> & q,
365  const Eigen::MatrixBase<TangentVectorType> & v);
366 
383  template<
384  typename Scalar,
385  int Options,
386  template<typename, int> class JointCollectionTpl,
387  typename ConfigVectorType>
388  const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowVectorXs &
389  computePotentialEnergyRegressor(
390  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
391  DataTpl<Scalar, Options, JointCollectionTpl> & data,
392  const Eigen::MatrixBase<ConfigVectorType> & q);
393 } // namespace pinocchio
394 
395 // IWYU pragma: begin_exports
396 #include "pinocchio/src/algorithm/regressor.hxx"
397 // IWYU pragma: end_exports
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,...