Template Function pinocchio::ComputeRNEASecondOrderDerivatives(const ModelTpl<Scalar, Options, JointCollectionTpl>&, DataTpl<Scalar, Options, JointCollectionTpl>&, const Eigen::MatrixBase<ConfigVectorType>&, const Eigen::MatrixBase<TangentVectorType1>&, const Eigen::MatrixBase<TangentVectorType2>&, const Tensor1&, const Tensor2&, const Tensor3&, const Tensor4&)

Function Documentation

template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, typename Tensor1, typename Tensor2, typename Tensor3, typename Tensor4>
inline void pinocchio::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.r.t the joint configuration, the joint velocity and the joint acceleration.

Remark

d2tau_dqdq, d2tau_dvdv, dtau_dqdv and dtau_dadq must be first initialized with zeros (d2tau_dqdq.setZero(), etc). The storage order of the 3D-tensor derivatives is important. For d2tau_dqdq, the elements of generalized torque varies along the rows, while elements of q vary along the columns and pages of the tensor. For dtau_dqdv, the elements of generalized torque varies along the rows, while elements of v vary along the columns and elements of q along the pages of the tensor. Hence, dtau_dqdv is essentially d (d tau/dq)/dv, with outer-most derivative representing the third dimension (pages) of the tensor. The tensor dtau_dadq reduces down to dM/dq, and hence the elements of q vary along the pages of the tensor. In other words, this tensor derivative is d(d tau/da)/dq. All other remaining combinations of second-order derivatives of generalized torque are zero.

See also

Template Parameters:

  • JointCollection – Collection of Joint types.

  • ConfigVectorType – Type of the joint configuration vector.

  • TangentVectorType1 – Type of the joint velocity vector.

  • TangentVectorType2 – Type of the joint acceleration vector.

  • Tensor1 – Type of the 3D-Tensor containing the SO partial derivative with respect to the joint configuration vector. The elements of Torque vector are along the 1st dim, and joint config along 2nd,3rd dimensions.

  • Tensor2 – Type of the 3D-Tensor containing the Second-Order partial derivative with respect to the joint velocity vector. The elements of Torque vector are along the 1st dim, and the velocity along 2nd,3rd dimensions.

  • Tensor3 – Type of the 3D-Tensor containing the cross Second-Order partial derivative with respect to the joint configuration and velocty vector. The elements of Torque vector are along the 1st dim, and the config. vector along 2nd dimension, and velocity along the third dimension.

  • Tensor4 – Type of the 3D-Tensor containing the cross Second-Order partial derivative with respect to the joint configuration and acceleration vector. This is also the First-order partial derivative of Mass-Matrix (M) with respect to configuration vector. The elements of Torque vector are along the 1st dim, and the acceleration vector along 2nd dimension, while configuration along the third dimension.

Parameters:

  • model – [in] The model structure of the rigid body system.

  • data – [in] The data structure of the rigid body system.

  • q – [in] The joint configuration vector (dim model.nq).

  • v – [in] The joint velocity vector (dim model.nv).

  • a – [in] The joint acceleration vector (dim model.nv).

  • d2tau_dqdq – [out] Second-Order Partial derivative of the generalized torque vector with respect to the joint configuration.

  • d2tau_dvdv – [out] Second-Order Partial derivative of the generalized torque vector with respect to the joint velocity

  • dtau_dqdv – [out] Cross Second-Order Partial derivative of the generalized torque vector with respect to the joint configuration and velocity.

  • dtau_dadq – [out] Cross Second-Order Partial derivative of the generalized torque vector with respect to the joint configuration and accleration.