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