pinocchio 4.1.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#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
29namespace 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::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)
Computes the kinetic energy regressor that links the kinetic energy of the system to the dynamic para...
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,...