| Directory: | ./ |
|---|---|
| File: | include/pinocchio/algorithm/kinematics-derivatives.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 9 | 9 | 100.0% |
| Branches: | 2 | 4 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2017-2020 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 | ||
| 18 | /// velocity and acceleration | ||
| 19 | /// for any joint of the model. | ||
| 20 | /// | ||
| 21 | /// \tparam JointCollection Collection of Joint types. | ||
| 22 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
| 23 | /// \tparam TangentVectorType1 Type of the joint velocity vector. | ||
| 24 | /// \tparam TangentVectorType2 Type of the joint acceleration 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 | /// \param[in] v The joint velocity (vector dim model.nv). | ||
| 30 | /// | ||
| 31 | /// \remarks This function is similar to do a forwardKinematics(model,data,q,v) followed by a | ||
| 32 | /// computeJointJacobians(model,data,q). | ||
| 33 | /// In addition, it computes the spatial velocity of the joint expressed in the world | ||
| 34 | /// frame (see data.ov). | ||
| 35 | /// | ||
| 36 | template< | ||
| 37 | typename Scalar, | ||
| 38 | int Options, | ||
| 39 | template<typename, int> class JointCollectionTpl, | ||
| 40 | typename ConfigVectorType, | ||
| 41 | typename TangentVectorType1, | ||
| 42 | typename TangentVectorType2> | ||
| 43 | void computeForwardKinematicsDerivatives( | ||
| 44 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 45 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 46 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 47 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
| 48 | const Eigen::MatrixBase<TangentVectorType2> & a); | ||
| 49 | |||
| 50 | /// | ||
| 51 | /// \brief Computes the partial derivaties of the spatial velocity of a given with respect to | ||
| 52 | /// the joint configuration and velocity. | ||
| 53 | /// You must first call computForwardKinematicsDerivatives before calling this function. | ||
| 54 | /// | ||
| 55 | /// \tparam JointCollection Collection of Joint types. | ||
| 56 | /// \tparam Matrix6xOut1 Matrix6x containing the partial derivatives with respect to the joint | ||
| 57 | /// configuration vector. \tparam Matrix6xOut2 Matrix6x containing the partial derivatives with | ||
| 58 | /// respect to the joint velocity vector. | ||
| 59 | /// | ||
| 60 | /// \param[in] model The model structure of the rigid body system. | ||
| 61 | /// \param[in] data The data structure of the rigid body system. | ||
| 62 | /// \param[in] rf Reference frame in which the Jacobian is expressed. | ||
| 63 | /// \param[out] v_partial_dq Partial derivative of the joint velociy w.r.t. \f$ q \f$. | ||
| 64 | /// \param[out] v_partial_dv Partial derivative of the joint velociy w.r.t. \f$ v \f$. | ||
| 65 | /// | ||
| 66 | template< | ||
| 67 | typename Scalar, | ||
| 68 | int Options, | ||
| 69 | template<typename, int> class JointCollectionTpl, | ||
| 70 | typename Matrix6xOut1, | ||
| 71 | typename Matrix6xOut2> | ||
| 72 | void getJointVelocityDerivatives( | ||
| 73 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 74 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 75 | const Model::JointIndex jointId, | ||
| 76 | const ReferenceFrame rf, | ||
| 77 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
| 78 | const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv); | ||
| 79 | |||
| 80 | /// | ||
| 81 | /// \brief Computes the partial derivaties of the spatial acceleration of a given with respect to | ||
| 82 | /// the joint configuration, velocity and acceleration. | ||
| 83 | /// You must first call computForwardKinematicsDerivatives before calling this function. | ||
| 84 | /// It is important to notice that a direct outcome (for free) of this algo is v_partial_dq | ||
| 85 | /// and v_partial_dv which is equal to a_partial_da. | ||
| 86 | /// | ||
| 87 | /// \tparam JointCollection Collection of Joint types. | ||
| 88 | /// \tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the spatial velocity with | ||
| 89 | /// respect to the joint configuration vector. \tparam Matrix6xOut2 Matrix6x containing the | ||
| 90 | /// partial derivatives of the spatial acceleration with respect to the joint configuration | ||
| 91 | /// vector. \tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the spatial | ||
| 92 | /// acceleration with respect to the joint velocity vector. \tparam Matrix6xOut4 Matrix6x | ||
| 93 | /// containing the partial derivatives of the spatial acceleration with respect to the joint | ||
| 94 | /// acceleration vector. | ||
| 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] jointId Index of the joint in model. | ||
| 99 | /// \param[in] rf Reference frame in which the Jacobian is expressed. | ||
| 100 | /// \param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$. | ||
| 101 | /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ q | ||
| 102 | /// \f$. \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ | ||
| 103 | /// v \f$. \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. | ||
| 104 | /// \f$ \dot{v} \f$. | ||
| 105 | /// | ||
| 106 | template< | ||
| 107 | typename Scalar, | ||
| 108 | int Options, | ||
| 109 | template<typename, int> class JointCollectionTpl, | ||
| 110 | typename Matrix6xOut1, | ||
| 111 | typename Matrix6xOut2, | ||
| 112 | typename Matrix6xOut3, | ||
| 113 | typename Matrix6xOut4> | ||
| 114 | void getJointAccelerationDerivatives( | ||
| 115 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 116 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 117 | const Model::JointIndex jointId, | ||
| 118 | const ReferenceFrame rf, | ||
| 119 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
| 120 | const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq, | ||
| 121 | const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv, | ||
| 122 | const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da); | ||
| 123 | |||
| 124 | /// | ||
| 125 | /// \brief Computes the partial derivaties of the spatial acceleration of a given with respect to | ||
| 126 | /// the joint configuration, velocity and acceleration. | ||
| 127 | /// You must first call computForwardKinematicsDerivatives before calling this function. | ||
| 128 | /// It is important to notice that a direct outcome (for free) of this algo is v_partial_dq | ||
| 129 | /// and v_partial_dv which is equal to a_partial_da. | ||
| 130 | /// | ||
| 131 | /// \tparam JointCollection Collection of Joint types. | ||
| 132 | /// \tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the spatial velocity with | ||
| 133 | /// respect to the joint configuration vector. \tparam Matrix6xOut2 Matrix6x containing the | ||
| 134 | /// partial derivatives of the spatial velocity with respect to the joint velocity vector. \tparam | ||
| 135 | /// Matrix6xOut3 Matrix6x containing the partial derivatives of the spatial acceleration with | ||
| 136 | /// respect to the joint configuration vector. \tparam Matrix6xOut4 Matrix6x containing the | ||
| 137 | /// partial derivatives of the spatial acceleration with respect to the joint velocity vector. | ||
| 138 | /// \tparam Matrix6xOut5 Matrix6x containing the partial derivatives of the spatial acceleration | ||
| 139 | /// with respect to the joint acceleration vector. | ||
| 140 | /// | ||
| 141 | /// \param[in] model The model structure of the rigid body system. | ||
| 142 | /// \param[in] data The data structure of the rigid body system. | ||
| 143 | /// \param[in] jointId Index of the joint in model. | ||
| 144 | /// \param[in] rf Reference frame in which the Jacobian is expressed. | ||
| 145 | /// \param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$. | ||
| 146 | /// \param[out] v_partial_dv Partial derivative of the joint spatial velociy w.r.t. \f$ v \f$. | ||
| 147 | /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ q | ||
| 148 | /// \f$. \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ | ||
| 149 | /// v \f$. \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. | ||
| 150 | /// \f$ \dot{v} \f$. | ||
| 151 | /// | ||
| 152 | template< | ||
| 153 | typename Scalar, | ||
| 154 | int Options, | ||
| 155 | template<typename, int> class JointCollectionTpl, | ||
| 156 | typename Matrix6xOut1, | ||
| 157 | typename Matrix6xOut2, | ||
| 158 | typename Matrix6xOut3, | ||
| 159 | typename Matrix6xOut4, | ||
| 160 | typename Matrix6xOut5> | ||
| 161 | void getJointAccelerationDerivatives( | ||
| 162 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 163 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 164 | const Model::JointIndex jointId, | ||
| 165 | const ReferenceFrame rf, | ||
| 166 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
| 167 | const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv, | ||
| 168 | const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq, | ||
| 169 | const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv, | ||
| 170 | const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da); | ||
| 171 | |||
| 172 | /// | ||
| 173 | /// \brief Computes the partial derivatives of the velocity of a point given by its placement | ||
| 174 | /// information w.r.t. the joint frame. | ||
| 175 | /// You must first call computForwardKinematicsDerivatives before calling this function. | ||
| 176 | /// | ||
| 177 | /// \tparam JointCollection Collection of Joint types. | ||
| 178 | /// \tparam Matrix3xOut1 Matrix3x containing the partial derivatives of the spatial velocity with | ||
| 179 | /// respect to the joint configuration vector. \tparam Matrix3xOut2 Matrix3x containing the | ||
| 180 | /// partial derivatives of the spatial velocity with respect to the joint velocity vector. | ||
| 181 | /// | ||
| 182 | /// \param[in] model The model structure of the rigid body system. | ||
| 183 | /// \param[in] data The data structure of the rigid body system. | ||
| 184 | /// \param[in] joint_id Index of the joint in model. | ||
| 185 | /// \param[in] placement Relative placement of the point w.r.t. the joint frame. | ||
| 186 | /// \param[in] rf Reference frame in which the Jacobian is expressed (either LOCAL or | ||
| 187 | /// LOCAL_WORLD_ALIGNED). \param[out] v_point_partial_dq Partial derivative of the point velocity | ||
| 188 | /// w.r.t. \f$ q \f$. \param[out] v_point_partial_dv Partial derivative of the point velociy | ||
| 189 | /// w.r.t. \f$ v \f$. | ||
| 190 | /// | ||
| 191 | template< | ||
| 192 | typename Scalar, | ||
| 193 | int Options, | ||
| 194 | template<typename, int> class JointCollectionTpl, | ||
| 195 | typename Matrix3xOut1, | ||
| 196 | typename Matrix3xOut2> | ||
| 197 | void getPointVelocityDerivatives( | ||
| 198 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 199 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 200 | const Model::JointIndex joint_id, | ||
| 201 | const SE3Tpl<Scalar, Options> & placement, | ||
| 202 | const ReferenceFrame rf, | ||
| 203 | const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq, | ||
| 204 | const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv); | ||
| 205 | |||
| 206 | /// | ||
| 207 | /// \brief Computes the partial derivatives of the classic acceleration of a point given by its | ||
| 208 | /// placement information w.r.t. the joint frame. | ||
| 209 | /// You must first call computForwardKinematicsDerivatives before calling this function. | ||
| 210 | /// It is important to notice that a direct outcome (for free) of this algo is | ||
| 211 | /// v_point_partial_dq. v_point_partial_dv is not computed it is equal to | ||
| 212 | /// a_point_partial_da. | ||
| 213 | /// | ||
| 214 | /// \tparam JointCollection Collection of Joint types. | ||
| 215 | /// \tparam Matrix3xOut1 Matrix3x containing the partial derivatives of the spatial velocity with | ||
| 216 | /// respect to the joint configuration vector. \tparam Matrix3xOut2 Matrix3x containing the | ||
| 217 | /// partial derivatives of the spatial acceleration with respect to the joint configuration | ||
| 218 | /// vector. \tparam Matrix3xOut3 Matrix3x containing the partial derivatives of the spatial | ||
| 219 | /// acceleration with respect to the joint velocity vector. \tparam Matrix3xOut4 Matrix3x | ||
| 220 | /// containing the partial derivatives of the spatial acceleration with respect to the joint | ||
| 221 | /// acceleration vector. | ||
| 222 | /// | ||
| 223 | /// \param[in] model The model structure of the rigid body system. | ||
| 224 | /// \param[in] data The data structure of the rigid body system. | ||
| 225 | /// \param[in] joint_id Index of the joint in model. | ||
| 226 | /// \param[in] placement Relative placement of the point w.r.t. the joint frame. | ||
| 227 | /// \param[in] rf Reference frame in which the Jacobian is expressed (either LOCAL or | ||
| 228 | /// LOCAL_WORLD_ALIGNED). \param[out] v_point_partial_dq Partial derivative of the point velocity | ||
| 229 | /// w.r.t. \f$ q \f$. \param[out] a_point_partial_dq Partial derivative of the point classic | ||
| 230 | /// acceleration w.r.t. \f$ q \f$. \param[out] a_point_partial_dv Partial derivative of the point | ||
| 231 | /// classic acceleration w.r.t. \f$ v \f$. \param[out] a_point_partial_da Partial derivative of | ||
| 232 | /// the point classic acceleration w.r.t. \f$ \dot{v} \f$. | ||
| 233 | /// | ||
| 234 | template< | ||
| 235 | typename Scalar, | ||
| 236 | int Options, | ||
| 237 | template<typename, int> class JointCollectionTpl, | ||
| 238 | typename Matrix3xOut1, | ||
| 239 | typename Matrix3xOut2, | ||
| 240 | typename Matrix3xOut3, | ||
| 241 | typename Matrix3xOut4> | ||
| 242 | void getPointClassicAccelerationDerivatives( | ||
| 243 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 244 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 245 | const Model::JointIndex joint_id, | ||
| 246 | const SE3Tpl<Scalar, Options> & placement, | ||
| 247 | const ReferenceFrame rf, | ||
| 248 | const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq, | ||
| 249 | const Eigen::MatrixBase<Matrix3xOut2> & a_point_partial_dq, | ||
| 250 | const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dv, | ||
| 251 | const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_da); | ||
| 252 | |||
| 253 | /// | ||
| 254 | /// \brief Computes the partial derivaties of the classic acceleration of a point given by its | ||
| 255 | /// placement information w.r.t. to the joint frame. | ||
| 256 | /// You must first call computForwardKinematicsDerivatives before calling this function. | ||
| 257 | /// It is important to notice that a direct outcome (for free) of this algo is | ||
| 258 | /// v_point_partial_dq and v_point_partial_dv.. | ||
| 259 | /// | ||
| 260 | /// \tparam JointCollection Collection of Joint types. | ||
| 261 | /// \tparam Matrix3xOut1 Matrix3x containing the partial derivatives of the spatial velocity with | ||
| 262 | /// respect to the joint configuration vector. \tparam Matrix3xOut2 Matrix3x containing the | ||
| 263 | /// partial derivatives of the spatial velocity with respect to the joint velocity vector. \tparam | ||
| 264 | /// Matrix3xOut3 Matrix3x containing the partial derivatives of the spatial acceleration with | ||
| 265 | /// respect to the joint configuration vector. \tparam Matrix3xOut4 Matrix3x containing the | ||
| 266 | /// partial derivatives of the spatial acceleration with respect to the joint velocity vector. | ||
| 267 | /// \tparam Matrix3xOut5 Matrix3x containing the partial derivatives of the spatial acceleration | ||
| 268 | /// with respect to the joint acceleration vector. | ||
| 269 | /// | ||
| 270 | /// \param[in] model The model structure of the rigid body system. | ||
| 271 | /// \param[in] data The data structure of the rigid body system. | ||
| 272 | /// \param[in] joint_id Index of the joint in model. | ||
| 273 | /// \param[in] placement Relative placement of the point w.r.t. the joint frame. | ||
| 274 | /// \param[in] rf Reference frame in which the Jacobian is expressed (either LOCAL or | ||
| 275 | /// LOCAL_WORLD_ALIGNED). \param[out] v_point_partial_dq Partial derivative of the point velocity | ||
| 276 | /// w.r.t. \f$ q \f$. \param[out] v_point_partial_dv Partial derivative of the point velociy | ||
| 277 | /// w.r.t. \f$ v \f$. \param[out] a_point_partial_dq Partial derivative of the point classic | ||
| 278 | /// acceleration w.r.t. \f$ q \f$. \param[out] a_point_partial_dv Partial derivative of the point | ||
| 279 | /// classic acceleration w.r.t. \f$ v \f$. \param[out] a_point_partial_da Partial derivative of | ||
| 280 | /// the point classic acceleration w.r.t. \f$ \dot{v} \f$. | ||
| 281 | /// | ||
| 282 | template< | ||
| 283 | typename Scalar, | ||
| 284 | int Options, | ||
| 285 | template<typename, int> class JointCollectionTpl, | ||
| 286 | typename Matrix3xOut1, | ||
| 287 | typename Matrix3xOut2, | ||
| 288 | typename Matrix3xOut3, | ||
| 289 | typename Matrix3xOut4, | ||
| 290 | typename Matrix3xOut5> | ||
| 291 | void getPointClassicAccelerationDerivatives( | ||
| 292 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 293 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 294 | const Model::JointIndex joint_id, | ||
| 295 | const SE3Tpl<Scalar, Options> & placement, | ||
| 296 | const ReferenceFrame rf, | ||
| 297 | const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq, | ||
| 298 | const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv, | ||
| 299 | const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dq, | ||
| 300 | const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_dv, | ||
| 301 | const Eigen::MatrixBase<Matrix3xOut5> & a_point_partial_da); | ||
| 302 | |||
| 303 | /// | ||
| 304 | /// \brief Computes all the terms required to compute the second order derivatives of the | ||
| 305 | /// placement information, also know as the | ||
| 306 | /// kinematic Hessian. This function assumes that the joint Jacobians (a.k.a data.J) has | ||
| 307 | /// been computed first. See \ref computeJointJacobians for such a function. | ||
| 308 | /// | ||
| 309 | /// \tparam Scalar Scalar type of the kinematic model. | ||
| 310 | /// \tparam Options Alignement options of the kinematic model. | ||
| 311 | /// \tparam JointCollection Collection of Joint types. | ||
| 312 | /// | ||
| 313 | /// \param[in] model The model structure of the rigid body system. | ||
| 314 | /// \param[in] data The data structure of the rigid body system. | ||
| 315 | /// | ||
| 316 | /// \remarks This function is also related to \see getJointKinematicHessian. | ||
| 317 | /// | ||
| 318 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 319 | void computeJointKinematicHessians( | ||
| 320 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 321 | DataTpl<Scalar, Options, JointCollectionTpl> & data); | ||
| 322 | |||
| 323 | /// | ||
| 324 | /// \brief Computes all the terms required to compute the second order derivatives of the | ||
| 325 | /// placement information, also know as the | ||
| 326 | /// kinematic Hessian. | ||
| 327 | /// | ||
| 328 | /// \tparam Scalar Scalar type of the kinematic model. | ||
| 329 | /// \tparam Options Alignement options of the kinematic model. | ||
| 330 | /// \tparam JointCollection Collection of Joint types. | ||
| 331 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
| 332 | /// | ||
| 333 | /// \param[in] model The model structure of the rigid body system. | ||
| 334 | /// \param[in] data The data structure of the rigid body system. | ||
| 335 | /// \param[in] q The joint configuration (vector dim model.nq). | ||
| 336 | /// | ||
| 337 | /// \remarks This function is also related to \see getJointKinematicHessian. | ||
| 338 | /// | ||
| 339 | template< | ||
| 340 | typename Scalar, | ||
| 341 | int Options, | ||
| 342 | template<typename, int> class JointCollectionTpl, | ||
| 343 | typename ConfigVectorType> | ||
| 344 | 1 | void computeJointKinematicHessians( | |
| 345 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 346 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 347 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
| 348 | { | ||
| 349 | 1 | computeJointJacobians(model, data, q); | |
| 350 | 1 | computeJointKinematicHessians(model, data); | |
| 351 | 1 | } | |
| 352 | |||
| 353 | /// | ||
| 354 | /// \brief Retrieves the kinematic Hessian of a given joint according to the values aleardy | ||
| 355 | /// computed by computeJointKinematicHessians | ||
| 356 | /// and stored in data. While the kinematic Jacobian of a given joint frame corresponds to | ||
| 357 | /// the first order derivative of the placement variation with respect to \f$ q \f$, the | ||
| 358 | /// kinematic Hessian corresponds to the second order derivation of placement variation, | ||
| 359 | /// which in turns also corresponds to the first order derivative of the kinematic | ||
| 360 | /// Jacobian. The frame in which the kinematic Hessian is precised by the input argument | ||
| 361 | /// rf. | ||
| 362 | /// | ||
| 363 | /// \tparam Scalar Scalar type of the kinematic model. | ||
| 364 | /// \tparam Options Alignement options of the kinematic model. | ||
| 365 | /// \tparam JointCollection Collection of Joint types. | ||
| 366 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
| 367 | /// | ||
| 368 | /// \param[in] model The model structure of the rigid body system. | ||
| 369 | /// \param[in] data The data structure of the rigid body system. | ||
| 370 | /// \param[in] joint_id Index of the joint in model. | ||
| 371 | /// \param[in] rf Reference frame with respect to which the derivative of the Jacobian is | ||
| 372 | /// expressed \param[out] kinematic_hessian Second order derivative of the joint placement w.r.t. | ||
| 373 | /// \f$ q \f$ expressed in the frame given by rf. | ||
| 374 | /// | ||
| 375 | /// \remarks This function is also related to \see computeJointKinematicHessians. | ||
| 376 | /// kinematic_hessian has to be initialized with zero when calling this function | ||
| 377 | /// for the first time and there is no dynamic memory allocation. | ||
| 378 | /// | ||
| 379 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 380 | void getJointKinematicHessian( | ||
| 381 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 382 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 383 | const Model::JointIndex joint_id, | ||
| 384 | const ReferenceFrame rf, | ||
| 385 | Tensor<Scalar, 3, Options> & kinematic_hessian); | ||
| 386 | |||
| 387 | /// | ||
| 388 | /// \brief Retrieves the kinematic Hessian of a given joint according to the values aleardy | ||
| 389 | /// computed by computeJointKinematicHessians | ||
| 390 | /// and stored in data. While the kinematic Jacobian of a given joint frame corresponds to | ||
| 391 | /// the first order derivative of the placement variation with respect to \f$ q \f$, the | ||
| 392 | /// kinematic Hessian corresponds to the second order derivation of placement variation, | ||
| 393 | /// which in turns also corresponds to the first order derivative of the kinematic | ||
| 394 | /// Jacobian. | ||
| 395 | /// | ||
| 396 | /// \tparam Scalar Scalar type of the kinematic model. | ||
| 397 | /// \tparam Options Alignement options of the kinematic model. | ||
| 398 | /// \tparam JointCollection Collection of Joint types. | ||
| 399 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
| 400 | /// | ||
| 401 | /// \param[in] model The model structure of the rigid body system. | ||
| 402 | /// \param[in] data The data structure of the rigid body system. | ||
| 403 | /// \param[in] joint_id Index of the joint in model. | ||
| 404 | /// \param[in] rf Reference frame with respect to which the derivative of the Jacobian is | ||
| 405 | /// expressed. | ||
| 406 | /// | ||
| 407 | /// \returns The kinematic Hessian of the joint provided by its joint_id and expressed in the | ||
| 408 | /// frame precised by the variable rf. | ||
| 409 | /// | ||
| 410 | /// \remarks This function is also related to \see computeJointKinematicHessians. This function | ||
| 411 | /// will proceed to some dynamic memory allocation for the return type. | ||
| 412 | /// Please refer to getJointKinematicHessian for a version without dynamic memory | ||
| 413 | /// allocation. | ||
| 414 | /// | ||
| 415 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 416 | 3 | Tensor<Scalar, 3, Options> getJointKinematicHessian( | |
| 417 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 418 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 419 | const Model::JointIndex joint_id, | ||
| 420 | const ReferenceFrame rf) | ||
| 421 | { | ||
| 422 | typedef Tensor<Scalar, 3, Options> ReturnType; | ||
| 423 | 3 | ReturnType res(6, model.nv, model.nv); | |
| 424 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | res.setZero(); |
| 425 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | getJointKinematicHessian(model, data, joint_id, rf, res); |
| 426 | 3 | return res; | |
| 427 | } | ||
| 428 | |||
| 429 | } // namespace pinocchio | ||
| 430 | |||
| 431 | #include "pinocchio/algorithm/kinematics-derivatives.hxx" | ||
| 432 | |||
| 433 | #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
| 434 | #include "pinocchio/algorithm/kinematics-derivatives.txx" | ||
| 435 | #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
| 436 | |||
| 437 | #endif // ifndef __pinocchio_algorithm_kinematics_derivatives_hpp__ | ||
| 438 |