| Directory: | ./ |
|---|---|
| File: | include/pinocchio/algorithm/jacobian.hpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 4 | 4 | 100.0% |
| Branches: | 2 | 4 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_algorithm_jacobian_hpp__ | ||
| 6 | #define __pinocchio_algorithm_jacobian_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/multibody/model.hpp" | ||
| 9 | #include "pinocchio/multibody/data.hpp" | ||
| 10 | |||
| 11 | namespace pinocchio | ||
| 12 | { | ||
| 13 | /// | ||
| 14 | /// \brief Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in | ||
| 15 | /// the world frame. | ||
| 16 | /// The result is accessible through data.J. This function computes also the | ||
| 17 | /// forwardKinematics of the model. | ||
| 18 | /// | ||
| 19 | /// \note This Jacobian does not correspond to any specific joint frame Jacobian. From this | ||
| 20 | /// Jacobian, it is then possible to easily extract the Jacobian of a specific joint frame. \sa | ||
| 21 | /// pinocchio::getJointJacobian for doing this specific extraction. | ||
| 22 | /// | ||
| 23 | /// \tparam JointCollection Collection of Joint types. | ||
| 24 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
| 25 | /// | ||
| 26 | /// \param[in] model The model structure of the rigid body system. | ||
| 27 | /// \param[in] data The data structure of the rigid body system. | ||
| 28 | /// \param[in] q The joint configuration vector (dim model.nq). | ||
| 29 | /// | ||
| 30 | /// \return The full model Jacobian (matrix 6 x model.nv). | ||
| 31 | /// | ||
| 32 | template< | ||
| 33 | typename Scalar, | ||
| 34 | int Options, | ||
| 35 | template<typename, int> | ||
| 36 | class JointCollectionTpl, | ||
| 37 | typename ConfigVectorType> | ||
| 38 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeJointJacobians( | ||
| 39 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 40 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 41 | const Eigen::MatrixBase<ConfigVectorType> & q); | ||
| 42 | |||
| 43 | /// | ||
| 44 | /// \brief Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in | ||
| 45 | /// the world frame. | ||
| 46 | /// The result is accessible through data.J. This function assumes that | ||
| 47 | /// pinocchio::forwardKinematics has been called before. | ||
| 48 | /// | ||
| 49 | /// \note This Jacobian does not correspond to any specific joint frame Jacobian. From this | ||
| 50 | /// Jacobian, it is then possible to easily extract the Jacobian of a specific joint frame. \sa | ||
| 51 | /// pinocchio::getJointJacobian for doing this specific extraction. | ||
| 52 | /// | ||
| 53 | /// \tparam JointCollection Collection of Joint types. | ||
| 54 | /// | ||
| 55 | /// \param[in] model The model structure of the rigid body system. | ||
| 56 | /// \param[in] data The data structure of the rigid body system. | ||
| 57 | /// | ||
| 58 | /// \return The full model Jacobian (matrix 6 x model.nv). | ||
| 59 | /// | ||
| 60 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 61 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeJointJacobians( | ||
| 62 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 63 | DataTpl<Scalar, Options, JointCollectionTpl> & data); | ||
| 64 | |||
| 65 | /// \brief Computes the Jacobian of a specific joint frame expressed in one of the | ||
| 66 | /// pinocchio::ReferenceFrame options. | ||
| 67 | /// | ||
| 68 | /// \details For the LOCAL reference frame, the Jacobian \f${}^j J_{0j}\f$ from the joint frame | ||
| 69 | /// \f$j\f$ to the world frame \f$0\f$ is such that \f${}^j v_{0j} = {}^j J_{0j} \dot{q}\f$, where | ||
| 70 | /// \f${}^j v_{0j}\f$ is the velocity of the origin of the moving joint frame relative to the | ||
| 71 | /// fixed world frame, projected into the basis of the joint frame. LOCAL_WORLD_ALIGNED is the | ||
| 72 | /// same velocity but projected into the world frame basis. | ||
| 73 | /// | ||
| 74 | /// For the WORLD reference frame, the Jacobian \f${}^0 J_{0j}\f$ from the joint frame \f$j\f$ to | ||
| 75 | /// the world frame \f$0\f$ is such that \f${}^0 v_{0j} = {}^0 J_{0j} \dot{q}\f$, where \f${}^0 | ||
| 76 | /// v_{0j}\f$ is the spatial velocity of the joint frame. The linear component of this spatial | ||
| 77 | /// velocity is the velocity of a (possibly imaginary) point attached to the moving joint frame j | ||
| 78 | /// which is traveling through the origin of the world frame at that instant. The angular | ||
| 79 | /// component is the instantaneous angular velocity of the joint frame as viewed in the world | ||
| 80 | /// frame. | ||
| 81 | /// | ||
| 82 | /// When serialized to a 6D vector, the order of coordinates is: three linear followed by three | ||
| 83 | /// angular. | ||
| 84 | /// | ||
| 85 | /// For further details regarding the different velocities or the Jacobian see Chapters 2 and 3 | ||
| 86 | /// respectively in [A Mathematical Introduction to Robotic | ||
| 87 | /// Manipulation](https://www.cse.lehigh.edu/~trink/Courses/RoboticsII/reading/murray-li-sastry-94-complete.pdf) | ||
| 88 | /// by Murray, Li and Sastry. | ||
| 89 | /// | ||
| 90 | /// \note This jacobian is extracted from data.J. You have to run pinocchio::computeJointJacobians | ||
| 91 | /// before calling it. | ||
| 92 | /// | ||
| 93 | /// \tparam JointCollection Collection of Joint types. | ||
| 94 | /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
| 95 | /// | ||
| 96 | /// \param[in] model The model structure of the rigid body system. | ||
| 97 | /// \param[in] data The data structure of the rigid body system. | ||
| 98 | /// \param[in] joint_id The id of the joint. | ||
| 99 | /// \param[in] reference_frame Reference frame in which the result is expressed. | ||
| 100 | /// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x | ||
| 101 | /// model.nv). You must fill J with zero elements, e.g. J.fill(0.). | ||
| 102 | /// | ||
| 103 | template< | ||
| 104 | typename Scalar, | ||
| 105 | int Options, | ||
| 106 | template<typename, int> | ||
| 107 | class JointCollectionTpl, | ||
| 108 | typename Matrix6Like> | ||
| 109 | void getJointJacobian( | ||
| 110 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 111 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 112 | const JointIndex joint_id, | ||
| 113 | const ReferenceFrame reference_frame, | ||
| 114 | const Eigen::MatrixBase<Matrix6Like> & J); | ||
| 115 | /// | ||
| 116 | /// \brief Computes the Jacobian of a specific joint frame expressed either in the world (rf = | ||
| 117 | /// WORLD) frame, in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the local | ||
| 118 | /// frame (rf = LOCAL) of the joint. \note This jacobian is extracted from data.J. You have to run | ||
| 119 | /// pinocchio::computeJointJacobians before calling it. | ||
| 120 | /// | ||
| 121 | /// \tparam JointCollection Collection of Joint types. | ||
| 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] joint_id The index of the joint. | ||
| 126 | /// \param[in] reference_frame Reference frame in which the result is expressed. | ||
| 127 | /// | ||
| 128 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 129 | 9 | Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getJointJacobian( | |
| 130 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 131 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 132 | const JointIndex joint_id, | ||
| 133 | const ReferenceFrame reference_frame) | ||
| 134 | { | ||
| 135 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> ReturnType; | ||
| 136 |
1/2✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
|
9 | ReturnType res(ReturnType::Zero(6, model.nv)); |
| 137 | |||
| 138 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | getJointJacobian(model, data, joint_id, reference_frame, res); |
| 139 | 9 | return res; | |
| 140 | } | ||
| 141 | |||
| 142 | /// | ||
| 143 | /// \brief Computes the Jacobian of a specific joint frame expressed in the local frame of the | ||
| 144 | /// joint and store the result in the input argument J. | ||
| 145 | /// | ||
| 146 | /// \tparam JointCollection Collection of Joint types. | ||
| 147 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
| 148 | /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
| 149 | /// | ||
| 150 | /// \param[in] model The model structure of the rigid body system. | ||
| 151 | /// \param[in] data The data structure of the rigid body system. | ||
| 152 | /// \param[in] q The joint configuration vector (dim model.nq). | ||
| 153 | /// \param[in] joint_id The id of the joint refering to model.joints[jointId]. | ||
| 154 | /// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x | ||
| 155 | /// model.nv). You must fill J with zero elements, e.g. J.setZero(). | ||
| 156 | /// | ||
| 157 | /// \return The Jacobian of the specific joint frame expressed in the local frame of the joint | ||
| 158 | /// (matrix 6 x model.nv). | ||
| 159 | /// | ||
| 160 | /// \remarks The result of this function is equivalent to call first | ||
| 161 | /// computeJointJacobians(model,data,q) and then call | ||
| 162 | /// getJointJacobian(model,data,jointId,LOCAL,J), | ||
| 163 | /// but forwardKinematics is not fully computed. | ||
| 164 | /// It is worth to call jacobian if you only need a single Jacobian for a specific joint. | ||
| 165 | /// Otherwise, for several Jacobians, it is better to call | ||
| 166 | /// computeJointJacobians(model,data,q) followed by | ||
| 167 | /// getJointJacobian(model,data,jointId,LOCAL,J) for each Jacobian. | ||
| 168 | /// | ||
| 169 | template< | ||
| 170 | typename Scalar, | ||
| 171 | int Options, | ||
| 172 | template<typename, int> | ||
| 173 | class JointCollectionTpl, | ||
| 174 | typename ConfigVectorType, | ||
| 175 | typename Matrix6Like> | ||
| 176 | void computeJointJacobian( | ||
| 177 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 178 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 179 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 180 | const JointIndex joint_id, | ||
| 181 | const Eigen::MatrixBase<Matrix6Like> & J); | ||
| 182 | |||
| 183 | /// | ||
| 184 | /// \brief Computes the full model Jacobian variations with respect to time. It corresponds to | ||
| 185 | /// dJ/dt which depends both on q and v. | ||
| 186 | /// The result is accessible through data.dJ. | ||
| 187 | /// | ||
| 188 | /// \tparam JointCollection Collection of Joint types. | ||
| 189 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
| 190 | /// \tparam TangentVectorType Type of the joint velocity vector. | ||
| 191 | /// | ||
| 192 | /// \param[in] model The model structure of the rigid body system. | ||
| 193 | /// \param[in] data The data structure of the rigid body system. | ||
| 194 | /// \param[in] q The joint configuration vector (dim model.nq). | ||
| 195 | /// \param[in] v The joint velocity vector (dim model.nv). | ||
| 196 | /// | ||
| 197 | /// \return The full model Jacobian (matrix 6 x model.nv). | ||
| 198 | /// | ||
| 199 | template< | ||
| 200 | typename Scalar, | ||
| 201 | int Options, | ||
| 202 | template<typename, int> | ||
| 203 | class JointCollectionTpl, | ||
| 204 | typename ConfigVectorType, | ||
| 205 | typename TangentVectorType> | ||
| 206 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & | ||
| 207 | computeJointJacobiansTimeVariation( | ||
| 208 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 209 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 210 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 211 | const Eigen::MatrixBase<TangentVectorType> & v); | ||
| 212 | |||
| 213 | /// | ||
| 214 | /// \brief Computes the Jacobian time variation of a specific joint frame expressed either in the | ||
| 215 | /// world frame (rf = WORLD), in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in | ||
| 216 | /// the local frame (rf = LOCAL) of the joint. \note This jacobian is extracted from data.dJ. You | ||
| 217 | /// have to run pinocchio::computeJointJacobiansTimeVariation before calling it. | ||
| 218 | /// | ||
| 219 | /// \tparam JointCollection Collection of Joint types. | ||
| 220 | /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
| 221 | /// | ||
| 222 | /// \param[in] model The model structure of the rigid body system. | ||
| 223 | /// \param[in] data The data structure of the rigid body system. | ||
| 224 | /// \param[in] joint_id The id of the joint. | ||
| 225 | /// \param[in] reference_frame Reference frame in which the result is expressed. | ||
| 226 | /// \param[out] dJ A reference on the Jacobian matrix where the results will be stored in (dim 6 x | ||
| 227 | /// model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.). | ||
| 228 | /// | ||
| 229 | template< | ||
| 230 | typename Scalar, | ||
| 231 | int Options, | ||
| 232 | template<typename, int> | ||
| 233 | class JointCollectionTpl, | ||
| 234 | typename Matrix6Like> | ||
| 235 | void getJointJacobianTimeVariation( | ||
| 236 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 237 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 238 | const JointIndex joint_id, | ||
| 239 | const ReferenceFrame reference_frame, | ||
| 240 | const Eigen::MatrixBase<Matrix6Like> & dJ); | ||
| 241 | |||
| 242 | } // namespace pinocchio | ||
| 243 | |||
| 244 | /* --- Details -------------------------------------------------------------------- */ | ||
| 245 | /* --- Details -------------------------------------------------------------------- */ | ||
| 246 | /* --- Details -------------------------------------------------------------------- */ | ||
| 247 | |||
| 248 | #include "pinocchio/algorithm/jacobian.hxx" | ||
| 249 | |||
| 250 | #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
| 251 | #include "pinocchio/algorithm/jacobian.txx" | ||
| 252 | #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
| 253 | |||
| 254 | #endif // ifndef __pinocchio_algorithm_jacobian_hpp__ | ||
| 255 |