GCC Code Coverage Report
 Directory: ./ Exec Total Coverage File: include/pinocchio/algorithm/kinematics-derivatives.hpp Lines: 8 8 100.0 % Date: 2024-04-26 13:14:21 Branches: 2 4 50.0 %

 Line Branch Exec Source 1 // 2 // Copyright (c) 2017-2019 CNRS INRIA 3 // 4 5 #ifndef __pinocchio_algorithm_kinematics_derivatives_hpp__ 6 #define __pinocchio_algorithm_kinematics_derivatives_hpp__ 7 8 #include "pinocchio/multibody/model.hpp" 9 #include "pinocchio/multibody/data.hpp" 10 11 #include "pinocchio/algorithm/jacobian.hpp" 12 13 namespace pinocchio 14 { 15 16  /// 17  /// \brief Computes all the terms required to compute the derivatives of the placement, spatial velocity and acceleration 18  /// for any joint of the model. 19  /// 20  /// \tparam JointCollection Collection of Joint types. 21  /// \tparam ConfigVectorType Type of the joint configuration vector. 22  /// \tparam TangentVectorType1 Type of the joint velocity vector. 23  /// \tparam TangentVectorType2 Type of the joint acceleration vector. 24  /// 25  /// \param[in] model The model structure of the rigid body system. 26  /// \param[in] data The data structure of the rigid body system. 27  /// \param[in] q The joint configuration (vector dim model.nq). 28  /// \param[in] v The joint velocity (vector dim model.nv). 29  /// 30  /// \remarks This function is similar to do a forwardKinematics(model,data,q,v) followed by a computeJointJacobians(model,data,q). 31  /// In addition, it computes the spatial velocity of the joint expressed in the world frame (see data.ov). 32  /// 33  template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> 34  inline void computeForwardKinematicsDerivatives(const ModelTpl & model, 35  DataTpl & data, 36  const Eigen::MatrixBase & q, 37  const Eigen::MatrixBase & v, 38  const Eigen::MatrixBase & a); 39 40  /// 41  /// \brief Computes the partial derivaties of the spatial velocity of a given with respect to 42  /// the joint configuration and velocity. 43  /// You must first call computForwardKinematicsDerivatives before calling this function. 44  /// 45  /// \tparam JointCollection Collection of Joint types. 46  /// \tparam Matrix6xOut1 Matrix6x containing the partial derivatives with respect to the joint configuration vector. 47  /// \tparam Matrix6xOut2 Matrix6x containing the partial derivatives with respect to the joint velocity vector. 48  /// 49  /// \param[in] model The model structure of the rigid body system. 50  /// \param[in] data The data structure of the rigid body system. 51  /// \param[in] rf Reference frame in which the Jacobian is expressed. 52  /// \param[out] v_partial_dq Partial derivative of the joint velociy w.r.t. \f$q \f$. 53  /// \param[out] v_partial_dv Partial derivative of the joint velociy w.r.t. \f$\dot{q} \f$. 54  /// 55  template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2> 56  inline void getJointVelocityDerivatives(const ModelTpl & model, 57  const DataTpl & data, 58  const Model::JointIndex jointId, 59  const ReferenceFrame rf, 60  const Eigen::MatrixBase & v_partial_dq, 61  const Eigen::MatrixBase & v_partial_dv); 62 63  /// 64  /// \brief Computes the partial derivaties of the spatial acceleration of a given with respect to 65  /// the joint configuration, velocity and acceleration. 66  /// You must first call computForwardKinematicsDerivatives before calling this function. 67  /// 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. 68  /// 69  /// \tparam JointCollection Collection of Joint types. 70  /// \tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the spatial velocity with respect to the joint configuration vector. 71  /// \tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the spatial acceleration with respect to the joint configuration vector. 72  /// \tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the spatial acceleration with respect to the joint velocity vector. 73  /// \tparam Matrix6xOut4 Matrix6x containing the partial derivatives of the spatial acceleration with respect to the joint acceleration vector. 74  /// 75  /// \param[in] model The model structure of the rigid body system. 76  /// \param[in] data The data structure of the rigid body system. 77  /// \param[in] jointId Index of the joint in model. 78  /// \param[in] rf Reference frame in which the Jacobian is expressed. 79  /// \param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$q \f$. 80  /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$q \f$. 81  /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$\dot{q} \f$. 82  /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$\ddot{q} \f$. 83  /// 84  template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4> 85  inline void getJointAccelerationDerivatives(const ModelTpl & model, 86  const DataTpl & data, 87  const Model::JointIndex jointId, 88  const ReferenceFrame rf, 89  const Eigen::MatrixBase & v_partial_dq, 90  const Eigen::MatrixBase & a_partial_dq, 91  const Eigen::MatrixBase & a_partial_dv, 92  const Eigen::MatrixBase & a_partial_da); 93 94  /// 95  /// \brief Computes the partial derivaties of the spatial acceleration of a given with respect to 96  /// the joint configuration, velocity and acceleration. 97  /// You must first call computForwardKinematicsDerivatives before calling this function. 98  /// 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. 99  /// 100  /// \tparam JointCollection Collection of Joint types. 101  /// \tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the spatial velocity with respect to the joint configuration vector. 102  /// \tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the spatial velocity with respect to the joint velocity vector. 103  /// \tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the spatial acceleration with respect to the joint configuration vector. 104  /// \tparam Matrix6xOut4 Matrix6x containing the partial derivatives of the spatial acceleration with respect to the joint velocity vector. 105  /// \tparam Matrix6xOut5 Matrix6x containing the partial derivatives of the spatial acceleration with respect to the joint acceleration vector. 106  /// 107  /// \param[in] model The model structure of the rigid body system. 108  /// \param[in] data The data structure of the rigid body system. 109  /// \param[in] jointId Index of the joint in model. 110  /// \param[in] rf Reference frame in which the Jacobian is expressed. 111  /// \param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$q \f$. 112  /// \param[out] v_partial_dv Partial derivative of the joint spatial velociy w.r.t. \f$\dot{q} \f$. 113  /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$q \f$. 114  /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$\dot{q} \f$. 115  /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$\ddot{q} \f$. 116  /// 117  template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5> 118  inline void getJointAccelerationDerivatives(const ModelTpl & model, 119  const DataTpl & data, 120  const Model::JointIndex jointId, 121  const ReferenceFrame rf, 122  const Eigen::MatrixBase & v_partial_dq, 123  const Eigen::MatrixBase & v_partial_dv, 124  const Eigen::MatrixBase & a_partial_dq, 125  const Eigen::MatrixBase & a_partial_dv, 126  const Eigen::MatrixBase & a_partial_da); 127 128  /// 129  /// \brief Computes all the terms required to compute the second order derivatives of the placement information, also know as the 130  /// kinematic Hessian. This function assumes that the joint Jacobians (a.k.a data.J) has been computed first. See \ref computeJointJacobians 131  /// for such a function. 132  /// 133  /// \tparam Scalar Scalar type of the kinematic model. 134  /// \tparam Options Alignement options of the kinematic model. 135  /// \tparam JointCollection Collection of Joint types. 136  /// 137  /// \param[in] model The model structure of the rigid body system. 138  /// \param[in] data The data structure of the rigid body system. 139  /// 140  /// \remarks This function is also related to \see getJointKinematicHessian. 141  /// 142  template class JointCollectionTpl> 143  inline void 144  computeJointKinematicHessians(const ModelTpl & model, 145  DataTpl & data); 146 147  /// 148  /// \brief Computes all the terms required to compute the second order derivatives of the placement information, also know as the 149  /// kinematic Hessian. 150  /// 151  /// \tparam Scalar Scalar type of the kinematic model. 152  /// \tparam Options Alignement options of the kinematic model. 153  /// \tparam JointCollection Collection of Joint types. 154  /// \tparam ConfigVectorType Type of the joint configuration vector. 155  /// 156  /// \param[in] model The model structure of the rigid body system. 157  /// \param[in] data The data structure of the rigid body system. 158  /// \param[in] q The joint configuration (vector dim model.nq). 159  /// 160  /// \remarks This function is also related to \see getJointKinematicHessian. 161  /// 162  template class JointCollectionTpl, typename ConfigVectorType> 163  inline void 164 1  computeJointKinematicHessians(const ModelTpl & model, 165  DataTpl & data, 166  const Eigen::MatrixBase & q) 167  { 168 1  computeJointJacobians(model,data,q); 169 1  computeJointKinematicHessians(model,data); 170 1  } 171 172  /// 173  /// \brief Retrieves the kinematic Hessian of a given joint according to the values aleardy computed by computeJointKinematicHessians 174  /// and stored in data. While the kinematic Jacobian of a given joint frame corresponds to the first order derivative of the placement variation with 175  /// respect to \f$q \f$, the kinematic Hessian corresponds to the second order derivation of placement variation, which in turns also corresponds 176  /// to the first order derivative of the kinematic Jacobian. The frame in which the kinematic Hessian is precised by the input argument rf. 177  /// 178  /// \tparam Scalar Scalar type of the kinematic model. 179  /// \tparam Options Alignement options of the kinematic model. 180  /// \tparam JointCollection Collection of Joint types. 181  /// \tparam ConfigVectorType Type of the joint configuration vector. 182  /// 183  /// \param[in] model The model structure of the rigid body system. 184  /// \param[in] data The data structure of the rigid body system. 185  /// \param[in] joint_id Index of the joint in model. 186  /// \param[in] rf Reference frame with respect to which the derivative of the Jacobian is expressed 187  /// \param[out] kinematic_hessian Second order derivative of the joint placement w.r.t. \f$q \f$ expressed in the frame given by rf. 188  /// 189  /// \remarks This function is also related to \see computeJointKinematicHessians. kinematic_hessian has to be initialized with zero when calling this function 190  /// for the first time and there is no dynamic memory allocation. 191  /// 192  template class JointCollectionTpl> 193  inline void 194  getJointKinematicHessian(const ModelTpl & model, 195  const DataTpl & data, 196  const Model::JointIndex joint_id, 197  const ReferenceFrame rf, 198  Tensor & kinematic_hessian); 199 200  /// 201  /// \brief Retrieves the kinematic Hessian of a given joint according to the values aleardy computed by computeJointKinematicHessians 202  /// and stored in data. While the kinematic Jacobian of a given joint frame corresponds to the first order derivative of the placement variation with 203  /// respect to \f$q \f$, the kinematic Hessian corresponds to the second order derivation of placement variation, which in turns also corresponds 204  /// to the first order derivative of the kinematic Jacobian. 205  /// 206  /// \tparam Scalar Scalar type of the kinematic model. 207  /// \tparam Options Alignement options of the kinematic model. 208  /// \tparam JointCollection Collection of Joint types. 209  /// \tparam ConfigVectorType Type of the joint configuration vector. 210  /// 211  /// \param[in] model The model structure of the rigid body system. 212  /// \param[in] data The data structure of the rigid body system. 213  /// \param[in] joint_id Index of the joint in model. 214  /// \param[in] rf Reference frame with respect to which the derivative of the Jacobian is expressed. 215  /// 216  /// \returns The kinematic Hessian of the joint provided by its joint_id and expressed in the frame precised by the variable rf. 217  /// 218  /// \remarks This function is also related to \see computeJointKinematicHessians. This function will proceed to some dynamic memory allocation for the return type. 219  /// Please refer to getJointKinematicHessian for a version without dynamic memory allocation. 220  /// 221  template class JointCollectionTpl> 222  inline Tensor 223 3  getJointKinematicHessian(const ModelTpl & model, 224  const DataTpl & data, 225  const Model::JointIndex joint_id, 226  const ReferenceFrame rf) 227  { 228  typedef Tensor ReturnType; 229 ✓✗ 3  ReturnType res(6,model.nv,model.nv); res.setZero(); 230 ✓✗ 3  getJointKinematicHessian(model,data,joint_id,rf,res); 231 3  return res; 232  } 233 234 } // namespace pinocchio 235 236 #include "pinocchio/algorithm/kinematics-derivatives.hxx" 237 238 #endif // ifndef __pinocchio_algorithm_kinematics_derivatives_hpp__

 Generated by: GCOVR (Version 4.2)