$darkmode
pinocchio 4.0.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
rnea-second-order-derivatives.hpp
1 //
2 // Copyright (c) 2017-2019 CNRS INRIA
3 
4 #pragma once
5 
6 // IWYU pragma: begin_keep
7 #include <Eigen/Core>
8 
9 #include <cassert>
10 #include <cstddef>
11 #include <type_traits>
12 #include <vector>
13 
14 #include <boost/fusion/container/vector.hpp>
15 
16 #include "pinocchio/macros.hpp"
17 #include "pinocchio/eigen-common.hpp"
18 #include "pinocchio/fwd.hpp"
19 
20 #include "pinocchio/math.hpp"
21 
22 #include "pinocchio/spatial.hpp"
23 
24 #include "pinocchio/multibody.hpp"
25 #include "pinocchio/multibody/joint.hpp"
26 #include "pinocchio/multibody/visitor.hpp"
27 
28 #include "pinocchio/algorithm/check.hpp"
29 // IWYU pragma: end_keep
30 
31 namespace pinocchio
32 {
91  template<
92  typename Scalar,
93  int Options,
94  template<typename, int> class JointCollectionTpl,
95  typename ConfigVectorType,
96  typename TangentVectorType1,
97  typename TangentVectorType2,
98  typename Tensor1,
99  typename Tensor2,
100  typename Tensor3,
101  typename Tensor4>
102  inline void ComputeRNEASecondOrderDerivatives(
103  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
104  DataTpl<Scalar, Options, JointCollectionTpl> & data,
105  const Eigen::MatrixBase<ConfigVectorType> & q,
106  const Eigen::MatrixBase<TangentVectorType1> & v,
107  const Eigen::MatrixBase<TangentVectorType2> & a,
108  const Tensor1 & d2tau_dqdq,
109  const Tensor2 & d2tau_dvdv,
110  const Tensor3 & dtau_dqdv,
111  const Tensor4 & dtau_dadq);
112 
150 
151  template<
152  typename Scalar,
153  int Options,
154  template<typename, int> class JointCollectionTpl,
155  typename ConfigVectorType,
156  typename TangentVectorType1,
157  typename TangentVectorType2>
158  inline void ComputeRNEASecondOrderDerivatives(
159  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
160  DataTpl<Scalar, Options, JointCollectionTpl> & data,
161  const Eigen::MatrixBase<ConfigVectorType> & q,
162  const Eigen::MatrixBase<TangentVectorType1> & v,
163  const Eigen::MatrixBase<TangentVectorType2> & a)
164  {
165  (data.d2tau_dqdq).setZero();
166  (data.d2tau_dvdv).setZero();
167  (data.d2tau_dqdv).setZero();
168  (data.d2tau_dadq).setZero();
169 
170  ComputeRNEASecondOrderDerivatives(
171  model, data, q.derived(), v.derived(), a.derived(), data.d2tau_dqdq, data.d2tau_dvdv,
172  data.d2tau_dqdv, data.d2tau_dadq);
173  }
174 
175 } // namespace pinocchio
176 
177 // IWYU pragma: begin_exports
178 #include "pinocchio/src/algorithm/rnea-second-order-derivatives.hxx"
179 // IWYU pragma: end_exports
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::ComputeRNEASecondOrderDerivatives
void ComputeRNEASecondOrderDerivatives(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, const Tensor1 &d2tau_dqdq, const Tensor2 &d2tau_dvdv, const Tensor3 &dtau_dqdv, const Tensor4 &dtau_dadq)
Computes the Second-Order partial derivatives of the Recursive Newton Euler Algorithm w....