Template Function pinocchio::getFrameAccelerationDerivatives(const ModelTpl<Scalar, Options, JointCollectionTpl>&, DataTpl<Scalar, Options, JointCollectionTpl>&, const JointIndex, const SE3Tpl<Scalar, Options>&, const ReferenceFrame, const Eigen::MatrixBase<Matrix6xOut1>&, const Eigen::MatrixBase<Matrix6xOut2>&, const Eigen::MatrixBase<Matrix6xOut3>&, const Eigen::MatrixBase<Matrix6xOut4>&)
Defined in File frames-derivatives.hxx
Function Documentation
-
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4>
void pinocchio::getFrameAccelerationDerivatives(const ModelTpl<Scalar, Options, JointCollectionTpl> &model, DataTpl<Scalar, Options, JointCollectionTpl> &data, const JointIndex joint_id, const SE3Tpl<Scalar, Options> &placement, const ReferenceFrame rf, const Eigen::MatrixBase<Matrix6xOut1> &v_partial_dq, const Eigen::MatrixBase<Matrix6xOut2> &a_partial_dq, const Eigen::MatrixBase<Matrix6xOut3> &a_partial_dv, const Eigen::MatrixBase<Matrix6xOut4> &a_partial_da) Computes the partial derivatives of the spatial acceleration of a frame given by its relative placement, with respect to q, v and a. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. It is important to notice that a direct outcome (for free) of this algo is v_partial_dq and v_partial_dv which is equal to a_partial_da.
- Template Parameters:
JointCollection – Collection of Joint types.
Matrix6xOut1 – Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector.
Matrix6xOut2 – Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint configuration vector.
Matrix6xOut3 – Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint velocity vector.
Matrix6xOut4 – Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint acceleration vector.
- Parameters:
model – [in] The kinematic model
data – [in] Data associated to model
joint_id – [in] Index of the supporting joint
placement – [in] Placement of the Frame w.r.t. the joint frame.
rf – [in] Reference frame in which the velocity is expressed.
v_partial_dq – [out] Partial derivative of the frame spatial velocity w.r.t.
.a_partial_dq – [out] Partial derivative of the frame spatial acceleration w.r.t.
.a_partial_dq – [out] Partial derivative of the frame spatial acceleration w.r.t.
.a_partial_dq – [out] Partial derivative of the frame spatial acceleration w.r.t.
.