pinocchio 4.1.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
aba-derivatives.hpp
1//
2// Copyright (c) 2018 CNRS
3// Copyright (c) 2018-2024 INRIA
4//
5
6#pragma once
7
8// IWYU pragma: begin_keep
9
10#include <Eigen/Core>
11
12#include <cassert>
13#include <cstddef>
14#include <type_traits>
15#include <vector>
16
17#include <boost/fusion/container/vector.hpp>
18
19#include "pinocchio/macros.hpp"
20#include "pinocchio/eigen-common.hpp"
21#include "pinocchio/fwd.hpp"
22
23#include "pinocchio/math.hpp"
24
25#include "pinocchio/spatial.hpp"
26
27#include "pinocchio/multibody.hpp"
28#include "pinocchio/multibody/joint.hpp"
29#include "pinocchio/multibody/visitor.hpp"
30
31#include "pinocchio/algorithm/check.hpp"
32#include "pinocchio/algorithm/aba.hpp"
33#include "pinocchio/algorithm/joint-configuration.hpp"
34
35// IWYU pragma: end_keep
36
37namespace pinocchio
38{
66 template<
67 typename Scalar,
68 int Options,
69 template<typename, int> class JointCollectionTpl,
70 typename ConfigVectorType,
71 typename TangentVectorType1,
72 typename TangentVectorType2,
73 typename MatrixType1,
74 typename MatrixType2,
75 typename MatrixType3>
76 void computeABADerivatives(
77 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
78 DataTpl<Scalar, Options, JointCollectionTpl> & data,
79 const Eigen::MatrixBase<ConfigVectorType> & q,
80 const Eigen::MatrixBase<TangentVectorType1> & v,
81 const Eigen::MatrixBase<TangentVectorType2> & tau,
82 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
83 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
84 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau);
113 template<
114 typename Scalar,
115 int Options,
116 template<typename, int> class JointCollectionTpl,
117 typename ConfigVectorType,
118 typename TangentVectorType1,
119 typename TangentVectorType2,
120 typename SpatialForce,
121 typename SpatialForceAllocator,
122 typename MatrixType1,
123 typename MatrixType2,
124 typename MatrixType3>
125 void computeABADerivatives(
126 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
127 DataTpl<Scalar, Options, JointCollectionTpl> & data,
128 const Eigen::MatrixBase<ConfigVectorType> & q,
129 const Eigen::MatrixBase<TangentVectorType1> & v,
130 const Eigen::MatrixBase<TangentVectorType2> & tau,
131 const std::vector<SpatialForce, SpatialForceAllocator> & fext,
132 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
133 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
134 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau);
135
158 template<
159 typename Scalar,
160 int Options,
161 template<typename, int> class JointCollectionTpl,
162 typename ConfigVectorType,
163 typename TangentVectorType1,
164 typename TangentVectorType2>
165 std::enable_if_t<
166 ConfigVectorType::IsVectorAtCompileTime || TangentVectorType1::IsVectorAtCompileTime
167 || TangentVectorType2::IsVectorAtCompileTime,
168 void>
169 computeABADerivatives(
170 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
171 DataTpl<Scalar, Options, JointCollectionTpl> & data,
172 const Eigen::MatrixBase<ConfigVectorType> & q,
173 const Eigen::MatrixBase<TangentVectorType1> & v,
174 const Eigen::MatrixBase<TangentVectorType2> & tau);
175
200 template<
201 typename Scalar,
202 int Options,
203 template<typename, int> class JointCollectionTpl,
204 typename ConfigVectorType,
205 typename TangentVectorType1,
206 typename TangentVectorType2,
207 typename SpatialForce,
208 typename SpatialForceAllocator>
209 void computeABADerivatives(
210 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
211 DataTpl<Scalar, Options, JointCollectionTpl> & data,
212 const Eigen::MatrixBase<ConfigVectorType> & q,
213 const Eigen::MatrixBase<TangentVectorType1> & v,
214 const Eigen::MatrixBase<TangentVectorType2> & tau,
215 const std::vector<SpatialForce, SpatialForceAllocator> & fext);
216
240 template<
241 typename Scalar,
242 int Options,
243 template<typename, int> class JointCollectionTpl,
244 typename MatrixType1,
245 typename MatrixType2,
246 typename MatrixType3>
247 std::enable_if_t<
248 !(MatrixType1::IsVectorAtCompileTime || MatrixType2::IsVectorAtCompileTime
249 || MatrixType3::IsVectorAtCompileTime),
250 void>
251 computeABADerivatives(
252 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
253 DataTpl<Scalar, Options, JointCollectionTpl> & data,
254 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
255 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
256 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau);
257
270 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
271 void computeABADerivatives(
272 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
273 DataTpl<Scalar, Options, JointCollectionTpl> & data);
274
299 template<
300 typename Scalar,
301 int Options,
302 template<typename, int> class JointCollectionTpl,
303 typename SpatialForce,
304 typename SpatialForceAllocator,
305 typename MatrixType1,
306 typename MatrixType2,
307 typename MatrixType3>
308 void computeABADerivatives(
309 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
310 DataTpl<Scalar, Options, JointCollectionTpl> & data,
311 const std::vector<SpatialForce, SpatialForceAllocator> & fext,
312 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
313 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
314 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau);
315
333 template<
334 typename Scalar,
335 int Options,
336 template<typename, int> class JointCollectionTpl,
337 typename SpatialForce,
338 typename SpatialForceAllocator>
339 void computeABADerivatives(
340 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
341 DataTpl<Scalar, Options, JointCollectionTpl> & data,
342 const std::vector<SpatialForce, SpatialForceAllocator> & fext);
343
344} // namespace pinocchio
345
346// IWYU pragma: begin_exports
347#include "pinocchio/src/algorithm/aba-derivatives.hxx"
348// IWYU pragma: end_exports
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::computeABADerivatives
void computeABADerivatives(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 > &tau, const Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
The derivatives of the Articulated-Body algorithm.