pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
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
11namespace 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::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::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::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::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::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::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::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::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