1 |
|
|
// |
2 |
|
|
// Copyright (c) 2018 CNRS, INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_aba_derivatives_hpp__ |
6 |
|
|
#define __pinocchio_aba_derivatives_hpp__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/multibody/model.hpp" |
9 |
|
|
#include "pinocchio/multibody/data.hpp" |
10 |
|
|
|
11 |
|
|
namespace pinocchio |
12 |
|
|
{ |
13 |
|
|
/// |
14 |
|
|
/// \brief The derivatives of the Articulated-Body algorithm. |
15 |
|
|
/// |
16 |
|
|
/// \tparam JointCollection Collection of Joint types. |
17 |
|
|
/// \tparam ConfigVectorType Type of the joint configuration vector. |
18 |
|
|
/// \tparam TangentVectorType1 Type of the joint velocity vector. |
19 |
|
|
/// \tparam TangentVectorType2 Type of the joint torque vector. |
20 |
|
|
/// \tparam MatrixType1 Type of the matrix containing the partial derivative with respect to the joint configuration vector. |
21 |
|
|
/// \tparam MatrixType2 Type of the matrix containing the partial derivative with respect to the joint velocity vector. |
22 |
|
|
/// \tparam MatrixType3 Type of the matrix containing the partial derivative with respect to the joint torque vector. |
23 |
|
|
/// |
24 |
|
|
/// \param[in] model The model structure of the rigid body system. |
25 |
|
|
/// \param[in] data The data structure of the rigid body system. |
26 |
|
|
/// \param[in] q The joint configuration vector (dim model.nq). |
27 |
|
|
/// \param[in] v The joint velocity vector (dim model.nv). |
28 |
|
|
/// \param[in] tau The joint torque vector (dim model.nv). |
29 |
|
|
/// \param[out] aba_partial_dq Partial derivative of the generalized torque vector with respect to the joint configuration. |
30 |
|
|
/// \param[out] aba_partial_dv Partial derivative of the generalized torque vector with respect to the joint velocity. |
31 |
|
|
/// \param[out] aba_partial_dtau Partial derivative of the generalized torque vector with respect to the joint torque. |
32 |
|
|
/// |
33 |
|
|
/// \note aba_partial_dtau is in fact nothing more than the inverse of the joint space inertia matrix. |
34 |
|
|
/// |
35 |
|
|
/// \sa pinocchio::aba |
36 |
|
|
/// |
37 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, |
38 |
|
|
typename MatrixType1, typename MatrixType2, typename MatrixType3> |
39 |
|
|
inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
40 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
41 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
42 |
|
|
const Eigen::MatrixBase<TangentVectorType1> & v, |
43 |
|
|
const Eigen::MatrixBase<TangentVectorType2> & tau, |
44 |
|
|
const Eigen::MatrixBase<MatrixType1> & aba_partial_dq, |
45 |
|
|
const Eigen::MatrixBase<MatrixType2> & aba_partial_dv, |
46 |
|
|
const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau); |
47 |
|
|
/// |
48 |
|
|
/// \brief The derivatives of the Articulated-Body algorithm with external forces. |
49 |
|
|
/// |
50 |
|
|
/// \tparam JointCollection Collection of Joint types. |
51 |
|
|
/// \tparam ConfigVectorType Type of the joint configuration vector. |
52 |
|
|
/// \tparam TangentVectorType1 Type of the joint velocity vector. |
53 |
|
|
/// \tparam TangentVectorType2 Type of the joint torque vector. |
54 |
|
|
/// \tparam MatrixType1 Type of the matrix containing the partial derivative with respect to the joint configuration vector. |
55 |
|
|
/// \tparam MatrixType2 Type of the matrix containing the partial derivative with respect to the joint velocity vector. |
56 |
|
|
/// \tparam MatrixType3 Type of the matrix containing the partial derivative with respect to the joint torque vector. |
57 |
|
|
/// |
58 |
|
|
/// \param[in] model The model structure of the rigid body system. |
59 |
|
|
/// \param[in] data The data structure of the rigid body system. |
60 |
|
|
/// \param[in] q The joint configuration vector (dim model.nq). |
61 |
|
|
/// \param[in] v The joint velocity vector (dim model.nv). |
62 |
|
|
/// \param[in] tau The joint torque vector (dim model.nv). |
63 |
|
|
/// \param[in] fext External forces expressed in the local frame of the joints (dim model.njoints). |
64 |
|
|
/// \param[out] aba_partial_dq Partial derivative of the generalized torque vector with respect to the joint configuration. |
65 |
|
|
/// \param[out] aba_partial_dv Partial derivative of the generalized torque vector with respect to the joint velocity. |
66 |
|
|
/// \param[out] aba_partial_dtau Partial derivative of the generalized torque vector with respect to the joint torque. |
67 |
|
|
/// |
68 |
|
|
/// \note aba_partial_dtau is in fact nothing more than the inverse of the joint space inertia matrix. |
69 |
|
|
/// |
70 |
|
|
/// \sa pinocchio::aba |
71 |
|
|
/// |
72 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, |
73 |
|
|
typename MatrixType1, typename MatrixType2, typename MatrixType3> |
74 |
|
|
inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
75 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
76 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
77 |
|
|
const Eigen::MatrixBase<TangentVectorType1> & v, |
78 |
|
|
const Eigen::MatrixBase<TangentVectorType2> & tau, |
79 |
|
|
const container::aligned_vector< ForceTpl<Scalar,Options> > & fext, |
80 |
|
|
const Eigen::MatrixBase<MatrixType1> & aba_partial_dq, |
81 |
|
|
const Eigen::MatrixBase<MatrixType2> & aba_partial_dv, |
82 |
|
|
const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau); |
83 |
|
|
|
84 |
|
|
/// |
85 |
|
|
/// \brief The derivatives of the Articulated-Body algorithm. |
86 |
|
|
/// |
87 |
|
|
/// \tparam JointCollection Collection of Joint types. |
88 |
|
|
/// \tparam ConfigVectorType Type of the joint configuration vector. |
89 |
|
|
/// \tparam TangentVectorType1 Type of the joint velocity vector. |
90 |
|
|
/// \tparam TangentVectorType2 Type of the joint torque vector. |
91 |
|
|
/// |
92 |
|
|
/// \param[in] model The model structure of the rigid body system. |
93 |
|
|
/// \param[in] data The data structure of the rigid body system. |
94 |
|
|
/// \param[in] q The joint configuration vector (dim model.nq). |
95 |
|
|
/// \param[in] v The joint velocity vector (dim model.nv). |
96 |
|
|
/// \param[in] tau The joint torque vector (dim model.nv). |
97 |
|
|
/// |
98 |
|
|
/// \returns The results are stored in data.ddq_dq, data.ddq_dv and data.Minv which respectively correspond |
99 |
|
|
/// to the partial derivatives of the joint acceleration vector with respect to the joint configuration, velocity and torque. |
100 |
|
|
/// And as for pinocchio::computeMinverse, only the upper triangular part of data.Minv is filled. |
101 |
|
|
/// |
102 |
|
|
/// \sa pinocchio::aba and \sa pinocchio::computeABADerivatives. |
103 |
|
|
/// |
104 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
105 |
|
24 |
inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
106 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
107 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
108 |
|
|
const Eigen::MatrixBase<TangentVectorType1> & v, |
109 |
|
|
const Eigen::MatrixBase<TangentVectorType2> & tau) |
110 |
|
|
{ |
111 |
|
24 |
computeABADerivatives(model,data,q,v,tau, |
112 |
|
24 |
data.ddq_dq,data.ddq_dv,data.Minv); |
113 |
|
24 |
} |
114 |
|
|
|
115 |
|
|
/// |
116 |
|
|
/// \brief The derivatives of the Articulated-Body algorithm with external forces. |
117 |
|
|
/// |
118 |
|
|
/// \tparam JointCollection Collection of Joint types. |
119 |
|
|
/// \tparam ConfigVectorType Type of the joint configuration vector. |
120 |
|
|
/// \tparam TangentVectorType1 Type of the joint velocity vector. |
121 |
|
|
/// \tparam TangentVectorType2 Type of the joint torque vector. |
122 |
|
|
/// |
123 |
|
|
/// \param[in] model The model structure of the rigid body system. |
124 |
|
|
/// \param[in] data The data structure of the rigid body system. |
125 |
|
|
/// \param[in] q The joint configuration vector (dim model.nq). |
126 |
|
|
/// \param[in] v The joint velocity vector (dim model.nv). |
127 |
|
|
/// \param[in] tau The joint torque vector (dim model.nv). |
128 |
|
|
/// \param[in] fext External forces expressed in the local frame of the joints (dim model.njoints). |
129 |
|
|
/// |
130 |
|
|
/// \returns The results are stored in data.ddq_dq, data.ddq_dv and data.Minv which respectively correspond |
131 |
|
|
/// to the partial derivatives of the joint acceleration vector with respect to the joint configuration, velocity and torque. |
132 |
|
|
/// And as for pinocchio::computeMinverse, only the upper triangular part of data.Minv is filled. |
133 |
|
|
/// |
134 |
|
|
/// \sa pinocchio::aba and \sa pinocchio::computeABADerivatives. |
135 |
|
|
/// |
136 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
137 |
|
1 |
inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
138 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
139 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
140 |
|
|
const Eigen::MatrixBase<TangentVectorType1> & v, |
141 |
|
|
const Eigen::MatrixBase<TangentVectorType2> & tau, |
142 |
|
|
const container::aligned_vector< ForceTpl<Scalar,Options> > & fext) |
143 |
|
|
{ |
144 |
|
1 |
computeABADerivatives(model,data,q,v,tau,fext, |
145 |
|
1 |
data.ddq_dq,data.ddq_dv,data.Minv); |
146 |
|
1 |
} |
147 |
|
|
|
148 |
|
|
} // namespace pinocchio |
149 |
|
|
|
150 |
|
|
/* --- Details -------------------------------------------------------------------- */ |
151 |
|
|
#include "pinocchio/algorithm/aba-derivatives.hxx" |
152 |
|
|
|
153 |
|
|
#endif // ifndef __pinocchio_aba_derivatives_hpp__ |